From 7cb5898db6ea4001077bc3e8795414abc01227a0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 3 Mar 2017 11:33:20 -0800 Subject: [PATCH] fix body name in GUI more expose getBodyInfo to b3RobotSimulatorClientAPI --- .../RobotSimulator/RobotSimulatorMain.cpp | 4 +++ .../b3RobotSimulatorClientAPI.cpp | 29 +++++++++++++++++++ .../b3RobotSimulatorClientAPI.h | 2 ++ .../PhysicsClientSharedMemory.cpp | 10 +++++-- 4 files changed, 43 insertions(+), 2 deletions(-) diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index 0bbc57113..6592cf5cb 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -24,6 +24,10 @@ int main(int argc, char* argv[]) sim->setGravity(b3MakeVector3(0,0,-10)); + int blockId = sim->loadURDF("cube.urdf"); + b3BodyInfo bodyInfo; + sim->getBodyInfo(blockId,&bodyInfo); + sim->loadURDF("plane.urdf"); MinitaurSetup minitaur; diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 4cf808e3b..65692358d 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -342,6 +342,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) { + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command; @@ -397,9 +403,26 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu return statusOk; } +bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); + return (result != 0); +} bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const { + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); b3SharedMemoryStatusHandle status_handle = @@ -431,6 +454,12 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) { + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle commandHandle; commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index 3d1dad434..f1ca5fe98 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -150,6 +150,8 @@ public: bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); + bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); + bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 8efaf6561..0910e1adc 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -414,13 +414,19 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) { Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; - + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; - + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } }