diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 0e8b0c451..18433a900 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -93,7 +93,7 @@ struct BulletErrorLogger : public ErrorLogger } }; -bool BulletURDFImporter::loadURDF(const char* fileName) +bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) { @@ -129,7 +129,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName) } BulletErrorLogger loggie; - bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(),&loggie); + bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase); return result; } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index d2493db89..7f712e1cd 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -16,7 +16,7 @@ public: virtual ~BulletURDFImporter(); - virtual bool loadURDF(const char* fileName); + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); const char* getPathPrefix(); diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp index 3c4a6daa4..6b8fcc545 100644 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp @@ -95,7 +95,7 @@ ROSURDFImporter::ROSURDFImporter(struct GUIHelperInterface* helper) } -bool ROSURDFImporter::loadURDF(const char* fileName) +bool ROSURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) { diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h index 75c6c0256..93e7bb101 100644 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h @@ -16,7 +16,8 @@ public: virtual ~ROSURDFImporter(); - virtual bool loadURDF(const char* fileName); + // Note that forceFixedBase is ignored in this implementation. + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); virtual const char* getPathPrefix(); diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 82ddc0613..622486aa4 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -15,7 +15,7 @@ public: virtual ~URDFImporterInterface() {} - virtual bool loadURDF(const char* fileName)=0; + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0; virtual const char* getPathPrefix()=0; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 03563b719..2e42525d5 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -715,7 +715,8 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger) return true; } -bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger) + +bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase) { TiXmlDocument xml_doc; @@ -840,8 +841,26 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger) } } + bool ok(initTreeAndRoot(logger)); + if (!ok) + { + return false; + } - - return initTreeAndRoot(logger); + if (forceFixedBase) + { + for (int i=0;im_inertia.m_mass = 0.0; + link->m_inertia.m_ixx = 0.0; + link->m_inertia.m_ixy = 0.0; + link->m_inertia.m_ixz = 0.0; + link->m_inertia.m_iyy = 0.0; + link->m_inertia.m_iyz = 0.0; + link->m_inertia.m_izz = 0.0; + } + } + return true; } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 55bf0aa99..cc0b7887a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -150,7 +150,7 @@ public: UrdfParser(); virtual ~UrdfParser(); - bool loadUrdf(const char* urdfText, ErrorLogger* logger); + bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase); const UrdfModel& getModel() const {