From 50f0cfca9e84aa56e33c580acf41cf0ae511dd99 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 23 Mar 2017 10:16:39 -0700 Subject: [PATCH 1/5] Add body name when loading urdf. --- examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp | 5 +++++ examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h | 2 ++ examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp | 5 +++++ examples/Importers/ImportURDFDemo/BulletUrdfImporter.h | 2 ++ examples/Importers/ImportURDFDemo/URDFImporterInterface.h | 3 +++ examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 3 +++ examples/SharedMemory/PhysicsServerCommandProcessor.h | 2 +- 7 files changed, 21 insertions(+), 1 deletion(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 563c657a4..7bdfd9282 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -1311,6 +1311,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); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index a719c984e..927f909e3 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/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; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 5ba817b51..65d8b01f6 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/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 { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 2ceb8e0d7..b27fe7b9c 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -34,6 +34,8 @@ public: virtual int getRootLinkIndex() const; virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; + + virtual std::string getBodyName() const; virtual std::string getLinkName(int linkIndex) const; diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 427f98b3b..cec640771 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/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;} diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4061cea95..284b5a60d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 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) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 7c1732002..8f4485a36 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -35,7 +35,7 @@ protected: bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, - bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); + bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); From d717b1db84ce819a2a23452b36b5c07baf05f70f Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 23 Mar 2017 13:54:44 -0700 Subject: [PATCH 2/5] 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" From de3f91b64ed899ce8173940dca5beef9448a5e1b Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 29 Mar 2017 14:56:05 -0700 Subject: [PATCH 3/5] Get body name from getBodyInfo. --- examples/SharedMemory/PhysicsClientSharedMemory.cpp | 4 ++++ examples/SharedMemory/PhysicsDirect.cpp | 3 +++ examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 ++ examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/pybullet/kuka_with_cube.py | 2 +- examples/pybullet/pybullet.c | 3 ++- 6 files changed, 13 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 1149ccab7..6011d5a68 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -20,6 +20,7 @@ struct BodyJointInfoCache { std::string m_baseName; btAlignedObjectArray 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++) { @@ -405,6 +408,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++) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 5ca0f35a2..40be6dbcb 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -17,6 +17,7 @@ struct BodyJointInfoCache2 { std::string m_baseName; btAlignedObjectArray 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; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2a0f36fd5..adb892805 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2629,6 +2629,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 diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ebb56998f..6049ea9e0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -101,6 +101,7 @@ struct BulletDataStreamArgs { char m_bulletFileName[MAX_FILENAME_LENGTH]; int m_bodyUniqueId; + char m_bodyName[MAX_FILENAME_LENGTH]; }; struct SetJointFeedbackArgs diff --git a/examples/pybullet/kuka_with_cube.py b/examples/pybullet/kuka_with_cube.py index f955ee978..aad32f489 100644 --- a/examples/pybullet/kuka_with_cube.py +++ b/examples/pybullet/kuka_with_cube.py @@ -50,7 +50,7 @@ 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]) + print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1]) while 1: if (useRealTimeSimulation): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a841939d1..5ff38861b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1711,8 +1711,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; } } From 64573c38e4e64748a00fc7c76d90449d5c882fbf Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 29 Mar 2017 15:03:29 -0700 Subject: [PATCH 4/5] Remove getBodyName API. --- examples/SharedMemory/PhysicsClientC_API.cpp | 26 ------- examples/SharedMemory/PhysicsClientC_API.h | 5 -- .../PhysicsClientSharedMemory.cpp | 9 --- .../PhysicsServerCommandProcessor.cpp | 20 ------ examples/SharedMemory/SharedMemoryCommands.h | 12 ---- examples/SharedMemory/SharedMemoryPublic.h | 3 - examples/pybullet/pybullet.c | 68 +------------------ 7 files changed, 1 insertion(+), 142 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 642a2a655..c5861b220 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -539,18 +539,6 @@ 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) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1068,20 +1056,6 @@ 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 aa64dccf3..7aef18f86 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -57,9 +57,6 @@ 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); @@ -314,8 +311,6 @@ 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 6011d5a68..dce4784d8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -943,15 +943,6 @@ 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 adb892805..d8a00513c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4917,26 +4917,6 @@ 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 6049ea9e0..f975932e7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -358,16 +358,6 @@ struct LoadBunnyArgs double m_collisionMargin; }; -struct RequestBodyNameArgs -{ - int m_bodyUniqueId; -}; - -struct SendBodyNameArgs -{ - char m_bodyName[MAX_FILENAME_LENGTH]; -}; - struct RequestActualStateArgs { int m_bodyUniqueId; @@ -722,7 +712,6 @@ struct SharedMemoryCommand struct VRCameraState m_vrCameraStateArguments; struct StateLoggingRequest m_stateLoggingArguments; struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments; - struct RequestBodyNameArgs m_requestBodyNameArguments; }; }; @@ -785,7 +774,6 @@ 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 3778a73fa..6610164af 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -54,7 +54,6 @@ 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, @@ -134,8 +133,6 @@ 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 }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5ff38861b..9ea9f16da 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2238,69 +2238,6 @@ 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; @@ -5168,10 +5105,7 @@ 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, + {"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" " frame."}, From f062847038510b636aa140ff76ccd4b15b6a818d Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 29 Mar 2017 15:37:33 -0700 Subject: [PATCH 5/5] Replace spaces with tabs. --- .../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp | 2 +- examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h | 2 +- .../Importers/ImportURDFDemo/BulletUrdfImporter.cpp | 2 +- examples/Importers/ImportURDFDemo/BulletUrdfImporter.h | 2 +- .../Importers/ImportURDFDemo/URDFImporterInterface.h | 2 +- examples/SharedMemory/PhysicsClientSharedMemory.cpp | 8 ++++---- examples/SharedMemory/PhysicsDirect.cpp | 6 +++--- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 10 +++++----- examples/SharedMemory/SharedMemoryCommands.h | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 2 +- examples/pybullet/pybullet.c | 6 +++--- 11 files changed, 22 insertions(+), 22 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 59f33a398..40a3616b7 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -1325,7 +1325,7 @@ int BulletMJCFImporter::getRootLinkIndex() const std::string BulletMJCFImporter::getBodyName() const { - return m_data->m_fileModelName; + return m_data->m_fileModelName; } bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index f1061dd57..306d311ca 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -42,7 +42,7 @@ 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; + 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; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 4ade85564..37a040985 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -272,7 +272,7 @@ std::string BulletURDFImporter::getLinkName(int linkIndex) const std::string BulletURDFImporter::getBodyName() const { - return m_data->m_urdfParser.getModel().m_name; + return m_data->m_urdfParser.getModel().m_name; } std::string BulletURDFImporter::getJointName(int linkIndex) const diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 20d414f8d..76d34c7c7 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -35,7 +35,7 @@ public: virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; - virtual std::string getBodyName() const; + virtual std::string getBodyName() const; virtual std::string getLinkName(int linkIndex) const; diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 655eded88..7cfa62323 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -28,7 +28,7 @@ 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; + 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;} diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 3ead5fcee..fba7ab366 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -20,7 +20,7 @@ struct BodyJointInfoCache { std::string m_baseName; btAlignedObjectArray m_jointInfo; - std::string m_bodyName; + std::string m_bodyName; }; struct PhysicsClientSharedMemoryInternalData { @@ -107,7 +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(); + info.m_bodyName = bodyJoints->m_bodyName.c_str(); return true; } @@ -308,7 +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; + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_multiBodies.size(); i++) { @@ -421,7 +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; + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_multiBodies.size(); i++) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 40be6dbcb..cb2c62e68 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -17,7 +17,7 @@ struct BodyJointInfoCache2 { std::string m_baseName; btAlignedObjectArray m_jointInfo; - std::string m_bodyName; + std::string m_bodyName; }; @@ -606,7 +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; + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_multiBodies.size(); i++) { @@ -933,7 +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(); + info.m_bodyName = bodyJoints->m_bodyName.c_str(); return true; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e47be7f85..be39dbf59 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -120,7 +120,7 @@ struct InteralBodyData btMultiBody* m_multiBody; btRigidBody* m_rigidBody; int m_testData; - std::string m_bodyName; + std::string m_bodyName; btTransform m_rootLocalInertialFrame; btAlignedObjectArray m_linkLocalInertialFrames; @@ -1608,8 +1608,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto btMultiBody* mb = creation.getBulletMultiBody(); btRigidBody* rb = creation.getRigidBody(); - - bodyHandle->m_bodyName = u2b.getBodyName(); + + bodyHandle->m_bodyName = u2b.getBodyName(); if (useMultiBody) { @@ -2653,8 +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()); + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); hasStatus = true; } else diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4a7bbb0af..f5038c6a4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -103,7 +103,7 @@ struct BulletDataStreamArgs { char m_bulletFileName[MAX_FILENAME_LENGTH]; int m_bodyUniqueId; - char m_bodyName[MAX_FILENAME_LENGTH]; + char m_bodyName[MAX_FILENAME_LENGTH]; }; struct SetJointFeedbackArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index bd3280ca8..7a201aee9 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -201,7 +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 + 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/pybullet.c b/examples/pybullet/pybullet.c index a46c401f3..7c6d22861 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1716,7 +1716,7 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject* { PyObject* pyListJointInfo = PyTuple_New(2); PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); - PyTuple_SetItem(pyListJointInfo, 1, PyString_FromString(info.m_bodyName)); + PyTuple_SetItem(pyListJointInfo, 1, PyString_FromString(info.m_bodyName)); return pyListJointInfo; } } @@ -5146,8 +5146,8 @@ 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, + + {"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" " frame."},