work-in-progress send object/joint information after loading SDF file

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<numBodies;i++)
+                    {
+                        int bodyUniqueId = bodyIndicesOut[i];
+                        int numJoints =  b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
+                        b3Printf("numJoints = %d", numJoints);
+                        for (int i=0;i<numJoints;i++)
+                        {
+                            b3JointInfo info;
+                            b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,i,&info);
+                            b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+                        }
+                    }
+                    */
+                }
+			    
+			    //int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
+			    
 				//int bodyIndex = b3GetStatusBodyIndex(status);
 				/*if (bodyIndex>=0)
 				{

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;i<numBodies;i++)
+                {
+                    int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
+                    SharedMemoryCommand infoRequestCommand;
+                    infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
+                    infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
+                    SharedMemoryStatus infoStatus;
+                    bool hasStatus = m_data->m_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(
+				    int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
-						&m_data->m_bulletStreamDataServerToClient[0],
+                    processBodyJointInfo(bodyIndex,serverCmd);
-						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");
-					}
 				}
                 break;
             }

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();

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;i<mb->getNumLinks();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;

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();

examples/SharedMemory/SharedMemoryCommands.h

@@ -312,7 +312,7 @@ struct SdfLoadedArgs
 
 struct SdfRequestInfoArgs
 {
-    int m_infoIndex;
+    int m_bodyUniqueId;
 };
 
 enum EnumSdfRequestInfoFlags

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
 };

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;