From e5d4c73c4040e5df78a170e52a6c13a7f24fffea Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 9 Oct 2017 22:40:53 -0700 Subject: [PATCH 1/3] fix very rare threading issue, let main thread compute the UID for user debug items add collision model to cartpole.urdf --- data/cartpole.urdf | 6 ++++++ examples/SharedMemory/PhysicsServerExample.cpp | 15 ++++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/data/cartpole.urdf b/data/cartpole.urdf index c22ca920c..e78b066c1 100644 --- a/data/cartpole.urdf +++ b/data/cartpole.urdf @@ -62,6 +62,12 @@ + + + + + + diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 3ab943e3c..3c9b4a70c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1131,7 +1131,7 @@ public: btAlignedObjectArray m_userDebugText; UserDebugText m_tmpText; - + int m_resultUserDebugTextUid; virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags) { @@ -1165,7 +1165,7 @@ public: m_cs->setSharedParam(1, eGUIUserDebugAddText); workerThreadWait(); - return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId; + return m_resultUserDebugTextUid; } btAlignedObjectArray m_userDebugParams; @@ -1183,6 +1183,7 @@ public: } return 0; } + int m_userDebugParamUid; virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue) { @@ -1196,13 +1197,13 @@ public: m_cs->setSharedParam(1, eGUIUserDebugAddParameter); workerThreadWait(); - return (*m_userDebugParams[m_userDebugParams.size()-1]).m_itemUniqueId; + return m_userDebugParamUid; } btAlignedObjectArray m_userDebugLines; UserDebugDrawLine m_tmpLine; - + int m_resultDebugLineUid; virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex) { m_tmpLine.m_lifeTime = lifeTime; @@ -1223,7 +1224,7 @@ public: m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddLine); workerThreadWait(); - return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId; + return m_resultDebugLineUid; } int m_removeDebugItemUid; @@ -2171,6 +2172,7 @@ void PhysicsServerExample::updateGraphics() B3_PROFILE("eGUIUserDebugAddText"); m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText); + m_multiThreadedHelper->m_resultUserDebugTextUid = m_multiThreadedHelper->m_userDebugText[m_multiThreadedHelper->m_userDebugText.size()-1].m_itemUniqueId; m_multiThreadedHelper->mainThreadRelease(); break; } @@ -2190,6 +2192,8 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->m_childGuiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + m_multiThreadedHelper->m_userDebugParamUid = (*m_multiThreadedHelper->m_userDebugParams[m_multiThreadedHelper->m_userDebugParams.size()-1]).m_itemUniqueId; + //also add actual menu m_multiThreadedHelper->mainThreadRelease(); break; @@ -2199,6 +2203,7 @@ void PhysicsServerExample::updateGraphics() B3_PROFILE("eGUIUserDebugAddLine"); m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); + m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId; m_multiThreadedHelper->mainThreadRelease(); break; } From 7bddc7706d7ba330b59f14bbcc12d1728024cb0b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 10 Oct 2017 09:08:31 -0700 Subject: [PATCH 2/3] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 612404491..aacca7f02 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.5.5', + version='1.5.6', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement 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', From c155e105a52f1768df8251e1da132b61991382b7 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 10 Oct 2017 11:10:42 -0700 Subject: [PATCH 3/3] unlock thread at exitPhysics pybullet: don't crash in inverse kinematic if #dofs don't match due to free base C-API: don't crash if status/statusHandle = 0 --- .../InProcessExampleBrowser.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 63 +++++-- .../PhysicsServerCommandProcessor.cpp | 155 +++++++++--------- .../SharedMemory/PhysicsServerExample.cpp | 3 +- 4 files changed, 128 insertions(+), 95 deletions(-) diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 7b42da5be..caf32a51a 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -391,7 +391,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data) } else { // printf("polling.."); - b3Clock::usleep(1000); + b3Clock::usleep(0); } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d8de6b489..58a251949 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1654,6 +1654,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3Ph B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + btAssert(status); + if (status==0) + return 0; const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs; btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED); if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED) @@ -1695,6 +1698,9 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle const double* actualStateQdot[], const double* jointReactionForces[]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + btAssert(status); + if (status==0) + return 0; const SendActualStateArgs &args = status->m_sendActualStateArgs; btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED); if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) @@ -1910,6 +1916,8 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; b3Assert(status); + if (status==0) + return statusUniqueId; b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) { @@ -1924,10 +1932,13 @@ B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHan const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; b3Assert(status); - b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); - if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) + if (status) { - statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId; + b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); + if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) + { + statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId; + } } return statusUniqueId; } @@ -2317,12 +2328,14 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); - b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED); - if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED) + if (status) { - return status->m_userConstraintResultArgs.m_userConstraintUniqueId; + b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED); + if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED) + { + return status->m_userConstraintResultArgs.m_userConstraintUniqueId; + } } - return -1; } @@ -2635,11 +2648,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; - btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED); - if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED)) + if (status) { - *paramValue = status->m_userDebugDrawArgs.m_parameterValue; - return 1; + btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED); + if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED)) + { + *paramValue = status->m_userDebugDrawArgs.m_parameterValue; + return 1; + } } return 0; } @@ -3280,10 +3296,13 @@ B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHa { int uid = -1; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; - btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED); - if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED) + if (status) { - uid = status->m_loadTextureResultArguments.m_textureUniqueId; + btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED); + if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED) + { + uid = status->m_loadTextureResultArguments.m_textureUniqueId; + } } return uid; } @@ -3420,6 +3439,9 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand double* jointForces) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status==0) + return false; + btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED); if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) return false; @@ -3475,6 +3497,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status==0) + return false; + btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED); if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED) return false; @@ -3528,6 +3553,9 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar b3Assert(cl); const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status==0) + return false; + btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED); if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED) return false; @@ -3661,9 +3689,12 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu double* jointPositions) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status); + if (status==0) + return 0; btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED); if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED) - return false; + return 0; if (dofCount) @@ -3682,7 +3713,7 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu } } - return true; + return 1; } B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4053158e3..6fbce838b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7659,7 +7659,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray q_current; q_current.resize(numDofs); - if (tree) + if (tree && (numDofs == tree->numDoFs())) { jacSize = jacobian_linear.size(); // Set jacobian value @@ -7697,92 +7697,93 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - } - btAlignedObjectArray q_new; - q_new.resize(numDofs); - int ikMethod = 0; - if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) - { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; - } - else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) - { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; - } - else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; - } - else - { - ikMethod = IK2_VEL_DLS; - } - if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - for (int i = 0; i < numDofs; ++i) - { - lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; - upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; - joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; - } - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } + btAlignedObjectArray q_new; + q_new.resize(numDofs); + int ikMethod = 0; + if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; + } + else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + } + else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; + } + else + { + ikMethod = IK2_VEL_DLS; + } - btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; + upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; + joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + } + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } + + btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); - btVector3DoubleData endEffectorWorldPosition; - btVector3DoubleData endEffectorWorldOrientation; + btVector3DoubleData endEffectorWorldPosition; + btVector3DoubleData endEffectorWorldOrientation; - btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation(); - btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); + btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin(); + btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation(); + btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); - endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); - endEffectorOri.serializeDouble(endEffectorWorldOrientation); + endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); + endEffectorOri.serializeDouble(endEffectorWorldOrientation); - // Set joint damping coefficents. A small default - // damping constant is added to prevent singularity - // with pseudo inverse. The user can set joint damping - // coefficients differently for each joint. The larger - // the damping coefficient is, the less we rely on - // this joint to achieve the IK target. - btAlignedObjectArray joint_damping; - joint_damping.resize(numDofs,0.5); - if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) - { - for (int i = 0; i < numDofs; ++i) - { - joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; - } - } - ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); + // Set joint damping coefficents. A small default + // damping constant is added to prevent singularity + // with pseudo inverse. The user can set joint damping + // coefficients differently for each joint. The larger + // the damping coefficient is, the less we rely on + // this joint to achieve the IK target. + btAlignedObjectArray joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) + { + for (int i = 0; i < numDofs; ++i) + { + joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + } + } + ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); - double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); + double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; + ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i=0;iunlock(); m_args[i].m_cs->lock(); m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion); m_args[i].m_cs->unlock(); @@ -1805,7 +1806,7 @@ void PhysicsServerExample::exitPhysics() } else { - b3Clock::usleep(1000); + b3Clock::usleep(0); } //we need to call 'stepSimulation' to make sure that //other threads get out of blocking state (workerThreadWait)