From e7bafbc71cf88b9fa689c54642064a6758f33dd8 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 29 Jun 2015 21:30:44 -0700 Subject: [PATCH] make Bullet URDF parser more similar to the ROS URDF parser. There is still a difference in ordering of links, due to the use of different hash-map implementations, with a difference in iterator order (btHashMap versus std::hashmap) --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 1 + .../ImportURDFDemo/BulletUrdfImporter.cpp | 2 +- .../ImportURDFDemo/ImportURDFSetup.cpp | 30 ++++++++++++++----- .../ImportURDFDemo/ROSURDFImporter.h | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 16 ++++++++++ .../ImportURDFDemo/URDFImporterInterface.h | 4 +++ .../Importers/ImportURDFDemo/UrdfParser.cpp | 8 ++++- 7 files changed, 52 insertions(+), 11 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 6b682e0fd..948bfac47 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -607,6 +607,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) ///most OpenCL drivers and OpenCL compilers have issues with our kernels. ///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers ///you could give it a try + ///Note that several old OpenCL physics examples still have to be ported over to this new Example Browser if (args.CheckCmdLineFlag("enable_experimental_opencl")) { gAllExamples->initOpenCLExampleEntries(); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 0c8d78bd4..ea2240150 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -177,7 +177,7 @@ std::string BulletURDFImporter::getLinkName(int linkIndex) const if (linkPtr) { UrdfLink* link = *linkPtr; - std::string n = link->m_name; + return link->m_name; } return ""; } diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 132a8607e..ce31fc8ba 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -10,7 +10,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "../CommonInterfaces/CommonParameterInterface.h" #include "ROSURDFImporter.h" -//#include "BulletURDFImporter.h" +#include "BulletURDFImporter.h" #include "URDF2Bullet.h" @@ -195,15 +195,28 @@ void ImportUrdfSetup::initPhysics() //now print the tree using the new interface - - //BulletURDFImporter u2b(m_guiHelper); - ROSURDFImporter u2b(m_guiHelper); + URDFImporterInterface* bla=0; + static bool newURDF = false; + newURDF = !newURDF; + if (newURDF) + { + b3Printf("using new URDF\n"); + bla = new BulletURDFImporter(m_guiHelper); + } else + { + b3Printf("using ROS URDF\n"); + bla = new ROSURDFImporter(m_guiHelper); + } + + URDFImporterInterface& u2b = *bla; bool loadOk = u2b.loadURDF(m_fileName); if (loadOk) { - u2b.printTree(); + printTree(u2b,u2b.getRootLinkIndex()); + + //u2b.printTree(); btTransform identityTrans; identityTrans.setIdentity(); @@ -227,8 +240,8 @@ void ImportUrdfSetup::initPhysics() { //create motors for each btMultiBody joint - - for (int i=0;igetNumLinks();i++) + int numLinks = mb->getNumLinks(); + for (int i=0;igetLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute @@ -263,7 +276,8 @@ void ImportUrdfSetup::initPhysics() if (1) { //create motors for each generic joint - for (int i=0;igetUserConstraintPtr()) diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h index 752ae5912..75c6c0256 100644 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h @@ -18,7 +18,7 @@ public: virtual bool loadURDF(const char* fileName); - const char* getPathPrefix(); + virtual const char* getPathPrefix(); void printTree(); //for debugging diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 7b05fc7b3..0fcc3d1a9 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -35,6 +35,20 @@ static btVector3 selectColor2() void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel) { + btScalar mass; + btVector3 localInertia; + btTransform inertialFrame; + u2b.getMassAndInertia(linkIndex,mass,localInertia,inertialFrame); + std::string name = u2b.getLinkName(linkIndex); + for(int j=0;j childIndices; u2b.getLinkChildIndices(linkIndex,childIndices); @@ -46,6 +60,8 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL { int childLinkIndex = childIndices[i]; std::string name = u2b.getLinkName(childLinkIndex); + + for(int j=0;j=0 for the root link index, -1 if there is no root link virtual int getRootLinkIndex() const = 0; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 70d1ea4c7..f3feb0722 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -377,7 +377,13 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo } } else { - logger->reportWarning("No inertial data for link"); + logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame"); + link.m_inertia.m_mass = 1.f; + link.m_inertia.m_linkLocalFrame.setIdentity(); + link.m_inertia.m_ixx = 1.f; + link.m_inertia.m_iyy = 1.f; + link.m_inertia.m_izz= 1.f; + logger->reportWarning(link.m_name.c_str()); }