Merge pull request #1038 from YunfeiBai/master

Load body name from URDF and add API to get the body name.

data/cube.urdf

@@ -1,5 +1,5 @@
 <?xml version="0.0" ?>
-<robot name="cube.urdf">
+<robot name="cube">
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>

data/plane.urdf

@@ -1,5 +1,5 @@
 <?xml version="0.0" ?>
-<robot name="cube.urdf">
+<robot name="plane">
   <link name="planeLink">
   <contact>
       <lateral_friction value="1"/>

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -1323,6 +1323,11 @@ int BulletMJCFImporter::getRootLinkIndex() const
 	return "";
  }
 
+std::string BulletMJCFImporter::getBodyName() const
+{
+	return m_data->m_fileModelName;
+}
+
 bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
 {
 //	UrdfLink* link = m_data->getLink(linkIndex);

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h

@@ -41,6 +41,8 @@ public:
     
     ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
     virtual std::string getLinkName(int linkIndex) const;
+    
+	virtual std::string getBodyName() const;
 
 	/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
 	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -269,6 +269,11 @@ std::string BulletURDFImporter::getLinkName(int linkIndex) const
 	}
 	return "";
 }
+
+std::string BulletURDFImporter::getBodyName() const
+{
+	return m_data->m_urdfParser.getModel().m_name;
+}
     
 std::string BulletURDFImporter::getJointName(int linkIndex) const
 {

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -34,6 +34,8 @@ public:
 	virtual int getRootLinkIndex() const;
     
     virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
+    
+	virtual std::string getBodyName() const;
 
     virtual std::string getLinkName(int linkIndex) const;
 

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -27,6 +27,9 @@ public:
     
     ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
     virtual std::string getLinkName(int linkIndex) const =0;
+    
+	virtual std::string getBodyName() const = 0;
+    
 	/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
 	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -552,7 +552,6 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
     return 0;
 }
 
-
 b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -20,6 +20,7 @@ struct BodyJointInfoCache
 {
 	std::string m_baseName;
 	btAlignedObjectArray<b3JointInfo> m_jointInfo;
+	std::string m_bodyName;
 };
 
 struct PhysicsClientSharedMemoryInternalData {
@@ -106,6 +107,7 @@ bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo&
 	{
 		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
 		info.m_baseName = bodyJoints->m_baseName.c_str();
+		info.m_bodyName = bodyJoints->m_bodyName.c_str();
 		return true;
 	}
 
@@ -306,6 +308,7 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
     
     BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
     m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+	bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
     
     for (int i = 0; i < bf.m_multiBodies.size(); i++)
     {
@@ -418,6 +421,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 
 					BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
                     m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+					bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
 
                     for (int i = 0; i < bf.m_multiBodies.size(); i++) {
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -17,6 +17,7 @@ struct BodyJointInfoCache2
 {
 	std::string m_baseName;
 	btAlignedObjectArray<b3JointInfo> m_jointInfo;
+	std::string m_bodyName;
 };
 
 
@@ -605,6 +606,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
 	
     BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
     m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+	bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
 
     for (int i = 0; i < bf.m_multiBodies.size(); i++) 
     {
@@ -931,6 +933,7 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
 	{
 		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
 		info.m_baseName = bodyJoints->m_baseName.c_str();
+		info.m_bodyName = bodyJoints->m_bodyName.c_str();
 		return true;
 	}
 	

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -120,6 +120,7 @@ struct InteralBodyData
 	btMultiBody* m_multiBody;
 	btRigidBody* m_rigidBody;
 	int m_testData;
+	std::string m_bodyName;
 
 	btTransform m_rootLocalInertialFrame;
 	btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
@@ -1607,6 +1608,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 
         btMultiBody* mb = creation.getBulletMultiBody();
         btRigidBody* rb = creation.getRigidBody();
+		
+		bodyHandle->m_bodyName = u2b.getBodyName();
 
 		if (useMultiBody)
 		{
@@ -2650,6 +2653,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
 						}
 						serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
+						InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+						strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
 						hasStatus = true;
 
                     } else
@@ -5015,8 +5020,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 						break;        
 					}
-		
-
                 default:
                 {
 					BT_PROFILE("CMD_UNKNOWN");

examples/SharedMemory/SharedMemoryCommands.h

@@ -103,6 +103,7 @@ struct BulletDataStreamArgs
 {
 	char m_bulletFileName[MAX_FILENAME_LENGTH];
 	int m_bodyUniqueId;
+	char m_bodyName[MAX_FILENAME_LENGTH];
 };
 
 struct SetJointFeedbackArgs
@@ -364,9 +365,6 @@ struct RequestActualStateArgs
 	int m_bodyUniqueId;
 };
 
-
-
-
 struct SendActualStateArgs
 {
 	int m_bodyUniqueId;

examples/SharedMemory/SharedMemoryPublic.h

@@ -201,6 +201,7 @@ struct b3UserConstraint
 struct b3BodyInfo
 {
 	const char* m_baseName;
+	const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
 };
 
 

examples/pybullet/examples/kuka_with_cube.py

@@ -48,6 +48,9 @@ p.setRealTimeSimulation(useRealTimeSimulation)
 trailDuration = 15
 
 logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
+
+for i in xrange(5):
+    print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1])
 	
 while 1:
 	if (useRealTimeSimulation):

examples/pybullet/pybullet.c

@@ -1714,8 +1714,9 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject*
 			struct b3BodyInfo info;
 			if (b3GetBodyInfo(sm, bodyUniqueId, &info))
 			{
-				PyObject* pyListJointInfo = PyTuple_New(1);
+				PyObject* pyListJointInfo = PyTuple_New(2);
 				PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName));
+				PyTuple_SetItem(pyListJointInfo, 1, PyString_FromString(info.m_bodyName));
 				return pyListJointInfo;
 			}
 		}
@@ -5145,7 +5146,7 @@ static PyMethodDef SpamMethods[] = {
 
 	{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
 	 "Get the state (position, velocity etc) for a joint on a body."},
-
+	
 	{"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS,
 	 "Provides extra information such as the Cartesian world coordinates"
 	 " center of mass (COM) of the link, relative to the world reference"