From 95a09bec9977cbba3a5587ffa54c38d1b1b42a23 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 15 Jun 2016 18:01:41 -0700 Subject: [PATCH] implement getJointInfo for objects loaded through SDF --- examples/SharedMemory/PhysicsClient.h | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsClientSharedMemory.cpp | 99 +++++++++++++++++-- .../SharedMemory/PhysicsClientSharedMemory.h | 4 +- examples/SharedMemory/PhysicsDirect.cpp | 9 +- examples/SharedMemory/PhysicsDirect.h | 2 +- examples/SharedMemory/PhysicsLoopBack.cpp | 4 +- examples/SharedMemory/PhysicsLoopBack.h | 2 +- test/SharedMemory/test.c | 14 ++- 10 files changed, 122 insertions(+), 20 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 59a1a4e90..07b10aa13 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -26,7 +26,7 @@ public: virtual int getNumJoints(int bodyIndex) const = 0; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0; virtual void setSharedMemoryKey(int key) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 81a58a992..a627e08af 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -653,10 +653,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) } -void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info) +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; - cl->getJointInfo(bodyIndex, linkIndex,*info); + return cl->getJointInfo(bodyIndex, linkIndex,*info); } b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 468347ef5..4ae1a5235 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -57,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); ///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h -void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); ///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index a4c57574d..3ca2aa5c5 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -37,6 +37,9 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedCameraPixelsRGBA; btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_bodyIdsRequestInfo; + SharedMemoryStatus m_tempBackupServerStatus; + SharedMemoryStatus m_lastServerStatus; int m_counter; @@ -82,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const } -void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const +bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; info = bodyJoints->m_jointInfo[jointIndex]; + return true; } - + return false; } PhysicsClientSharedMemory::PhysicsClientSharedMemory() @@ -167,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() { return true; } + +///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo +void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + bParse::btBulletFile bf( + &this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0], + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf.setFileDNAisMemoryDNA(); + bf.parse(false); + + + BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } +} + const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { SharedMemoryStatus* stat = 0; @@ -208,6 +254,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_verboseOutput) { b3Printf("Server loading the SDF OK\n"); } + break; } case CMD_URDF_LOADING_COMPLETED: { @@ -275,6 +322,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + case CMD_BODY_INFO_COMPLETED: + { + if (m_data->m_verboseOutput) { + b3Printf("Received body info\n"); + } + int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyUniqueId, serverCmd); + + break; + } case CMD_SDF_LOADING_FAILED: { if (m_data->m_verboseOutput) { b3Printf("Server failed loading the SDF...\n"); @@ -504,21 +561,49 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_waitingForServer = true; } - /*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) + if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) { int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; if (numBodies>0) { + m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; + + for (int i=0;im_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]); + } + + int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1]; + m_data->m_bodyIdsRequestInfo.pop_back(); + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; //now transfer the information of the individual objects etc. - command.m_type = CMD_REQUEST_SDF_INFO; - command.m_updateFlags = SDF_REQUEST_INFO_BODY; - command.m_sdfRequestInfoArgs.m_infoIndex = 0; + command.m_type = CMD_REQUEST_BODY_INFO; + command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; submitClientCommand(command); return 0; } } - */ + + if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED) + { + //are there any bodies left to be processed? + if (m_data->m_bodyIdsRequestInfo.size()) + { + int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1]; + m_data->m_bodyIdsRequestInfo.pop_back(); + + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + //now transfer the information of the individual objects etc. + command.m_type = CMD_REQUEST_BODY_INFO; + command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; + submitClientCommand(command); + return 0; + } else + { + m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; + } + } if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 1081771e5..a47fc551a 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -11,7 +11,9 @@ class PhysicsClientSharedMemory : public PhysicsClient { protected: virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + public: PhysicsClientSharedMemory(); virtual ~PhysicsClientSharedMemory(); @@ -34,7 +36,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 36212cc8c..511290778 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -393,14 +393,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const return 0; } -void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - info = bodyJoints->m_jointInfo[jointIndex]; + if (jointIndex < bodyJoints->m_jointInfo.size()) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } } + return false; } ///todo: move this out of the diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 1e9ebbcf3..aaa2d8639 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -48,7 +48,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 985352113..ada37ce88 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -79,9 +79,9 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const return m_data->m_physicsClient->getNumJoints(bodyIndex); } -void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const { - m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); + return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); } ///todo: move this out of the diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index edc71ceb9..8c0e187b6 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -40,7 +40,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index f6b95f37e..01d0a16a8 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -65,8 +65,18 @@ void testSharedMemory(b3PhysicsClientHandle sm) numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); ASSERT_EQ(numBodies,1); - numJoints = b3GetNumJoints(sm,bodyIndicesOut[0]); - printf("numJoints: %d\n", numJoints); + int bodyUniqueId = bodyIndicesOut[0]; + + numJoints = b3GetNumJoints(sm,bodyUniqueId); + b3Printf("numJoints: %d\n", numJoints); + for (i=0;i