From de3f91b64ed899ce8173940dca5beef9448a5e1b Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 29 Mar 2017 14:56:05 -0700 Subject: [PATCH] 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; } }