Merge pull request #807 from erwincoumans/master

expose getNumBodies, getBodyUniqueId, getBodyInfo in sharedmem/pybullet

examples/SharedMemory/PhysicsClient.h

@@ -24,9 +24,15 @@ public:
 
     virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
 
-    virtual int getNumJoints(int bodyIndex) const = 0;
+	virtual int getNumBodies() const = 0;
 
-    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
+	virtual int getBodyUniqueId(int serialIndex) const = 0;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const = 0;
+	
+    virtual int getNumJoints(int bodyUniqueId) const = 0;
+
+    virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
 
     virtual void setSharedMemoryKey(int key) = 0;
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -739,6 +739,29 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
 }
 
 
+///return the total number of bodies in the simulation
+int	b3GetNumBodies(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	return cl->getNumBodies();
+}
+
+/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
+int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	return cl->getBodyUniqueId(serialIndex);
+}
+
+///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
+int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	return cl->getBodyInfo(bodyUniqueId,*info);
+}
+
+
+
 int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -53,6 +53,15 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
                            const double* actualStateQdot[],
                            const double* jointReactionForces[]);
 
+///return the total number of bodies in the simulation
+int	b3GetNumBodies(b3PhysicsClientHandle physClient);
+
+/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
+int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
+
+///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
+int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
+
 ///give a unique body index (after loading the body) return the number of joints.
 int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
 

examples/SharedMemory/PhysicsClientExample.cpp

@@ -624,7 +624,7 @@ void	PhysicsClientExample::initPhysics()
 	{
         MyCallback(CMD_LOAD_URDF, true, this);
         MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
-        MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
+        
         MyCallback(CMD_RESET_SIMULATION,true,this);
 	}
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -18,6 +18,7 @@
 
 struct BodyJointInfoCache
 {
+	std::string m_baseName;
 	btAlignedObjectArray<b3JointInfo> m_jointInfo;
 };
 
@@ -74,10 +75,37 @@ struct PhysicsClientSharedMemoryInternalData {
 
 
 
-
-int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const 
+int PhysicsClientSharedMemory::getNumBodies() const
 {
-	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+	return m_data->m_bodyJointMap.size();
+}
+
+int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const
+{
+	if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
+	{
+		return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
+	}
+	return -1;
+}
+
+bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
+{
+	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
+		info.m_baseName = bodyJoints->m_baseName.c_str();
+		return true;
+	}
+	return false;
+}
+
+
+
+int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const 
+{
+	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
 	if (bodyJointsPtr && *bodyJointsPtr)
 	{
 		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
@@ -88,9 +116,9 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
 	
 }
 
-bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
+bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
 {
-	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
 	if (bodyJointsPtr && *bodyJointsPtr)
 	{
 		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
@@ -196,12 +224,13 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
             Bullet::btMultiBodyDoubleData* mb =
             (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
             
+			bodyJoints->m_baseName = mb->m_baseName;
             addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
         } else
         {
             Bullet::btMultiBodyFloatData* mb =
             (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
-            
+			bodyJoints->m_baseName = mb->m_baseName;
             addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
         }
     }
@@ -272,10 +301,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                         serverCmd.m_dataStreamArguments.m_streamChunkLength);
                     bf.setFileDNAisMemoryDNA();
                     bf.parse(false);
-					int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+					int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
 
 					BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
-                    m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
+                    m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
 
                     for (int i = 0; i < bf.m_multiBodies.size(); i++) {
 

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -34,9 +34,15 @@ public:
 
     virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
 
-    virtual int getNumJoints(int bodyIndex) const;
+	virtual int getNumBodies() const;
 
-    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+	virtual int getBodyUniqueId(int serialIndex) const;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
+    virtual int getNumJoints(int bodyUniqueId) const;
+
+    virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
 
     virtual void setSharedMemoryKey(int key);
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -9,10 +9,11 @@
 #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
 #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
 #include "BodyJointInfoUtility.h"
-
+#include <string>
 
 struct BodyJointInfoCache2
 {
+	std::string m_baseName;
 	btAlignedObjectArray<b3JointInfo> m_jointInfo;
 };
 
@@ -329,8 +330,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
         serverCmd.m_dataStreamArguments.m_streamChunkLength);
     bf.setFileDNAisMemoryDNA();
     bf.parse(false);
-
-
+	
     BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
     m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
 
@@ -341,13 +341,15 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
         {
             Bullet::btMultiBodyDoubleData* mb =
                 (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
-
+			bodyJoints->m_baseName = mb->m_baseName;
+			
             addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
         } else 
         {
             Bullet::btMultiBodyFloatData* mb =
                 (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
-
+			
+			bodyJoints->m_baseName = mb->m_baseName;
             addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
         }
     }
@@ -458,6 +460,34 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
 	return hasStatus;
 }
 
+int PhysicsDirect::getNumBodies() const
+{
+	return m_data->m_bodyJointMap.size();
+}
+
+
+int PhysicsDirect::getBodyUniqueId(int serialIndex) const
+{
+	if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
+	{
+		return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
+	}
+	return -1;
+}
+
+bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
+{
+	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+		info.m_baseName = bodyJoints->m_baseName.c_str();
+		return true;
+	}
+	
+	return false;
+}
+
 int PhysicsDirect::getNumJoints(int bodyIndex) const
 {
 	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];

examples/SharedMemory/PhysicsDirect.h

@@ -49,6 +49,12 @@ public:
 
     virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
 
+	virtual int getNumBodies() const;
+
+	virtual int getBodyUniqueId(int serialIndex) const;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
     virtual int getNumJoints(int bodyIndex) const;
 
     virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -74,6 +74,21 @@ bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& comm
 	return  m_data->m_physicsClient->submitClientCommand(command);
 }
 
