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/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index acce1dfde..691dbb9c6 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -237,7 +237,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -656,6 +656,32 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (statusType == CMD_SDF_LOADING_COMPLETED) { + int bodyIndicesOut[1024]; + int bodyCapacity = 1024; + int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity); + if (numBodies > bodyCapacity) + { + b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity); + } else + { + /* + for (int i=0;i=0) { 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 478e31031..511290778 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -248,6 +248,47 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) } + +void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf.setFileDNAisMemoryDNA(); + bf.parse(false); + + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + 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"); + } +} + bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { if (command.m_type==CMD_REQUEST_DEBUG_LINES) @@ -296,46 +337,35 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman break; } + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i=0;im_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, infoStatus); + } + } + break; + } case CMD_URDF_LOADING_COMPLETED: { + if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) { - bParse::btBulletFile bf( - &m_data->m_bulletStreamDataServerToClient[0], - serverCmd.m_dataStreamArguments.m_streamChunkLength); - bf.setFileDNAisMemoryDNA(); - bf.parse(false); - int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; - - BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; - m_data->m_bodyJointMap.insert(bodyIndex,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"); - } + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex,serverCmd); } break; } @@ -363,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 9dea925d3..aaa2d8639 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -21,6 +21,8 @@ protected: bool processCamera(const struct SharedMemoryCommand& orgCommand); + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + public: PhysicsDirect(); @@ -46,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/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 98994ea68..e3a9b7f48 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -762,12 +762,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe createJointMotors(mb); - //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire - - UrdfLinkNameMapUtil* util2 = new UrdfLinkNameMapUtil; - m_data->m_urdfLinkNameMapper.push_back(util2); - util2->m_mb = mb; - util2->m_memSerializer = 0; + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); @@ -963,7 +958,44 @@ void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, } } +int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes) +{ + int streamSizeInBytes = 0; + //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + btMultiBody* mb = bodyHandle->m_multiBody; + if (mb) + { + UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; + m_data->m_urdfLinkNameMapper.push_back(util); + util->m_mb = mb; + util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); + + bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); + util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName); + util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName); + } + + util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); + + util->m_memSerializer->insertHeader(); + + int len = mb->calculateSerializeBufferSize(); + btChunk* chunk = util->m_memSerializer->allocate(len,1); + const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); + util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); + streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); + + } + return streamSizeInBytes; +} bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) { @@ -1171,6 +1203,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_REQUEST_BODY_INFO: + { + const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; + //stream info into memory + int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; + serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes; + hasStatus = true; + break; + } case CMD_LOAD_SDF: { const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 242a41176..bc74a0a27 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -27,6 +27,8 @@ protected: bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); bool supportsJointMotor(class btMultiBody* body, int linkIndex); + + int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes); public: PhysicsServerCommandProcessor(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index a4c93180e..83fd91c34 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -312,7 +312,7 @@ struct SdfLoadedArgs struct SdfRequestInfoArgs { - int m_infoIndex; + int m_bodyUniqueId; }; enum EnumSdfRequestInfoFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a038ecf51..5e6da5c50 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -19,7 +19,7 @@ enum EnumSharedMemoryClientCommand CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_DEBUG_LINES, - CMD_REQUEST_SDF_INFO, + CMD_REQUEST_BODY_INFO, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, CMD_PICK_BODY, @@ -55,6 +55,8 @@ enum EnumSharedMemoryServerStatus CMD_RESET_SIMULATION_COMPLETED, CMD_CAMERA_IMAGE_COMPLETED, CMD_CAMERA_IMAGE_FAILED, + CMD_BODY_INFO_COMPLETED, + CMD_BODY_INFO_FAILED, CMD_INVALID_STATUS, CMD_MAX_SERVER_COMMANDS }; diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index c6417b802..01d0a16a8 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -35,6 +35,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) int sensorJointIndexLeft=-1; int sensorJointIndexRight=-1; const char* urdfFileName = "r2d2.urdf"; + const char* sdfFileName = "kuka_iiwa/model.sdf"; double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; @@ -52,7 +53,33 @@ void testSharedMemory(b3PhysicsClientHandle sm) ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } - + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int bodyIndicesOut[10];//MAX_SDF_BODIES = 10 + int numJoints, numBodies; + b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED); + + numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); + ASSERT_EQ(numBodies,1); + int bodyUniqueId = bodyIndicesOut[0]; + + numJoints = b3GetNumJoints(sm,bodyUniqueId); + b3Printf("numJoints: %d\n", numJoints); + for (i=0;i