From a3e4582bef526d5d808a3fb74b3f379c98fb7b4b Mon Sep 17 00:00:00 2001 From: donghokang Date: Tue, 29 May 2018 16:23:17 +0200 Subject: [PATCH 1/3] getCollisionShapeData and getVisualShapeData were added to RobotSimulatorClinetAPI_NoGUI. b3RobotSimulatorJointMotorArrayArgs initialization bug fix. --- .../b3RobotSimulatorClientAPI_NoGUI.cpp | 55 ++++++++++++++++++- .../b3RobotSimulatorClientAPI_NoGUI.h | 17 +++++- 2 files changed, 69 insertions(+), 3 deletions(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index cc32a3913..760670382 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -1626,6 +1626,10 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); } + if (args.m_restitutionVelocityThreshold >= 0) { + b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); return true; } @@ -2060,4 +2064,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); }; From 6bd7a52bf8d2c43ec4cfa95972b6dbd91c5c8ffe Mon Sep 17 00:00:00 2001 From: donghokang Date: Tue, 29 May 2018 16:25:28 +0200 Subject: [PATCH 2/3] solverResidualThreshold now can be set. --- examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index 760670382..dfe6e19a2 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -1626,7 +1626,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); } - if (args.m_restitutionVelocityThreshold >= 0) { + if (args.m_solverResidualThreshold >= 0) { b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold); } From 325ccad2582257d30105857fef6ed0067f718118 Mon Sep 17 00:00:00 2001 From: donghokang Date: Tue, 29 May 2018 16:41:37 +0200 Subject: [PATCH 3/3] getDynamicsInfo function now works. Changed to call b3GetDynamicsInfo in getDynamicsInfo. --- examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index dfe6e19a2..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");