diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index cc32a3913..ee6a20989 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -1270,6 +1270,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int link status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); status_type = b3GetStatusType(status_handle); if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) { + b3GetDynamicsInfo(status_handle, dynamicsInfo); return true; } else { b3Warning("getDynamicsInfo did not complete"); @@ -1626,6 +1627,10 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); } + if (args.m_solverResidualThreshold >= 0) { + b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); return true; } @@ -2060,4 +2065,53 @@ void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* gu struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper() { return m_data->m_guiHelper; -} \ No newline at end of file +} + +bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, int linkIndex, + b3CollisionShapeInformation &collisionShapeInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + { + command = b3InitRequestCollisionShapeInformation(sm, bodyUniqueId, linkIndex); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + } + + btAssert(statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED); + if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED) { + b3GetCollisionShapeInformation(sm, &collisionShapeInfo); + } + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + { + commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED); + if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { + b3GetVisualShapeInformation(sm, &visualShapeInfo); + } + return true; + } +} diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h index 02594cf40..2a3dcc328 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h @@ -152,7 +152,14 @@ struct b3RobotSimulatorJointMotorArrayArgs double *m_forces; b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs) - : m_controlMode(controlMode), m_numControlledDofs(numControlledDofs) + : m_controlMode(controlMode), + m_numControlledDofs(numControlledDofs), + m_jointIndices(NULL), + m_targetPositions(NULL), + m_kps(NULL), + m_targetVelocities(NULL), + m_kds(NULL), + m_forces(NULL) { } }; @@ -204,6 +211,7 @@ struct b3RobotSimulatorSetPhysicsEngineParameters double m_erp; double m_contactERP; double m_frictionERP; + double m_solverResidualThreshold; b3RobotSimulatorSetPhysicsEngineParameters() : m_fixedTimeStep(-1), @@ -218,7 +226,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters m_restitutionVelocityThreshold(-1), m_erp(-1), m_contactERP(-1), - m_frictionERP(-1) + m_frictionERP(-1), + m_solverResidualThreshold(-1) {} }; @@ -566,6 +575,10 @@ public: virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); virtual struct GUIHelperInterface* getGuiHelper(); + + bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo); + + bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo); };