Add the API to get the body name and pybullet example call.

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/SharedMemory/PhysicsClientC_API.cpp

@@ -539,6 +539,17 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
     return 0;
 }
 
+b3SharedMemoryCommandHandle b3RequestBodyNameCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
+{
+    PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type = CMD_REQUEST_BODY_NAME;
+    command->m_requestBodyNameArguments.m_bodyUniqueId = bodyUniqueId;
+    return (b3SharedMemoryCommandHandle) command;
+}
 
 b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
@@ -1019,6 +1030,20 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
     return true;
 }
 
+int b3GetBodyName(b3SharedMemoryStatusHandle statusHandle,
+                  struct b3BodyInfo* info)
+{
+    const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+    const SendBodyNameArgs &args = status->m_sendBodyNameArgs;
+    btAssert(status->m_type == CMD_REQUEST_BODY_NAME_COMPLETED);
+    if (status->m_type != CMD_REQUEST_BODY_NAME_COMPLETED)
+        return false;
+    
+    info->m_bodyName = args.m_bodyName;
+    
+    return true;
+}
+
 int	b3CanSubmitCommand(b3PhysicsClientHandle physClient)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -57,6 +57,9 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
                            const double* actualStateQdot[],
                            const double* jointReactionForces[]);
 
+int b3GetBodyName(b3SharedMemoryStatusHandle statusHandle,
+                           struct b3BodyInfo* info);
+
 ///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
 b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
 
@@ -308,6 +311,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
 ///b3CreateSensorEnableIMUForLink is not implemented yet.
 ///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
 int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
+    
+b3SharedMemoryCommandHandle b3RequestBodyNameCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
 
 b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
 int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -939,6 +939,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 				b3Warning("State Logging failed");
 				break;
 			}
+            case CMD_REQUEST_BODY_NAME_COMPLETED:
+            {
+                break;
+            }
+            case CMD_REQUEST_BODY_NAME_FAILED:
+            {
+                b3Warning("Request body name failed");
+                break;
+            }
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4906,8 +4906,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 						break;        
 					}
-		
+                case CMD_REQUEST_BODY_NAME:
-
+                {
+                    int bodyUniqueId = clientCmd.m_requestBodyNameArguments.m_bodyUniqueId;
+                    InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+                    if (body)
+                    {
+                        SharedMemoryStatus& serverCmd = serverStatusOut;
+                        serverCmd.m_type = CMD_REQUEST_BODY_NAME_COMPLETED;
+                        strcpy(serverCmd.m_sendBodyNameArgs.m_bodyName, body->m_bodyName.c_str());
+                        hasStatus = true;
+                    }
+                    else
+                    {
+                        b3Warning("The body name requested is not available");
+                        SharedMemoryStatus& serverCmd = serverStatusOut;
+                        serverCmd.m_type = CMD_REQUEST_BODY_NAME_FAILED;
+                        hasStatus = true;
+                    }
+                    break;
+                }
                 default:
                 {
                     b3Error("Unknown command encountered");

examples/SharedMemory/SharedMemoryCommands.h

@@ -356,14 +356,21 @@ struct LoadBunnyArgs
     double m_collisionMargin;
 };
 
+struct RequestBodyNameArgs
+{
+    int m_bodyUniqueId;
+};
+
+struct SendBodyNameArgs
+{
+    char m_bodyName[MAX_FILENAME_LENGTH];
+};
+
 struct RequestActualStateArgs
 {
 	int m_bodyUniqueId;
 };
 
-
-
-
 struct SendActualStateArgs
 {
 	int m_bodyUniqueId;
@@ -713,6 +720,7 @@ struct SharedMemoryCommand
 		struct VRCameraState m_vrCameraStateArguments;
 		struct StateLoggingRequest m_stateLoggingArguments;
         struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
+        struct RequestBodyNameArgs m_requestBodyNameArguments;
     };
 };
 
@@ -775,6 +783,7 @@ struct SharedMemoryStatus
 		struct SendKeyboardEvents m_sendKeyboardEvents;
 		struct SendRaycastHits m_raycastHits;
 		struct StateLoggingResultArgs m_stateLoggingResultArgs;
+        struct SendBodyNameArgs m_sendBodyNameArgs;
 
 	};
 };

examples/SharedMemory/SharedMemoryPublic.h

@@ -54,6 +54,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_STATE_LOGGING,
     CMD_CONFIGURE_OPENGL_VISUALIZER,
 	CMD_REQUEST_KEYBOARD_EVENTS_DATA,
+    CMD_REQUEST_BODY_NAME,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -133,6 +134,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_STATE_LOGGING_FAILED,
 		CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED,
 		CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED,
+        CMD_REQUEST_BODY_NAME_COMPLETED,
+        CMD_REQUEST_BODY_NAME_FAILED,
         //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
@@ -197,6 +200,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/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.getBodyName(i)[0])
 	
 while 1:
 	if (useRealTimeSimulation):

examples/pybullet/pybullet.c

@@ -2232,6 +2232,69 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject
 	return Py_None;
 }
 
+// Returns the name of a body given the bodyIndex
+//
+// Args:
+//  bodyIndex - integer indicating body in simulation
+//
+// The returned pylist includes the body name in string format
+
+static PyObject* pybullet_getBodyName(PyObject* self, PyObject* args, PyObject* keywds)
+{
+    {
+        int bodyUniqueId = -1;
+        b3PhysicsClientHandle sm = 0;
+        
+        int physicsClientId = 0;
+        static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL};
+        if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
+        {
+            return NULL;
+        }
+        sm = getPhysicsClient(physicsClientId);
+        if (sm == 0)
+        {
+            PyErr_SetString(SpamError, "Not connected to physics server.");
+            return NULL;
+        }
+        
+        {
+            int status_type = 0;
+            b3SharedMemoryCommandHandle cmd_handle;
+            b3SharedMemoryStatusHandle status_handle;
+            
+            if (bodyUniqueId < 0)
+            {
+                PyErr_SetString(SpamError, "getBodyName failed; invalid bodyIndex");
+                return NULL;
+            }
+            
+            cmd_handle =
+            b3RequestBodyNameCommandInit(sm, bodyUniqueId);
+            status_handle =
+            b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+            
+            status_type = b3GetStatusType(status_handle);
+            if (status_type != CMD_REQUEST_BODY_NAME_COMPLETED)
+            {
+                PyErr_SetString(SpamError, "getBodyName failed; invalid return status");
+                return NULL;
+            }
+
+            struct b3BodyInfo info;
+            if (b3GetBodyName(status_handle, &info))
+            {
+                PyObject* pyBodyNameInfo = PyTuple_New(1);
+                PyTuple_SetItem(pyBodyNameInfo, 0, PyString_FromString(info.m_bodyName));
+                return pyBodyNameInfo;
+            }
+        }
+    }
+    PyErr_SetString(SpamError, "Couldn't get body name");
+    return NULL;
+}
+
+
 static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* pyLinkState;
@@ -5100,6 +5163,9 @@ static PyMethodDef SpamMethods[] = {
 
 	{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
 	 "Get the state (position, velocity etc) for a joint on a body."},
+    
+    {"getBodyName", (PyCFunction)pybullet_getBodyName, METH_VARARGS | METH_KEYWORDS,
+     "Get the name of a body."},
 
 	{"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS,
 	 "Provides extra information such as the Cartesian world coordinates"