+int PhysicsLoopBack::getNumBodies() const
+{
+	return m_data->m_physicsClient->getNumBodies();
+}
+
+int PhysicsLoopBack::getBodyUniqueId(int serialIndex) const
+{
+	return m_data->m_physicsClient->getBodyUniqueId(serialIndex);
+}
+
+bool PhysicsLoopBack::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
+{
+	return m_data->m_physicsClient->getBodyInfo(bodyUniqueId, info);
+}
+
 int PhysicsLoopBack::getNumJoints(int bodyIndex) const
 {
 	return m_data->m_physicsClient->getNumJoints(bodyIndex);

examples/SharedMemory/PhysicsLoopBack.h

@@ -38,6 +38,12 @@ public:
 
     virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
 
+	virtual int getNumBodies() const;
+
+	virtual int getBodyUniqueId(int serialIndex) const;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
     virtual int getNumJoints(int bodyIndex) const;
 
     virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1064,6 +1064,10 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
         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);
+		if (mb->getBaseName())
+		{
+			util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
+		}
 
         bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
         for (int i=0;i<mb->getNumLinks();i++)

examples/SharedMemory/SharedMemoryPublic.h

@@ -117,6 +117,13 @@ struct b3JointInfo
     double m_jointAxis[3]; // joint axis in parent local frame
 };
 
+struct b3BodyInfo
+{
+	const char* m_baseName;
+};
+
+
+
 struct b3JointSensorState
 {
   double m_jointPosition;

examples/pybullet/premake4.lua

@@ -3,8 +3,7 @@
 project ("pybullet")
 		language "C++"
 		kind "SharedLib"
-		targetsuffix ("")
-		targetprefix ("")
+		
 		includedirs {"../../src", "../../examples",
 		"../../examples/ThirdPartyLibs"}
 		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}

examples/pybullet/pybullet.c

@@ -567,7 +567,7 @@ static int pybullet_internalGetBasePositionAndOrientation(
       const int status_type = b3GetStatusType(status_handle);
       const double* actualStateQ;
       // const double* jointReactionForces[];
-      int i;
+      
 
       if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
         PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
@@ -661,10 +661,88 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
   }
 }
 
+static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
+{
+	if (0 == sm) {
+    PyErr_SetString(SpamError, "Not connected to physics server.");
+    return NULL;
+  }
+
+ {
+    int numBodies = b3GetNumBodies(sm);
+
+#if PY_MAJOR_VERSION >= 3
+    return PyLong_FromLong(numBodies);
+#else
+    return PyInt_FromLong(numBodies);
+#endif
+  }
+}
+
+static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args)
+{
+	if (0 == sm) {
+    PyErr_SetString(SpamError, "Not connected to physics server.");
+    return NULL;
+  }
+
+	{
+    int serialIndex = -1;
+    int bodyUniqueId = -1;
+    if (!PyArg_ParseTuple(args, "i", &serialIndex)) {
+      PyErr_SetString(SpamError, "Expected a serialIndex in range [0..number of bodies).");
+      return NULL;
+    }
+    bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex);
+
+#if PY_MAJOR_VERSION >= 3
+    return PyLong_FromLong(bodyUniqueId);
+#else
+    return PyInt_FromLong(bodyUniqueId);
+#endif
+  }
+}
+
+static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args)
+{
+	if (0 == sm) {
+    PyErr_SetString(SpamError, "Not connected to physics server.");
+    return NULL;
+  }
+
+	{
+    int bodyUniqueId= -1;
+    int numJoints = 0;
+    if (!PyArg_ParseTuple(args, "i", &bodyUniqueId)) 
+	{
+      PyErr_SetString(SpamError, "Expected a body unique id (integer).");
+      return NULL;
+    }
+		{
+			struct b3BodyInfo info;
+			if (b3GetBodyInfo(sm,bodyUniqueId,&info))
+			{
+				PyObject* pyListJointInfo = PyTuple_New(1);
+				PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName));
+				return pyListJointInfo;
+			} else
+			{
+				PyErr_SetString(SpamError, "Couldn't get body info");
+				return NULL;
+			}
+		}
+	}
+
+  PyErr_SetString(SpamError, "error in getBodyInfo.");
+  return NULL;
+}
+
+
 // Return the number of joints in an object based on
 // body index; body index is based on order of sequence
 // the object is loaded into simulation
-static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) {
+static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) 
+{
   if (0 == sm) {
     PyErr_SetString(SpamError, "Not connected to physics server.");
     return NULL;
@@ -888,16 +966,15 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) {
 static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
   PyObject* pyListJointForceTorque;
   PyObject* pyListJointState;
-  PyObject* item;
 
-  struct b3JointInfo info;
+
   struct b3JointSensorState sensorState;
 
   int bodyIndex = -1;
   int jointIndex = -1;
   int sensorStateSize = 4;  // size of struct b3JointSensorState
   int forceTorqueSize = 6;  // size of force torque list from b3JointSensorState
-  int i, j;
+  int j;
 
   int size = PySequence_Size(args);
 
@@ -2005,6 +2082,15 @@ static PyMethodDef SpamMethods[] = {
     {"loadSDF", pybullet_loadSDF, METH_VARARGS,
      "Load multibodies from an SDF file."},
 
+	{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
+     "Get the number of bodies in the simulation."},
+
+	{"getBodyUniqueId", pybullet_getBodyUniqueId, METH_VARARGS,
+     "Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."},
+
+	{"getBodyInfo", pybullet_getBodyInfo, METH_VARARGS,
+     "Get the body info, given a body unique id."},
+	
     {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation,
      METH_VARARGS,
      "Get the world position and orientation of the base of the object. "