From 477342cfa3f20c752b49f34ef2c343c7439383ae Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 16 Aug 2019 01:41:55 -0700 Subject: [PATCH 1/3] Add option for parsing root joint type without world link --- dart/dynamics/SharedLibraryIkFast.cpp | 4 +- dart/utils/urdf/DartLoader.cpp | 139 +++++++++++++++----------- dart/utils/urdf/DartLoader.hpp | 28 ++++-- unittests/unit/test_DartLoader.cpp | 46 ++++++++- 4 files changed, 146 insertions(+), 71 deletions(-) diff --git a/dart/dynamics/SharedLibraryIkFast.cpp b/dart/dynamics/SharedLibraryIkFast.cpp index 3b4d1b1310438..a11faee2233f2 100644 --- a/dart/dynamics/SharedLibraryIkFast.cpp +++ b/dart/dynamics/SharedLibraryIkFast.cpp @@ -56,8 +56,8 @@ bool loadFunction( if (!symbol) { - dterr << "Failed to load the symbol '" << symbolName - << "' from the file '" << fileName << "'.\n"; + dterr << "Failed to load the symbol '" << symbolName << "' from the file '" + << fileName << "'.\n"; return false; } diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index e6dde1b4915e5..16faa97ec1bfc 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -80,7 +80,8 @@ void DartLoader::addPackageDirectory( dynamics::SkeletonPtr DartLoader::parseSkeleton( const common::Uri& _uri, - const common::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever, + unsigned int flags) { const common::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); @@ -98,13 +99,15 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton( return nullptr; } - return modelInterfaceToSkeleton(urdfInterface.get(), _uri, resourceRetriever); + return modelInterfaceToSkeleton( + urdfInterface.get(), _uri, resourceRetriever, flags); } dynamics::SkeletonPtr DartLoader::parseSkeletonString( const std::string& _urdfString, const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever, + unsigned int flags) { if (_urdfString.empty()) { @@ -121,12 +124,16 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( } return modelInterfaceToSkeleton( - urdfInterface.get(), _baseUri, getResourceRetriever(_resourceRetriever)); + urdfInterface.get(), + _baseUri, + getResourceRetriever(_resourceRetriever), + flags); } simulation::WorldPtr DartLoader::parseWorld( const common::Uri& _uri, - const common::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever, + unsigned int flags) { const common::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); @@ -135,13 +142,14 @@ simulation::WorldPtr DartLoader::parseWorld( if (!readFileToString(resourceRetriever, _uri, content)) return nullptr; - return parseWorldString(content, _uri, _resourceRetriever); + return parseWorldString(content, _uri, _resourceRetriever, flags); } simulation::WorldPtr DartLoader::parseWorldString( const std::string& _urdfString, const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever, + unsigned int flags) { const common::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); @@ -168,7 +176,7 @@ simulation::WorldPtr DartLoader::parseWorldString( { const urdf_parsing::Entity& entity = worldInterface->models[i]; dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton( - entity.model.get(), entity.uri, resourceRetriever); + entity.model.get(), entity.uri, resourceRetriever, flags); if (!skeleton) { @@ -198,16 +206,16 @@ simulation::WorldPtr DartLoader::parseWorldString( dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( const urdf::ModelInterface* model, const common::Uri& baseUri, - const common::ResourceRetrieverPtr& resourceRetriever) + const common::ResourceRetrieverPtr& resourceRetriever, + unsigned int flags) { dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(model->getName()); - dynamics::BodyNode* rootNode = nullptr; const urdf::Link* root = model->getRoot().get(); - // If the link name is "world" then the link is regarded as the inertial frame - // rather than a body in the robot model so we don't create a BodyNode for it. - // This is not officially specified in the URDF spec, but "world" is + // If the link name is "world" then the link is regarded as the inertial + // frame rather than a body in the robot model so we don't create a BodyNode + // for it. This is not officially specified in the URDF spec, but "world" is // practically treated as a keyword. if (root->name == "world") { @@ -219,45 +227,26 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( << "not the URDF standard. Please consider changing the robot " << "model as a single tree robot.\n"; } - } - else - { - // If the root link is not "world" then it means the robot model is a free - // floating robot (like humanoid robots). So we create the root body with - // FreeJoint. - dynamics::BodyNode::Properties rootProperties; - if (!createDartNodeProperties( - root, rootProperties, baseUri, resourceRetriever)) + for (std::size_t i = 0; i < root->child_links.size(); i++) { - return nullptr; + if (!createSkeletonRecursive( + model, + skeleton, + root->child_links[i].get(), + nullptr, + baseUri, + resourceRetriever, + flags)) + { + return nullptr; + } } - - std::pair pair - = skeleton->createJointAndBodyNodePair( - nullptr, - dynamics::FreeJoint::Properties( - dynamics::GenericJoint::Properties( - dynamics::Joint::Properties("rootJoint"))), - rootProperties); - rootNode = pair.second; - - const auto result - = createShapeNodes(model, root, rootNode, baseUri, resourceRetriever); - - if (!result) - return nullptr; } - - for (std::size_t i = 0; i < root->child_links.size(); i++) + else { if (!createSkeletonRecursive( - model, - skeleton, - root->child_links[i].get(), - rootNode, - baseUri, - resourceRetriever)) + model, skeleton, root, nullptr, baseUri, resourceRetriever, flags)) { return nullptr; } @@ -277,19 +266,23 @@ bool DartLoader::createSkeletonRecursive( const urdf::Link* lk, dynamics::BodyNode* parentNode, const common::Uri& baseUri, - const common::ResourceRetrieverPtr& resourceRetriever) + const common::ResourceRetrieverPtr& resourceRetriever, + unsigned int flags) { + assert(lk); + + if (parentNode != nullptr && lk->name == "world") + { + dtwarn << "[DartLoader] Link name 'world' is reserved for the inertial " + << "frame. Consider changing the name to something else.\n"; + } + dynamics::BodyNode::Properties properties; if (!createDartNodeProperties(lk, properties, baseUri, resourceRetriever)) return false; dynamics::BodyNode* node = createDartJointAndNode( - lk->parent_joint.get(), - properties, - parentNode, - skel, - baseUri, - resourceRetriever); + lk->parent_joint.get(), properties, parentNode, skel, flags); if (!node) return false; @@ -308,7 +301,8 @@ bool DartLoader::createSkeletonRecursive( lk->child_links[i].get(), node, baseUri, - resourceRetriever)) + resourceRetriever, + flags)) { return false; } @@ -382,6 +376,32 @@ bool DartLoader::readFileToString( return true; } +dynamics::BodyNode* createDartJointAndNodeForRoot( + const dynamics::BodyNode::Properties& _body, + dynamics::BodyNode* _parent, + dynamics::SkeletonPtr _skeleton, + unsigned int flags) +{ + dynamics::Joint::Properties basicProperties; + + dynamics::GenericJoint::UniqueProperties singleDof; + std::pair pair; + if (flags & DartLoader::Flags::FIXED_BASE_LINK) + { + pair = _skeleton->createJointAndBodyNodePair( + _parent, basicProperties, _body); + } + else + { + dynamics::GenericJoint::Properties properties( + basicProperties); + pair = _skeleton->createJointAndBodyNodePair( + _parent, properties, _body); + } + + return pair.second; +} + /** * @function createDartJoint */ @@ -390,9 +410,16 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, - const common::Uri& /*_baseUri*/, - const common::ResourceRetrieverPtr& /*_resourceRetriever*/) + unsigned int flags) { + // Special case for the root link (A root link doesn't have the parent joint). + // We don't have sufficient information what the joint type should be for the + // root link. So we create the root joint by the specified flgas. + if (!_jt) + { + return createDartJointAndNodeForRoot(_body, _parent, _skeleton, flags); + } + dynamics::Joint::Properties basicProperties; basicProperties.mName = _jt->name; diff --git a/dart/utils/urdf/DartLoader.hpp b/dart/utils/urdf/DartLoader.hpp index 349b638abbfbc..625b8b3935cae 100644 --- a/dart/utils/urdf/DartLoader.hpp +++ b/dart/utils/urdf/DartLoader.hpp @@ -76,6 +76,13 @@ namespace utils { class DartLoader { public: + enum Flags + { + NONE = 0, + FIXED_BASE_LINK = 1 << 1, + DEFAULT = NONE, + }; + /// Constructor with the default ResourceRetriever. DartLoader(); @@ -104,24 +111,28 @@ class DartLoader /// Parse a file to produce a Skeleton dynamics::SkeletonPtr parseSkeleton( const common::Uri& _uri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + unsigned int flags = DEFAULT); /// Parse a text string to produce a Skeleton dynamics::SkeletonPtr parseSkeletonString( const std::string& _urdfString, const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + unsigned int flags = DEFAULT); /// Parse a file to produce a World dart::simulation::WorldPtr parseWorld( const common::Uri& _uri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + unsigned int flags = DEFAULT); /// Parse a text string to produce a World dart::simulation::WorldPtr parseWorldString( const std::string& _urdfString, const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + unsigned int flags = DEFAULT); private: typedef std::shared_ptr BodyPropPtr; @@ -131,7 +142,8 @@ class DartLoader static dart::dynamics::SkeletonPtr modelInterfaceToSkeleton( const urdf::ModelInterface* model, const common::Uri& baseUri, - const common::ResourceRetrieverPtr& resourceRetriever); + const common::ResourceRetrieverPtr& resourceRetriever, + unsigned int flags); static bool createSkeletonRecursive( const urdf::ModelInterface* model, @@ -139,7 +151,8 @@ class DartLoader const urdf::Link* lk, dynamics::BodyNode* parent, const common::Uri& baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever); + const common::ResourceRetrieverPtr& _resourceRetriever, + unsigned int flags); static bool addMimicJointsRecursive( const urdf::ModelInterface* model, @@ -157,8 +170,7 @@ class DartLoader const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, - const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever); + unsigned int flgas); static bool createDartNodeProperties( const urdf::Link* _lk, diff --git a/unittests/unit/test_DartLoader.cpp b/unittests/unit/test_DartLoader.cpp index 6e3fbe0bbd5bd..e960bf47e2218 100644 --- a/unittests/unit/test_DartLoader.cpp +++ b/unittests/unit/test_DartLoader.cpp @@ -34,6 +34,7 @@ #include #include "dart/dynamics/FreeJoint.hpp" #include "dart/dynamics/MeshShape.hpp" +#include "dart/dynamics/WeldJoint.hpp" #include "dart/utils/urdf/DartLoader.hpp" using namespace dart; @@ -160,10 +161,10 @@ TEST(DartLoader, parseJointProperties) DartLoader loader; auto robot = loader.parseSkeletonString(urdfStr, ""); - EXPECT_TRUE(nullptr != robot); + ASSERT_TRUE(nullptr != robot); auto joint1 = robot->getJoint(1); - EXPECT_TRUE(nullptr != joint1); + ASSERT_TRUE(nullptr != joint1); EXPECT_NEAR(joint1->getDampingCoefficient(0), 1.2, 1e-12); EXPECT_NEAR(joint1->getCoulombFriction(0), 2.3, 1e-12); @@ -175,6 +176,41 @@ TEST(DartLoader, parseJointProperties) EXPECT_TRUE(joint2->isCyclic(0)); } +TEST(DartLoader, parseUrdfWithoutWorldLink) +{ + const std::string urdfStr + = " " + " " + " " + " " + " " + " " + " " + " " + " " + " "; + + DartLoader loader; + + auto robot1 = loader.parseSkeletonString(urdfStr, "", nullptr); + ASSERT_TRUE(nullptr != robot1); + EXPECT_EQ( + robot1->getRootJoint()->getType(), dynamics::FreeJoint::getStaticType()); + + auto robot2 = loader.parseSkeletonString( + urdfStr, "", nullptr, DartLoader::Flags::NONE); + ASSERT_TRUE(nullptr != robot2); + EXPECT_EQ( + robot2->getRootJoint()->getType(), dynamics::FreeJoint::getStaticType()); + + auto robot3 = loader.parseSkeletonString( + urdfStr, "", nullptr, DartLoader::Flags::FIXED_BASE_LINK); + ASSERT_TRUE(nullptr != robot3); + EXPECT_EQ( + robot3->getRootJoint()->getType(), dynamics::WeldJoint::getStaticType()); +} + TEST(DartLoader, mimicJoint) { std::string urdfStr @@ -383,9 +419,9 @@ TEST(DartLoader, SingleLinkWithoutJoint) EXPECT_TRUE(robot->getNumBodyNodes() == 1u); EXPECT_TRUE(robot->getRootBodyNode()->getName() == "link_0"); - EXPECT_TRUE( - robot->getRootJoint()->getType() - == dart::dynamics::FreeJoint::getStaticType()); + EXPECT_EQ( + robot->getRootJoint()->getType(), + dart::dynamics::FreeJoint::getStaticType()); } TEST(DartLoader, MultiTreeRobot) From 3cfa2b07850e07f7f749cb4bfc2a2398ef1a0c52 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 16 Aug 2019 01:47:59 -0700 Subject: [PATCH 2/3] Add comment on the flags --- dart/utils/urdf/DartLoader.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/dart/utils/urdf/DartLoader.hpp b/dart/utils/urdf/DartLoader.hpp index 625b8b3935cae..ae420f4563ff8 100644 --- a/dart/utils/urdf/DartLoader.hpp +++ b/dart/utils/urdf/DartLoader.hpp @@ -76,10 +76,16 @@ namespace utils { class DartLoader { public: + /// Flags for specifying URDF file parsing policies. enum Flags { NONE = 0, + + /// Parser the root link's joint type to be "fixed" joint when not + /// specified. FIXED_BASE_LINK = 1 << 1, + + /// The default flgas DEFAULT = NONE, }; From 0b13b0e8d3521075396a475e3fc3ea9a44accac7 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 16 Aug 2019 20:55:38 -0700 Subject: [PATCH 3/3] Update changelog --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 09e95c209dbeb..2469a9194cbe3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,7 +4,7 @@ * Dynamics - * Fixex friction and restitution of individual shapes in a body: [#1369](https://github.com/dartsim/dart/pull/1369) + * Fixed friction and restitution of individual shapes in a body: [#1369](https://github.com/dartsim/dart/pull/1369) * Fixed soft body simulation when command input is not resetted: [#1372](https://github.com/dartsim/dart/pull/1372) * GUI @@ -18,6 +18,7 @@ * Allowed parsing SDF up to version 1.6: [#1385](https://github.com/dartsim/dart/pull/1385) * Fixed SDF parser not creating dynamics aspect for collision shape: [#1386](https://github.com/dartsim/dart/pull/1386) + * Added root joint parsing option in URDF parser: [#1399](https://github.com/dartsim/dart/pull/1399) * dartpy