From 50f0cfca9e84aa56e33c580acf41cf0ae511dd99 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 23 Mar 2017 10:16:39 -0700 Subject: [PATCH] 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);