Skip to content

Commit

Permalink
Change urdf parser to warn if robot model has multi-tree (#1270)
Browse files Browse the repository at this point in the history
This PR addresses #1267.

***

**Before creating a pull request**

- [x] Document new methods and classes
- [x] Format new code files using `clang-format`

**Before merging a pull request**

- [x] Set version target by selecting a milestone on the right side
- [x] Summarize this change in `CHANGELOG.md`
- [x] Add unit test(s) for this change
  • Loading branch information
jslee02 authored Apr 6, 2019
1 parent 3deec2c commit 0f4cf21
Show file tree
Hide file tree
Showing 4 changed files with 137 additions and 60 deletions.
4 changes: 4 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,10 @@

* Added (experimental) Python binding: [#1237](https://github.com/dartsim/dart/pull/1237)

* Parsers

* Changed urdf parser to warn if robot model has multi-tree: [#1270](https://github.com/dartsim/dart/pull/1270)

* Build System

* Changed to use GNUInstallDirs for install paths: [#1241](https://github.com/dartsim/dart/pull/1241)
Expand Down
99 changes: 47 additions & 52 deletions dart/utils/urdf/DartLoader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -190,53 +190,44 @@ simulation::WorldPtr DartLoader::parseWorldString(
return world;
}

/**
* @function modelInterfaceToSkeleton
* @brief Read the ModelInterface and spits out a Skeleton object
*/
//==============================================================================
dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(
const urdf::ModelInterface* _model,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
const urdf::ModelInterface* model,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever)
{
dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(_model->getName());
dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(model->getName());

dynamics::BodyNode* rootNode = nullptr;
const urdf::Link* root = _model->getRoot().get();
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
// practically treated as a keyword.
if(root->name == "world")
{
if(_model->getRoot()->child_links.size() != 1)
{
dterr << "[DartLoader::modelInterfaceToSkeleton] No unique link attached to world.\n";
}
else
if(model->getRoot()->child_links.size() > 1)
{
root = root->child_links[0].get();
dynamics::BodyNode::Properties rootProperties;
if (!createDartNodeProperties(root, rootProperties, _baseUri, _resourceRetriever))
return nullptr;

rootNode = createDartJointAndNode(
root->parent_joint.get(), rootProperties, nullptr, skeleton,
_baseUri, _resourceRetriever);
if(nullptr == rootNode)
{
dterr << "[DartLoader::modelInterfaceToSkeleton] Failed to create root node!\n";
return nullptr;
}

const auto result
= createShapeNodes(_model, root, rootNode,
_baseUri, _resourceRetriever);
if(!result)
return nullptr;
dtwarn << "[DartLoader::modelInterfaceToSkeleton] The world link has "
<< "more than one child links. This leads to creating a "
<< "multi-tree robot. Multi-tree robot is supported by DART, but "
<< "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))
if (!createDartNodeProperties(
root, rootProperties, baseUri, resourceRetriever))
{
return nullptr;
}

std::pair<dynamics::Joint*, dynamics::BodyNode*> pair =
skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>(
Expand All @@ -247,58 +238,62 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(
rootNode = pair.second;

const auto result
= createShapeNodes(_model, root, rootNode,
_baseUri, _resourceRetriever);
= createShapeNodes(model, root, rootNode, baseUri, resourceRetriever);

if(!result)
return nullptr;
}

for(std::size_t i = 0; i < root->child_links.size(); i++)
{
if (!createSkeletonRecursive(
_model, skeleton, root->child_links[i].get(), rootNode,
_baseUri, _resourceRetriever))
model, skeleton, root->child_links[i].get(), rootNode,
baseUri, resourceRetriever))
{
return nullptr;

}
}

// Find mimic joints
for(std::size_t i = 0; i < root->child_links.size(); i++)
addMimicJointsRecursive(_model, skeleton, root->child_links[i].get());
addMimicJointsRecursive(model, skeleton, root->child_links[i].get());

return skeleton;
}

//==============================================================================
bool DartLoader::createSkeletonRecursive(
const urdf::ModelInterface* model,
dynamics::SkeletonPtr _skel,
const urdf::Link* _lk,
dynamics::BodyNode* _parentNode,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
dynamics::SkeletonPtr skel,
const urdf::Link* lk,
dynamics::BodyNode* parentNode,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever)
{
dynamics::BodyNode::Properties properties;
if (!createDartNodeProperties(_lk, properties, _baseUri, _resourceRetriever))
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,
baseUri, resourceRetriever);

if(!node)
return false;

const auto result = createShapeNodes(
model, _lk, node, _baseUri, _resourceRetriever);
model, lk, node, baseUri, resourceRetriever);

if(!result)
return false;

for(std::size_t i = 0; i < _lk->child_links.size(); ++i)
for(std::size_t i = 0; i < lk->child_links.size(); ++i)
{
if (!createSkeletonRecursive(model, _skel, _lk->child_links[i].get(), node,
_baseUri, _resourceRetriever))
if (!createSkeletonRecursive(model, skel, lk->child_links[i].get(), node,
baseUri, resourceRetriever))
{
return false;
}
}
return true;
}
Expand Down
15 changes: 8 additions & 7 deletions dart/utils/urdf/DartLoader.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,17 +126,18 @@ class DartLoader {
typedef std::shared_ptr<dynamics::BodyNode::Properties> BodyPropPtr;
typedef std::shared_ptr<dynamics::Joint::Properties> JointPropPtr;

/// Parses the ModelInterface and spits out a Skeleton object
static dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(
const urdf::ModelInterface* _model,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever);
const urdf::ModelInterface* model,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever);

static bool createSkeletonRecursive(
const urdf::ModelInterface* model,
dynamics::SkeletonPtr _skel,
const urdf::Link* _lk,
dynamics::BodyNode* _parent,
const common::Uri& _baseUri,
dynamics::SkeletonPtr skel,
const urdf::Link* lk,
dynamics::BodyNode* parent,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever);

static bool addMimicJointsRecursive(
Expand Down
79 changes: 78 additions & 1 deletion unittests/unit/test_DartLoader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@

#include <iostream>
#include <gtest/gtest.h>
#include "dart/dynamics/FreeJoint.hpp"
#include "dart/utils/urdf/DartLoader.hpp"

using dart::common::Uri;
Expand Down Expand Up @@ -248,7 +249,6 @@ TEST(DartLoader, mimicJoint)
EXPECT_DOUBLE_EQ(joint2->getMimicOffset(), 0.1);
}


TEST(DartLoader, badMimicJoint)
{
std::string urdfStr =
Expand Down Expand Up @@ -330,3 +330,80 @@ TEST(DartLoader, badMimicJoint)
EXPECT_TRUE(joint2->getActuatorType() != dart::dynamics::Joint::MIMIC);
EXPECT_TRUE(nullptr == joint2->getMimicJoint());
}

TEST(DartLoader, WorldShouldBeTreatedAsKeyword)
{
const std::string urdfStr =
"<robot name=\"testRobot\"> "
" <link name=\"world\"/> "
" <joint name=\"world_to_1\" type=\"revolute\"> "
" <parent link=\"world\"/> "
" <child link=\"link_0\"/> "
" <axis xyz=\"0 0 1\"/> "
" <limit effort=\"2.5\" lower=\"-3.14159265359\" "
" upper=\"3.14159265359\" velocity=\"3.00545697193\"/> "
" <axis xyz=\"0 0 1\"/> "
" <dynamics damping=\"1.2\" friction=\"2.3\"/> "
" </joint> "
" <link name=\"link_0\"/> "
"</robot> ";

DartLoader loader;
auto robot = loader.parseSkeletonString(urdfStr, "");
EXPECT_TRUE(nullptr != robot);

EXPECT_TRUE(robot->getNumBodyNodes() == 1u);
EXPECT_TRUE(robot->getRootBodyNode()->getName() == "link_0");
}

TEST(DartLoader, SingleLinkWithoutJoint)
{
const std::string urdfStr =
"<robot name=\"testRobot\"> "
" <link name=\"link_0\"/> "
"</robot> ";

DartLoader loader;
auto robot = loader.parseSkeletonString(urdfStr, "");
EXPECT_TRUE(nullptr != robot);

EXPECT_TRUE(robot->getNumBodyNodes() == 1u);
EXPECT_TRUE(robot->getRootBodyNode()->getName() == "link_0");
EXPECT_TRUE(robot->getRootJoint()->getType()
== dart::dynamics::FreeJoint::getStaticType());
}

TEST(DartLoader, MultiTreeRobot)
{
const std::string urdfStr =
"<robot name=\"testRobot\"> "
" <link name=\"world\"/> "
" <joint name=\"world_to_0\" type=\"revolute\"> "
" <parent link=\"world\"/> "
" <child link=\"link_0\"/> "
" <axis xyz=\"0 0 1\"/> "
" <limit effort=\"2.5\" lower=\"-3.14159265359\" "
" upper=\"3.14159265359\" velocity=\"3.00545697193\"/> "
" <axis xyz=\"0 0 1\"/> "
" <dynamics damping=\"1.2\" friction=\"2.3\"/> "
" </joint> "
" <link name=\"link_0\"/> "
" <joint name=\"world_to_1\" type=\"revolute\"> "
" <parent link=\"world\"/> "
" <child link=\"link_1\"/> "
" <axis xyz=\"0 0 1\"/> "
" <limit effort=\"2.5\" lower=\"-3.14159265359\" "
" upper=\"3.14159265359\" velocity=\"3.00545697193\"/> "
" <axis xyz=\"0 0 1\"/> "
" <dynamics damping=\"1.2\" friction=\"2.3\"/> "
" </joint> "
" <link name=\"link_1\"/> "
"</robot> ";

DartLoader loader;
auto robot = loader.parseSkeletonString(urdfStr, "");
EXPECT_TRUE(nullptr != robot);

EXPECT_EQ(robot->getNumBodyNodes(), 2u);
EXPECT_EQ(robot->getNumTrees(), 2u);
}

0 comments on commit 0f4cf21

Please sign in to comment.