implement getJointInfo for objects loaded through SDF

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;
 

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,

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

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -37,6 +37,9 @@ struct PhysicsClientSharedMemoryInternalData {
 	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
 	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
 
+    btAlignedObjectArray<int> 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;i<numBodies;i++)
+                {
+                    m_data->m_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_type = CMD_REQUEST_BODY_INFO;
-                command.m_updateFlags = SDF_REQUEST_INFO_BODY;
+                command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
-                command.m_sdfRequestInfoArgs.m_infoIndex = 0;
                 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)
 		{

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

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

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

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

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

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]);
+            int bodyUniqueId = bodyIndicesOut[0];
-            printf("numJoints: %d\n", numJoints);
+            
+            numJoints = b3GetNumJoints(sm,bodyUniqueId);
+            b3Printf("numJoints: %d\n", numJoints);
+            for (i=0;i<numJoints;i++)
+            {
+                struct b3JointInfo jointInfo;
+                if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
+                {
+                    b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
+                }
+            }
             //ASSERT_EQ(numBodies ==1);
         }