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/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index ebedaed80..2c8fbea2a 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ 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..4877ff95b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -890,7 +890,7 @@ struct GenericRobotStateLogger : public InternalStateLogger { m_structTypes.append("f"); char jointName[256]; - sprintf(jointName,"v%d",i); + sprintf(jointName,"u%d",i); structNames.push_back(jointName); } @@ -900,7 +900,7 @@ struct GenericRobotStateLogger : public InternalStateLogger { m_structTypes.append("f"); char jointName[256]; - sprintf(jointName,"u%d",i); + sprintf(jointName,"t%d",i); structNames.push_back(jointName); } } @@ -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/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 80968c880..208eeafe4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -25,6 +25,8 @@ bool gEnablePicking=true; bool gEnableTeleporting=true; bool gEnableRendering= true; +bool gActivedVRRealTimeSimulation = false; + bool gEnableSyncPhysicsRendering= true; bool gEnableUpdateDebugDrawLines = true; static int gCamVisualizerWidth = 320; @@ -2627,8 +2629,10 @@ void PhysicsServerExample::renderScene() if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { - if (!m_physicsServer.isRealTimeSimulationEnabled()) + if (!m_physicsServer.isRealTimeSimulationEnabled() && !gActivedVRRealTimeSimulation) { + //only activate real-time simulation once (for backward compatibility) + gActivedVRRealTimeSimulation = true; m_physicsServer.enableRealTimeSimulation(1); } } diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index 9495a7acc..c6d93d05f 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 = False +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=0]) + return btenvs diff --git a/setup.py b/setup.py index c57657c10..afd39b132 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.3.0', + version='1.3.1', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 2940f7550..c893b60d3 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1333,7 +1333,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const // Draw a small simplex at the center of the object if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) { - getDebugDrawer()->drawTransform(worldTransform,1); + getDebugDrawer()->drawTransform(worldTransform,.1); } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index c2c091b48..e2d3a6983 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -729,6 +729,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( if (limot->m_enableMotor && limot->m_servoMotor) { btScalar error = limot->m_currentPosition - limot->m_servoTarget; + btScalar curServoTarget = limot->m_servoTarget; + if (rotational) + { + if (error > SIMD_PI) + { + error -= SIMD_2_PI; + curServoTarget +=SIMD_2_PI; + } + if (error < -SIMD_PI) + { + error += SIMD_2_PI; + curServoTarget -=SIMD_2_PI; + } + } + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; btScalar tag_vel = -targetvelocity; @@ -739,13 +754,13 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( btScalar hiLimit; if(limot->m_loLimit > limot->m_hiLimit) { - lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY; - hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY; + lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY; + hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY; } else { - lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit; - hiLimit = error < 0 && limot->m_servoTargetm_hiLimit ? limot->m_servoTarget : limot->m_hiLimit; + lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit; + hiLimit = error < 0 && curServoTargetm_hiLimit ? curServoTarget : limot->m_hiLimit; } mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); } @@ -998,13 +1013,49 @@ void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar veloc m_angularLimits[index - 3].m_targetVelocity = velocity; } -void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target) + + +void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg) { btAssert((index >= 0) && (index < 6)); if (index<3) - m_linearLimits.m_servoTarget[index] = target; + { + m_linearLimits.m_servoTarget[index] = targetOrg; + } else + { + //wrap between -PI and PI, see also + //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi + + btScalar target = targetOrg+SIMD_PI; + if (1) + { + btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI); + // handle boundary cases resulted from floating-point cut off: + { + if (m>=SIMD_2_PI) + { + target = 0; + } else + { + if (m<0 ) + { + if (SIMD_2_PI+m == SIMD_2_PI) + target = 0; + else + target = SIMD_2_PI+m; + } + else + { + target = m; + } + } + } + target -= SIMD_PI; + } + m_angularLimits[index - 3].m_servoTarget = target; + } } void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 736a64a1c..9f04f2805 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" -#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) +#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) :btTypedObject(type),