From 63b6a74f03e25c47ae74a4067b5cc136c19a7049 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 18 Sep 2021 13:00:38 -0700 Subject: [PATCH 1/2] Add options to DartLoader --- dart/utils/urdf/DartLoader.cpp | 198 ++++++++++++++++++++------ dart/utils/urdf/DartLoader.hpp | 92 +++++++++--- python/dartpy/utils/DartLoader.cpp | 218 +++++++++++++++++------------ unittests/unit/test_DartLoader.cpp | 55 +++++++- 4 files changed, 415 insertions(+), 148 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index ea4785aaeea94..11ae87e7ad2ab 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -62,8 +62,21 @@ namespace utils { using ModelInterfacePtr = urdf_shared_ptr; -DartLoader::DartLoader() - : mLocalRetriever(new common::LocalResourceRetriever), +DartLoader::Options::Options( + common::ResourceRetrieverPtr resourceRetriever, + RootJointType defaultRootJointType, + const dynamics::Inertia& defaultInertia) + : mResourceRetriever(std::move(resourceRetriever)), + mDefaultRootJointType(defaultRootJointType), + mDefaultInertia(defaultInertia) +{ + // Do nothing +} + +//============================================================================== +DartLoader::DartLoader(const Options& options) + : mOptions(options), + mLocalRetriever(new common::LocalResourceRetriever), mPackageRetriever(new utils::PackageResourceRetriever(mLocalRetriever)), mRetriever(new utils::CompositeResourceRetriever) { @@ -72,22 +85,52 @@ DartLoader::DartLoader() mRetriever->addSchemaRetriever("dart", DartResourceRetriever::create()); } +//============================================================================== +void DartLoader::setOptions(const Options& options) +{ + mOptions = options; +} + +//============================================================================== +const DartLoader::Options& DartLoader::getOptions() const +{ + return mOptions; +} + +//============================================================================== void DartLoader::addPackageDirectory( const std::string& _packageName, const std::string& _packageDirectory) { mPackageRetriever->addPackageDirectory(_packageName, _packageDirectory); } +//============================================================================== dynamics::SkeletonPtr DartLoader::parseSkeleton( const common::Uri& _uri, const common::ResourceRetrieverPtr& _resourceRetriever, unsigned int flags) +{ + const auto oldOptions = mOptions; + auto options = mOptions; + options.mResourceRetriever = _resourceRetriever; + if (flags & DartLoader::Flags::FIXED_BASE_LINK) + options.mDefaultRootJointType = RootJointType::FIXED; + else + options.mDefaultRootJointType = RootJointType::FLOATING; + mOptions = options; + auto out = parseSkeleton(_uri); + mOptions = oldOptions; + return out; +} + +//============================================================================== +dynamics::SkeletonPtr DartLoader::parseSkeleton(const common::Uri& uri) { const common::ResourceRetrieverPtr resourceRetriever - = getResourceRetriever(_resourceRetriever); + = getResourceRetriever(mOptions.mResourceRetriever); std::string content; - if (!readFileToString(resourceRetriever, _uri, content)) + if (!readFileToString(resourceRetriever, uri, content)) return nullptr; // Use urdfdom to load the URDF file. @@ -95,28 +138,46 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton( if (!urdfInterface) { dtwarn << "[DartLoader::readSkeleton] Failed loading URDF file '" - << _uri.toString() << "'.\n"; + << uri.toString() << "'.\n"; return nullptr; } return modelInterfaceToSkeleton( - urdfInterface.get(), _uri, resourceRetriever, flags); + urdfInterface.get(), uri, resourceRetriever, mOptions); } +//============================================================================== dynamics::SkeletonPtr DartLoader::parseSkeletonString( const std::string& _urdfString, const common::Uri& _baseUri, const common::ResourceRetrieverPtr& _resourceRetriever, unsigned int flags) { - if (_urdfString.empty()) + const auto oldOptions = mOptions; + auto options = mOptions; + options.mResourceRetriever = _resourceRetriever; + if (flags & DartLoader::Flags::FIXED_BASE_LINK) + options.mDefaultRootJointType = RootJointType::FIXED; + else + options.mDefaultRootJointType = RootJointType::FLOATING; + mOptions = options; + auto out = parseSkeletonString(_urdfString, _baseUri); + mOptions = oldOptions; + return out; +} + +//============================================================================== +dynamics::SkeletonPtr DartLoader::parseSkeletonString( + const std::string& urdfString, const common::Uri& baseUri) +{ + if (urdfString.empty()) { dtwarn << "[DartLoader::parseSkeletonString] A blank string cannot be " << "parsed into a Skeleton. Returning a nullptr\n"; return nullptr; } - ModelInterfacePtr urdfInterface = urdf::parseURDF(_urdfString); + ModelInterfacePtr urdfInterface = urdf::parseURDF(urdfString); if (!urdfInterface) { dtwarn << "[DartLoader::parseSkeletonString] Failed loading URDF.\n"; @@ -125,36 +186,71 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( return modelInterfaceToSkeleton( urdfInterface.get(), - _baseUri, - getResourceRetriever(_resourceRetriever), - flags); + baseUri, + getResourceRetriever(mOptions.mResourceRetriever), + mOptions); } +//============================================================================== simulation::WorldPtr DartLoader::parseWorld( const common::Uri& _uri, const common::ResourceRetrieverPtr& _resourceRetriever, unsigned int flags) +{ + const auto oldOptions = mOptions; + auto options = mOptions; + options.mResourceRetriever = _resourceRetriever; + if (flags & DartLoader::Flags::FIXED_BASE_LINK) + options.mDefaultRootJointType = RootJointType::FIXED; + else + options.mDefaultRootJointType = RootJointType::FLOATING; + mOptions = options; + auto out = parseWorld(_uri); + mOptions = oldOptions; + return out; +} + +//============================================================================== +simulation::WorldPtr DartLoader::parseWorld(const common::Uri& uri) { const common::ResourceRetrieverPtr resourceRetriever - = getResourceRetriever(_resourceRetriever); + = getResourceRetriever(mOptions.mResourceRetriever); std::string content; - if (!readFileToString(resourceRetriever, _uri, content)) + if (!readFileToString(resourceRetriever, uri, content)) return nullptr; - return parseWorldString(content, _uri, _resourceRetriever, flags); + return parseWorldString(content, uri); } +//============================================================================== simulation::WorldPtr DartLoader::parseWorldString( const std::string& _urdfString, const common::Uri& _baseUri, const common::ResourceRetrieverPtr& _resourceRetriever, unsigned int flags) +{ + const auto oldOptions = mOptions; + auto options = mOptions; + options.mResourceRetriever = _resourceRetriever; + if (flags & DartLoader::Flags::FIXED_BASE_LINK) + options.mDefaultRootJointType = RootJointType::FIXED; + else + options.mDefaultRootJointType = RootJointType::FLOATING; + mOptions = options; + auto out = parseWorldString(_urdfString, _baseUri); + mOptions = oldOptions; + return out; +} + +//============================================================================== +simulation::WorldPtr DartLoader::parseWorldString( + const std::string& urdfString, const common::Uri& baseUri) { const common::ResourceRetrieverPtr resourceRetriever - = getResourceRetriever(_resourceRetriever); + = getResourceRetriever(mOptions.mResourceRetriever); - if (_urdfString.empty()) + if (urdfString.empty()) { dtwarn << "[DartLoader::parseWorldString] A blank string cannot be " << "parsed into a World. Returning a nullptr\n"; @@ -162,7 +258,7 @@ simulation::WorldPtr DartLoader::parseWorldString( } std::shared_ptr worldInterface - = urdf_parsing::parseWorldURDF(_urdfString, _baseUri, resourceRetriever); + = urdf_parsing::parseWorldURDF(urdfString, baseUri, resourceRetriever); if (!worldInterface) { @@ -176,7 +272,7 @@ simulation::WorldPtr DartLoader::parseWorldString( { const urdf_parsing::Entity& entity = worldInterface->models[i]; dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton( - entity.model.get(), entity.uri, resourceRetriever, flags); + entity.model.get(), entity.uri, resourceRetriever, mOptions); if (!skeleton) { @@ -207,7 +303,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( const urdf::ModelInterface* model, const common::Uri& baseUri, const common::ResourceRetrieverPtr& resourceRetriever, - unsigned int flags) + const Options& options) { dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(model->getName()); @@ -237,7 +333,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( nullptr, baseUri, resourceRetriever, - flags)) + options)) { return nullptr; } @@ -246,7 +342,13 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( else { if (!createSkeletonRecursive( - model, skeleton, root, nullptr, baseUri, resourceRetriever, flags)) + model, + skeleton, + root, + nullptr, + baseUri, + resourceRetriever, + options)) { return nullptr; } @@ -267,7 +369,7 @@ bool DartLoader::createSkeletonRecursive( dynamics::BodyNode* parentNode, const common::Uri& baseUri, const common::ResourceRetrieverPtr& resourceRetriever, - unsigned int flags) + const Options& options) { assert(lk); @@ -278,11 +380,12 @@ bool DartLoader::createSkeletonRecursive( } dynamics::BodyNode::Properties properties; - if (!createDartNodeProperties(lk, properties, baseUri, resourceRetriever)) + if (!createDartNodeProperties( + lk, properties, baseUri, resourceRetriever, options)) return false; dynamics::BodyNode* node = createDartJointAndNode( - lk->parent_joint.get(), properties, parentNode, skel, flags); + lk->parent_joint.get(), properties, parentNode, skel, options); if (!node) return false; @@ -302,7 +405,7 @@ bool DartLoader::createSkeletonRecursive( node, baseUri, resourceRetriever, - flags)) + options)) { return false; } @@ -380,23 +483,35 @@ dynamics::BodyNode* createDartJointAndNodeForRoot( const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, - unsigned int flags) + const DartLoader::Options& options) { dynamics::Joint::Properties basicProperties("rootJoint"); dynamics::GenericJoint::UniqueProperties singleDof; std::pair pair; - if (flags & DartLoader::Flags::FIXED_BASE_LINK) + switch (options.mDefaultRootJointType) { - pair = _skeleton->createJointAndBodyNodePair( - _parent, basicProperties, _body); - } - else - { - dynamics::GenericJoint::Properties properties( - basicProperties); - pair = _skeleton->createJointAndBodyNodePair( - _parent, properties, _body); + case DartLoader::RootJointType::FLOATING: { + dynamics::GenericJoint::Properties properties( + basicProperties); + pair = _skeleton->createJointAndBodyNodePair( + _parent, properties, _body); + break; + } + case DartLoader::RootJointType::FIXED: { + pair = _skeleton->createJointAndBodyNodePair( + _parent, basicProperties, _body); + break; + } + default: { + dterr << "Unsupported RootJointType '" + << static_cast(options.mDefaultRootJointType) + << "'. Using FLOATING instead.\n"; + dynamics::GenericJoint::Properties properties( + basicProperties); + pair = _skeleton->createJointAndBodyNodePair( + _parent, properties, _body); + } } return pair.second; @@ -410,14 +525,14 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, - unsigned int flags) + const Options& options) { // 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); + return createDartJointAndNodeForRoot(_body, _parent, _skeleton, options); } dynamics::Joint::Properties basicProperties; @@ -545,7 +660,8 @@ bool DartLoader::createDartNodeProperties( const urdf::Link* _lk, dynamics::BodyNode::Properties& node, const common::Uri& /*_baseUri*/, - const common::ResourceRetrieverPtr& /*_resourceRetriever*/) + const common::ResourceRetrieverPtr& /*_resourceRetriever*/, + const Options& options) { node.mName = _lk->name; @@ -570,6 +686,10 @@ bool DartLoader::createDartNodeProperties( node.mInertia.setMoment( J(0, 0), J(1, 1), J(2, 2), J(0, 1), J(0, 2), J(1, 2)); } + else + { + node.mInertia = options.mDefaultInertia; + } return true; } diff --git a/dart/utils/urdf/DartLoader.hpp b/dart/utils/urdf/DartLoader.hpp index 931c0cfa70cfa..81df0581a8534 100644 --- a/dart/utils/urdf/DartLoader.hpp +++ b/dart/utils/urdf/DartLoader.hpp @@ -77,6 +77,8 @@ namespace utils { class DartLoader { public: + /// \deprecated Deprecated in 6.11. Use RootJointType and Options instead. + /// /// Flags for specifying URDF file parsing policies. enum Flags { @@ -90,8 +92,46 @@ class DartLoader DEFAULT = NONE, }; + /// Root joint type to be used when the parent joint of the root link is not + /// specified in the URDF file. + enum class RootJointType + { + /// Floating joint type of URDF. + FLOATING = 0, + + /// Fixed joint type of URDF. + FIXED = 1, + }; + + /// Options to be used in parsing URDF files. + struct Options + { + /// Resource retriever. LocalResourceRetriever is used if it's nullptr. + common::ResourceRetrieverPtr mResourceRetriever; + + /// Default root joint type to be used when the parent joint of the root + /// link is not specified in the URDF file. + RootJointType mDefaultRootJointType; + + /// Default inertia properties to be used when the inertial element is not + /// specified in the link element. + dynamics::Inertia mDefaultInertia; + + /// Default constructor + Options( + common::ResourceRetrieverPtr resourceRetriever = nullptr, + RootJointType defaultRootJointType = RootJointType::FLOATING, + const dynamics::Inertia& defaultInertia = dynamics::Inertia()); + }; + /// Constructor with the default ResourceRetriever. - DartLoader(); + explicit DartLoader(const Options& options = Options()); + + /// Sets options + void setOptions(const Options& options); + + /// Returns options + const Options& getOptions() const; /// Specify the directory of a ROS package. In your URDF files, you may see /// strings with a package URI pattern such as: @@ -113,34 +153,52 @@ class DartLoader /// specify as the package directory will end up replacing the 'package /// keyword' and 'package name' components of the URI string. void addPackageDirectory( - const std::string& _packageName, const std::string& _packageDirectory); + const std::string& packageName, const std::string& packageDirectory); /// Parse a file to produce a Skeleton + DART_DEPRECATED(6.11) dynamics::SkeletonPtr parseSkeleton( - const common::Uri& _uri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + const common::Uri& uri, + const common::ResourceRetrieverPtr& resourceRetriever, unsigned int flags = DEFAULT); + /// Parse a file to produce a Skeleton + dynamics::SkeletonPtr parseSkeleton(const common::Uri& uri); + /// Parse a text string to produce a Skeleton + DART_DEPRECATED(6.11) dynamics::SkeletonPtr parseSkeletonString( - const std::string& _urdfString, - const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + const std::string& urdfString, + const common::Uri& baseUri, + const common::ResourceRetrieverPtr& resourceRetriever, unsigned int flags = DEFAULT); + /// Parse a text string to produce a Skeleton + dynamics::SkeletonPtr parseSkeletonString( + const std::string& urdfString, const common::Uri& baseUri); + /// Parse a file to produce a World + DART_DEPRECATED(6.11) dart::simulation::WorldPtr parseWorld( - const common::Uri& _uri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + const common::Uri& uri, + const common::ResourceRetrieverPtr& resourceRetriever, unsigned int flags = DEFAULT); + /// Parse a file to produce a World + dart::simulation::WorldPtr parseWorld(const common::Uri& uri); + /// Parse a text string to produce a World + DART_DEPRECATED(6.11) dart::simulation::WorldPtr parseWorldString( - const std::string& _urdfString, - const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever = nullptr, + const std::string& urdfString, + const common::Uri& baseUri, + const common::ResourceRetrieverPtr& resourceRetriever, unsigned int flags = DEFAULT); + /// Parse a text string to produce a World + dart::simulation::WorldPtr parseWorldString( + const std::string& urdfString, const common::Uri& baseUri); + private: typedef std::shared_ptr BodyPropPtr; typedef std::shared_ptr JointPropPtr; @@ -150,7 +208,7 @@ class DartLoader const urdf::ModelInterface* model, const common::Uri& baseUri, const common::ResourceRetrieverPtr& resourceRetriever, - unsigned int flags); + const Options& options); static bool createSkeletonRecursive( const urdf::ModelInterface* model, @@ -159,7 +217,7 @@ class DartLoader dynamics::BodyNode* parent, const common::Uri& baseUri, const common::ResourceRetrieverPtr& _resourceRetriever, - unsigned int flags); + const Options& options); static bool addMimicJointsRecursive( const urdf::ModelInterface* model, @@ -177,13 +235,14 @@ class DartLoader const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, - unsigned int flgas); + const Options& options); static bool createDartNodeProperties( const urdf::Link* _lk, dynamics::BodyNode::Properties& properties, const common::Uri& _baseUri, - const common::ResourceRetrieverPtr& _resourceRetriever); + const common::ResourceRetrieverPtr& _resourceRetriever, + const Options& options); static bool createShapeNodes( const urdf::ModelInterface* model, @@ -203,6 +262,7 @@ class DartLoader const common::Uri& _uri, std::string& _output); + Options mOptions; common::LocalResourceRetrieverPtr mLocalRetriever; utils::PackageResourceRetrieverPtr mPackageRetriever; utils::CompositeResourceRetrieverPtr mRetriever; diff --git a/python/dartpy/utils/DartLoader.cpp b/python/dartpy/utils/DartLoader.cpp index 2a3658c75d3a7..24a4381092090 100644 --- a/python/dartpy/utils/DartLoader.cpp +++ b/python/dartpy/utils/DartLoader.cpp @@ -41,94 +41,136 @@ namespace python { void DartLoader(py::module& m) { - ::py::class_(m, "DartLoader") - .def(::py::init<>()) - .def( - "addPackageDirectory", - +[](dart::utils::DartLoader* self, - const std::string& _packageName, - const std::string& _packageDirectory) -> void { - return self->addPackageDirectory(_packageName, _packageDirectory); - }, - ::py::arg("packageName"), - ::py::arg("packageDirectory")) - .def( - "parseSkeleton", - +[](dart::utils::DartLoader* self, - const dart::common::Uri& _uri) -> dart::dynamics::SkeletonPtr { - return self->parseSkeleton(_uri); - }, - ::py::arg("uri")) - .def( - "parseSkeleton", - +[](dart::utils::DartLoader* self, - const dart::common::Uri& _uri, - const dart::common::ResourceRetrieverPtr& _resourceRetriever) - -> dart::dynamics::SkeletonPtr { - return self->parseSkeleton(_uri, _resourceRetriever); - }, - ::py::arg("uri"), - ::py::arg("resourceRetriever")) - .def( - "parseSkeletonString", - +[](dart::utils::DartLoader* self, - const std::string& _urdfString, - const dart::common::Uri& _baseUri) - -> dart::dynamics::SkeletonPtr { - return self->parseSkeletonString(_urdfString, _baseUri); - }, - ::py::arg("urdfString"), - ::py::arg("baseUri")) - .def( - "parseSkeletonString", - +[](dart::utils::DartLoader* self, - const std::string& _urdfString, - const dart::common::Uri& _baseUri, - const dart::common::ResourceRetrieverPtr& _resourceRetriever) - -> dart::dynamics::SkeletonPtr { - return self->parseSkeletonString( - _urdfString, _baseUri, _resourceRetriever); - }, - ::py::arg("urdfString"), - ::py::arg("baseUri"), - ::py::arg("resourceRetriever")) - .def( - "parseWorld", - +[](dart::utils::DartLoader* self, const dart::common::Uri& _uri) - -> dart::simulation::WorldPtr { return self->parseWorld(_uri); }, - ::py::arg("uri")) - .def( - "parseWorld", - +[](dart::utils::DartLoader* self, - const dart::common::Uri& _uri, - const dart::common::ResourceRetrieverPtr& _resourceRetriever) - -> dart::simulation::WorldPtr { - return self->parseWorld(_uri, _resourceRetriever); - }, - ::py::arg("uri"), - ::py::arg("resourceRetriever")) - .def( - "parseWorldString", - +[](dart::utils::DartLoader* self, - const std::string& _urdfString, - const dart::common::Uri& _baseUri) -> dart::simulation::WorldPtr { - return self->parseWorldString(_urdfString, _baseUri); - }, - ::py::arg("urdfString"), - ::py::arg("baseUri")) - .def( - "parseWorldString", - +[](dart::utils::DartLoader* self, - const std::string& _urdfString, - const dart::common::Uri& _baseUri, - const dart::common::ResourceRetrieverPtr& _resourceRetriever) - -> dart::simulation::WorldPtr { - return self->parseWorldString( - _urdfString, _baseUri, _resourceRetriever); - }, - ::py::arg("urdfString"), - ::py::arg("baseUri"), - ::py::arg("resourceRetriever")); + auto dartLoaderFlags + = ::py::enum_(m, "DartLoaderFlags") + .value("NONE", utils::DartLoader::Flags::NONE) + .value("FIXED_BASE_LINK", utils::DartLoader::Flags::FIXED_BASE_LINK) + .value("DEFAULT", utils::DartLoader::Flags::DEFAULT); + + auto dartLoaderRootJointType + = ::py::enum_( + m, "DartLoaderRootJointType") + .value("FLOATING", utils::DartLoader::RootJointType::FLOATING) + .value("FIXED", utils::DartLoader::RootJointType::FIXED); + + auto dartLoaderOptions + = ::py::class_(m, "DartLoaderOptions") + .def( + ::py::init< + common::ResourceRetrieverPtr, + utils::DartLoader::RootJointType, + const dynamics::Inertia&>(), + ::py::arg("resourceRetriever") = nullptr, + ::py::arg("defaultRootJointType") + = utils::DartLoader::RootJointType::FLOATING, + ::py::arg("defaultInertia") = dynamics::Inertia()) + .def_readwrite( + "mResourceRetriever", + &utils::DartLoader::Options::mResourceRetriever) + .def_readwrite( + "mDefaultRootJointType", + &utils::DartLoader::Options::mDefaultRootJointType) + .def_readwrite( + "mDefaultInertia", + &utils::DartLoader::Options::mDefaultInertia); + + auto dartLoader + = ::py::class_(m, "DartLoader") + .def(::py::init<>()) + .def( + "setOptions", + &utils::DartLoader::setOptions, + ::py::arg("options") = utils::DartLoader::Options()) + .def("getOptions", &utils::DartLoader::getOptions) + .def( + "addPackageDirectory", + &utils::DartLoader::addPackageDirectory, + ::py::arg("packageName"), + ::py::arg("packageDirectory")) + .def( + "parseSkeleton", + +[](dart::utils::DartLoader* self, + const dart::common::Uri& uri, + const common::ResourceRetrieverPtr& resourceRetriever, + unsigned int flags) -> dart::dynamics::SkeletonPtr { + DART_SUPPRESS_DEPRECATED_BEGIN + return self->parseSkeleton(uri, resourceRetriever, flags); + DART_SUPPRESS_DEPRECATED_END + }, + ::py::arg("uri"), + ::py::arg("resourceRetriever"), + ::py::arg("flags") = utils::DartLoader::DEFAULT) + .def( + "parseSkeleton", + ::py::overload_cast( + &utils::DartLoader::parseSkeleton), + ::py::arg("uri")) + .def( + "parseSkeletonString", + +[](utils::DartLoader* self, + const std::string& urdfString, + const common::Uri& baseUri, + const common::ResourceRetrieverPtr& resourceRetriever, + unsigned int flags) -> dynamics::SkeletonPtr { + DART_SUPPRESS_DEPRECATED_BEGIN + return self->parseSkeletonString( + urdfString, baseUri, resourceRetriever, flags); + DART_SUPPRESS_DEPRECATED_END + }, + ::py::arg("urdfString"), + ::py::arg("baseUri"), + ::py::arg("resourceRetriever"), + ::py::arg("flags") = utils::DartLoader::DEFAULT) + .def( + "parseSkeletonString", + ::py::overload_cast( + &utils::DartLoader::parseSkeletonString), + ::py::arg("urdfString"), + ::py::arg("baseUri")) + .def( + "parseWorld", + +[](utils::DartLoader* self, + const common::Uri& _uri, + const common::ResourceRetrieverPtr& resourceRetriever, + unsigned int flags) -> simulation::WorldPtr { + DART_SUPPRESS_DEPRECATED_BEGIN + return self->parseWorld(_uri, resourceRetriever, flags); + DART_SUPPRESS_DEPRECATED_END + }, + ::py::arg("uri"), + ::py::arg("resourceRetriever"), + ::py::arg("flags") = utils::DartLoader::DEFAULT) + .def( + "parseWorld", + ::py::overload_cast( + &utils::DartLoader::parseWorld), + ::py::arg("uri")) + .def( + "parseWorldString", + +[](utils::DartLoader* self, + const std::string& urdfString, + const common::Uri& baseUri, + const common::ResourceRetrieverPtr& resourceRetriever, + unsigned int flags) -> simulation::WorldPtr { + DART_SUPPRESS_DEPRECATED_BEGIN + return self->parseWorldString( + urdfString, baseUri, resourceRetriever, flags); + DART_SUPPRESS_DEPRECATED_END + }, + ::py::arg("urdfString"), + ::py::arg("baseUri"), + ::py::arg("resourceRetriever"), + ::py::arg("flags") = utils::DartLoader::DEFAULT) + .def( + "parseWorldString", + ::py::overload_cast( + &utils::DartLoader::parseWorldString), + ::py::arg("urdfString"), + ::py::arg("baseUri")); + + dartLoader.attr("Flags") = dartLoaderFlags; + dartLoader.attr("RootJointType") = dartLoaderRootJointType; + dartLoader.attr("Options") = dartLoaderOptions; } } // namespace python diff --git a/unittests/unit/test_DartLoader.cpp b/unittests/unit/test_DartLoader.cpp index a7446ecafd818..44984a042aeba 100644 --- a/unittests/unit/test_DartLoader.cpp +++ b/unittests/unit/test_DartLoader.cpp @@ -39,6 +39,8 @@ #include "dart/dynamics/WeldJoint.hpp" #include "dart/utils/urdf/DartLoader.hpp" +#include "TestHelpers.hpp" + using namespace dart; using dart::common::Uri; using dart::utils::DartLoader; @@ -209,22 +211,28 @@ TEST(DartLoader, parseUrdfWithoutWorldLink) // clang-format on DartLoader loader; + DartLoader::Options options; - auto robot1 = loader.parseSkeletonString(urdfStr, "", nullptr); + // Default + auto robot1 = loader.parseSkeletonString(urdfStr, ""); ASSERT_TRUE(nullptr != robot1); EXPECT_EQ( robot1->getRootJoint()->getType(), dynamics::FreeJoint::getStaticType()); EXPECT_EQ(robot1->getRootJoint()->getName(), "rootJoint"); - auto robot2 = loader.parseSkeletonString( - urdfStr, "", nullptr, DartLoader::Flags::NONE); + // Floating + options.mDefaultRootJointType = DartLoader::RootJointType::FLOATING; + loader.setOptions(options); + auto robot2 = loader.parseSkeletonString(urdfStr, ""); ASSERT_TRUE(nullptr != robot2); EXPECT_EQ( robot2->getRootJoint()->getType(), dynamics::FreeJoint::getStaticType()); EXPECT_EQ(robot2->getRootJoint()->getName(), "rootJoint"); - auto robot3 = loader.parseSkeletonString( - urdfStr, "", nullptr, DartLoader::Flags::FIXED_BASE_LINK); + // Fixed + options.mDefaultRootJointType = DartLoader::RootJointType::FIXED; + loader.setOptions(options); + auto robot3 = loader.parseSkeletonString(urdfStr, ""); ASSERT_TRUE(nullptr != robot3); EXPECT_EQ( robot3->getRootJoint()->getType(), dynamics::WeldJoint::getStaticType()); @@ -591,3 +599,40 @@ TEST(DartLoader, parseVisualCollisionName) EXPECT_EQ(shape_nodes1[0]->getName(), body1->getName() + "_ShapeNode_0"); EXPECT_EQ(shape_nodes1[1]->getName(), body1->getName() + "_ShapeNode_1"); } + +//============================================================================== +TEST(DartLoader, Options) +{ + // clang-format off + std::string urdfStr = R"( + + + + + )"; + // clang-format on + + DartLoader loader; + DartLoader::Options options; + + // Default inertia + auto robot = loader.parseSkeletonString(urdfStr, ""); + ASSERT_TRUE(nullptr != robot); + auto link_0 = robot->getBodyNode("link_0"); + ASSERT_TRUE(link_0 != nullptr); + EXPECT_TRUE(equals( + link_0->getInertia().getSpatialTensor(), + dynamics::Inertia().getSpatialTensor())); + + // Custom inertia + options.mDefaultInertia.setMass(5); + options.mDefaultInertia.setMoment(1, 2, 3, 4, 5, 6); + loader.setOptions(options); + robot = loader.parseSkeletonString(urdfStr, ""); + ASSERT_TRUE(nullptr != robot); + link_0 = robot->getBodyNode("link_0"); + ASSERT_TRUE(link_0 != nullptr); + EXPECT_TRUE(equals( + link_0->getInertia().getSpatialTensor(), + options.mDefaultInertia.getSpatialTensor())); +} From 7b2a6352a7b1bcfaff8b7cc3dbc64fbb83e85562 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 18 Sep 2021 14:29:25 -0700 Subject: [PATCH 2/2] Update changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7215216cebac0..ceaba67dd502a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,10 @@ * Remove DART_BUILD_DARTPY option: [#1600](https://github.com/dartsim/dart/pull/1600) +* Parsers + + * Added default options to DartLoader for missing properties in URDF files: : [#1605](https://github.com/dartsim/dart/pull/1605) + ### [DART 6.11.1 (2021-08-23)](https://github.com/dartsim/dart/milestone/67?closed=1) * Dynamics