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/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 478e31031..36212cc8c 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; } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 9dea925d3..1e9ebbcf3 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(); 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..f6b95f37e 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,23 @@ 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); + numJoints = b3GetNumJoints(sm,bodyIndicesOut[0]); + printf("numJoints: %d\n", numJoints); + //ASSERT_EQ(numBodies ==1); + } + { b3SharedMemoryStatusHandle statusHandle; int statusType;