From d717b1db84ce819a2a23452b36b5c07baf05f70f Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 23 Mar 2017 13:54:44 -0700 Subject: [PATCH] Add the API to get the body name and pybullet example call. --- data/cube.urdf | 2 +- data/plane.urdf | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 5 ++ .../PhysicsClientSharedMemory.cpp | 9 +++ .../PhysicsServerCommandProcessor.cpp | 22 ++++++- examples/SharedMemory/SharedMemoryCommands.h | 15 ++++- examples/SharedMemory/SharedMemoryPublic.h | 4 ++ examples/pybullet/kuka_with_cube.py | 3 + examples/pybullet/pybullet.c | 66 +++++++++++++++++++ 10 files changed, 146 insertions(+), 7 deletions(-) diff --git a/data/cube.urdf b/data/cube.urdf index 679e106b5..4a6d484ec 100644 --- a/data/cube.urdf +++ b/data/cube.urdf @@ -1,5 +1,5 @@ - + diff --git a/data/plane.urdf b/data/plane.urdf index 1b3d09682..b04f67e96 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -1,5 +1,5 @@ - + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5ac6ea74a..39ea58041 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 01eb55cc1..0333d4dc4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 0910e1adc..1149ccab7 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 284b5a60d..5a694bdf1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 1c119c4d1..1c5ecb0b1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 6421a37e0..3778a73fa 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 }; diff --git a/examples/pybullet/kuka_with_cube.py b/examples/pybullet/kuka_with_cube.py index 309449138..f955ee978 100644 --- a/examples/pybullet/kuka_with_cube.py +++ b/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): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 36ceb794c..0807f80af 100644 --- a/examples/pybullet/pybullet.c +++ b/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"