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)