diff --git a/data/quadruped/minitaur.urdf b/data/quadruped/minitaur.urdf index fa4bc2d37..7bbeed76b 100644 --- a/data/quadruped/minitaur.urdf +++ b/data/quadruped/minitaur.urdf @@ -267,6 +267,11 @@ + + + + + @@ -427,8 +432,10 @@ - - + + + + @@ -448,11 +455,12 @@ + - + @@ -495,8 +503,10 @@ - - + + + + @@ -516,11 +526,12 @@ + - + @@ -561,8 +572,10 @@ - - + + + + @@ -582,11 +595,12 @@ + - + @@ -628,8 +642,10 @@ - - + + + + @@ -649,11 +665,12 @@ + - + @@ -692,8 +709,10 @@ - - + + + + @@ -713,11 +732,12 @@ + - + @@ -760,8 +780,10 @@ - - + + + + @@ -781,11 +803,12 @@ + - + @@ -826,8 +849,10 @@ - - + + + + @@ -847,11 +872,12 @@ + - + @@ -891,8 +917,10 @@ - - + + + + @@ -912,11 +940,12 @@ + - + diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 3de9af446..7672e1962 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -221,6 +221,11 @@ void MyKeyboardCallback(int key, int state) gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation; gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0); } + if (key == 'j' && state) + { + gDebugDrawFlags ^= btIDebugDraw::DBG_DrawFrames; + } + if (key == 'k' && state) { gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints; diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 215766e1b..3d5b32cd6 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -140,7 +140,7 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in dof6->setLinearUpperLimit(btVector3(0,0,0)); dof6->setAngularUpperLimit(btVector3(0,0,-1)); - dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularLowerLimit(btVector3(0,0,1)); } }; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 45bd86b93..e23c4ceee 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -396,7 +396,7 @@ template void addJointInfoFromConstraint(int linkIndex, info.m_linkName = 0; info.m_flags = 0; info.m_jointIndex = linkIndex; - info.m_qIndex = linkIndex+6; + info.m_qIndex = linkIndex+7; info.m_uIndex = linkIndex+6; //derive type from limits diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2e944481f..3ed0f5b5a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4601,7 +4601,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Using CONTROL_MODE_TORQUE"); } // mb->clearForcesAndTorques(); - int torqueIndex = 6; + ///see addJointInfoFromConstraint + int velIndex = 6; + int posIndex = 7; //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) { for (int link=0;linkm_rigidBodyJoints.size();link++) @@ -4616,14 +4618,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm //for (int dof=0;dofgetLink(link).m_dofCount;dof++) { - double torque = 0.f; - //if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; - btScalar qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[torqueIndex]; - btScalar qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[torqueIndex]; + + { + + int torqueIndex = velIndex; + double torque = 100; + bool hasDesiredTorque = false; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]; + hasDesiredTorque = true; + } + bool hasDesiredPosOrVel = false; + btScalar qdotTarget = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + } + btScalar qTarget = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + } + con->getLinearLowerLimit(linearLowerLimit); con->getLinearUpperLimit(linearUpperLimit); con->getAngularLowerLimit(angularLowerLimit); @@ -4650,30 +4671,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { case CONTROL_MODE_TORQUE: { - con->getRigidBodyA().applyTorque(torque*axisA); - con->getRigidBodyB().applyTorque(-torque*axisB); + if (hasDesiredTorque) + { + con->getRigidBodyA().applyTorque(torque*axisA); + con->getRigidBodyB().applyTorque(-torque*axisB); + } break; } case CONTROL_MODE_VELOCITY: { - con->enableMotor(3+limitAxis,true); - con->setTargetVelocity(3+limitAxis, qdotTarget); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); + if (hasDesiredPosOrVel) + { + con->enableMotor(3+limitAxis,true); + con->setTargetVelocity(3+limitAxis, qdotTarget); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(3+limitAxis,torqueImpulse); + } break; } case CONTROL_MODE_POSITION_VELOCITY_PD: { - con->setServo(3+limitAxis,true); - con->setServoTarget(3+limitAxis,qTarget); - //next one is the maximum velocity to reach target position. - //the maximum velocity is limited by maxMotorForce - con->setTargetVelocity(3+limitAxis, 100); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); - con->enableMotor(3+limitAxis,true); + if (hasDesiredPosOrVel) + { + con->setServo(3+limitAxis,true); + con->setServoTarget(3+limitAxis,-qTarget); + //next one is the maximum velocity to reach target position. + //the maximum velocity is limited by maxMotorForce + con->setTargetVelocity(3+limitAxis, 100); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(3+limitAxis,torqueImpulse); + con->enableMotor(3+limitAxis,true); + } break; } default: @@ -4733,7 +4763,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } }//fi - torqueIndex++; + ///see addJointInfoFromConstraint + velIndex ++;//info.m_uIndex + posIndex ++;//info.m_qIndex + } } }//fi diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index 9495a7acc..b676dfff3 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -2,34 +2,49 @@ import pybullet as p import time import math -useRealTime = 0 -fixedTimeStep = 0.01 +toeConstraint = True +useMaximalCoordinates = True +useRealTime = 1 + +#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance +fixedTimeStep = 1./100 +numSolverIterations = 50 + +if (useMaximalCoordinates): + fixedTimeStep = 1./500 + numSolverIterations = 200 + speed = 10 amplitude = 0.8 jump_amp = 0.5 maxForce = 3.5 -kneeFrictionForce = 0.00 +kneeFrictionForce = 0 kp = 1 -kd = .1 +kd = .5 +maxKneeForce = 1000 physId = p.connect(p.SHARED_MEMORY) if (physId<0): p.connect(p.GUI) #p.resetSimulation() -p.loadURDF("plane.urdf",0,0,0) -p.setPhysicsEngineParameter(numSolverIterations=50) + +angle = 0 # pick in range 0..0.2 radians +orn = p.getQuaternionFromEuler([0,angle,0]) +p.loadURDF("plane.urdf",[0,0,0],orn) +p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES) -p.setTimeOut(4) +p.setTimeOut(4000000) p.setGravity(0,0,0) p.setTimeStep(fixedTimeStep) orn = p.getQuaternionFromEuler([0,0,0.4]) p.setRealTimeSimulation(0) -quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False) +quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates) nJoints = p.getNumJoints(quadruped) + jointNameToId = {} for i in range(nJoints): jointInfo = p.getJointInfo(quadruped, i) @@ -37,11 +52,11 @@ for i in range(nJoints): motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] +motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] +knee_front_rightL_link = jointNameToId['knee_front_rightL_link'] hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] -motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] motor_front_rightL_link = jointNameToId['motor_front_rightL_link'] -knee_front_rightL_link = jointNameToId['knee_front_rightL_link'] motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint'] hip_front_leftR_link = jointNameToId['hip_front_leftR_link'] knee_front_leftR_link = jointNameToId['knee_front_leftR_link'] @@ -61,51 +76,83 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] +#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0]) + motordir=[-1,-1,-1,-1,1,1,1,1] halfpi = 1.57079632679 +twopi = 4*halfpi kneeangle = -2.1834 -p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi) -p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle) -p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi) -p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle) -cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) -p.changeConstraint(cid,maxForce=10000) -p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) - -p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi) -p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle) -p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi) -p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle) -cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) -p.changeConstraint(cid,maxForce=10000) -p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +if (useMaximalCoordinates): + steps = 400 + for aa in range (steps): + p.setJointMotorControl2(quadruped,motor_front_leftL_joint,p.POSITION_CONTROL,motordir[0]*halfpi*float(aa)/steps) + p.setJointMotorControl2(quadruped,motor_front_leftR_joint,p.POSITION_CONTROL,motordir[1]*halfpi*float(aa)/steps) + p.setJointMotorControl2(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,motordir[2]*halfpi*float(aa)/steps) + p.setJointMotorControl2(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,motordir[3]*halfpi*float(aa)/steps) + p.setJointMotorControl2(quadruped,motor_front_rightL_joint,p.POSITION_CONTROL,motordir[4]*halfpi*float(aa)/steps) + p.setJointMotorControl2(quadruped,motor_front_rightR_joint,p.POSITION_CONTROL,motordir[5]*halfpi*float(aa)/steps) + p.setJointMotorControl2(quadruped,motor_back_rightL_joint,p.POSITION_CONTROL,motordir[6]*halfpi*float(aa)/steps) + p.setJointMotorControl2(quadruped,motor_back_rightR_joint,p.POSITION_CONTROL,motordir[7]*halfpi*float(aa)/steps) + + p.setJointMotorControl2(quadruped,knee_front_leftL_link,p.POSITION_CONTROL,motordir[0]*(kneeangle+twopi)*float(aa)/steps) + p.setJointMotorControl2(quadruped,knee_front_leftR_link,p.POSITION_CONTROL,motordir[1]*kneeangle*float(aa)/steps) + p.setJointMotorControl2(quadruped,knee_back_leftL_link,p.POSITION_CONTROL,motordir[2]*kneeangle*float(aa)/steps) + p.setJointMotorControl2(quadruped,knee_back_leftR_link,p.POSITION_CONTROL,motordir[3]*(kneeangle+twopi)*float(aa)/steps) + p.setJointMotorControl2(quadruped,knee_front_rightL_link,p.POSITION_CONTROL,motordir[4]*(kneeangle)*float(aa)/steps) + p.setJointMotorControl2(quadruped,knee_front_rightR_link,p.POSITION_CONTROL,motordir[5]*(kneeangle+twopi)*float(aa)/steps) + p.setJointMotorControl2(quadruped,knee_back_rightL_link,p.POSITION_CONTROL,motordir[6]*(kneeangle+twopi)*float(aa)/steps) + p.setJointMotorControl2(quadruped,knee_back_rightR_link,p.POSITION_CONTROL,motordir[7]*kneeangle*float(aa)/steps) + + p.stepSimulation() + #time.sleep(fixedTimeStep) +else: + + p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi) + p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle) + p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi) + p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle) + + p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi) + p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle) + p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi) + p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle) + + p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi) + p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle) + p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi) + p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle) + + p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi) + p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle) + p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi) + p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle) #p.getNumJoints(1) -p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi) -p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle) -p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi) -p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle) -cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) -p.changeConstraint(cid,maxForce=10000) - -p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) - -p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi) -p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle) -p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi) -p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle) -cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) -p.changeConstraint(cid,maxForce=10000) - -p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +if (toeConstraint): + cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) + p.changeConstraint(cid,maxForce=maxKneeForce) + cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) + p.changeConstraint(cid,maxForce=maxKneeForce) + cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) + p.changeConstraint(cid,maxForce=maxKneeForce) + cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) + p.changeConstraint(cid,maxForce=maxKneeForce) + +if (1): + p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setGravity(0,0,-10) @@ -135,10 +182,18 @@ p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMo #stand still p.setRealTimeSimulation(useRealTime) -#while (True): -# time.sleep(0.01) -#p.stepSimulation() +t = 0.0 +t_end = t + 15 +ref_time = time.time() +while (t