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/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 123ceea68..40a3616b7 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/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); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 66e9a2424..306d311ca 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 8407c6191..37a040985 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 c61bf77a2..76d34c7c7 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 c8f996bf5..7cfa62323 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/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7615782ba..b0695cd20 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index a0e18235e..fba7ab366 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++) { @@ -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++) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 5ca0f35a2..cb2c62e68 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 2f5aa52bf..be39dbf59 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) { @@ -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"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 096349762..f5038c6a4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 21c8b685e..7a201aee9 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 }; diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index 309449138..aad32f489 100644 --- a/examples/pybullet/examples/kuka_with_cube.py +++ b/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): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0f4eef675..7c6d22861 100644 --- a/examples/pybullet/pybullet.c +++ b/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"