Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Change urdf parser to warn if robot model has multi-tree #1270

Merged
merged 5 commits into from
Apr 6, 2019
Merged

Change urdf parser to warn if robot model has multi-tree #1270

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
885d4ce
Make multi-tree robot support for URDF explicit
Apr 2, 2019
a667350
Update changelog
Apr 3, 2019
bebc388
Update CHANGELOG.md
jslee02 Apr 3, 2019
c8dddca
Merge branch 'master' into urdf_parser_v2
jslee02 Apr 5, 2019
8ebf720
Merge branch 'master' into urdf_parser_v2
jslee02 Apr 6, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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);
}