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

Warn when ignoring urdf links due to small masses or no inertia defined #1230

Closed
wants to merge 3 commits into from
Closed

Warn when ignoring urdf links due to small masses or no inertia defined #1230

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a6ffcad
Promote sdfdbg messages when ignoring urdf links to sdfwarn, added tests
aaronchongth Feb 8, 2023
fcc6706
Make warnings more epxlicit, whether inertia is missing or mass is small
aaronchongth Feb 8, 2023
b323657
Fix line length lint
aaronchongth Feb 8, 2023
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
30 changes: 21 additions & 9 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2651,32 +2651,44 @@ void CreateSDF(TiXmlElement *_root,
if (_link->name != "world" &&
((!_link->inertial) || gz::math::equal(_link->inertial->mass, 0.0)))
{
const std::string inertia_issue =
_link->inertial && gz::math::equal(_link->inertial->mass, 0.0) ?
"a mass value of less than 1e-6" :
"no inertia defined";

if (!_link->child_links.empty())
{
sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, ["
sdfwarn << "urdf2sdf: link[" << _link->name
<< "] has "
<< inertia_issue
<< ", ["
<< static_cast<int>(_link->child_links.size())
<< "] children links ignored.\n";
}

if (!_link->child_joints.empty())
{
sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, ["
sdfwarn << "urdf2sdf: link[" << _link->name
<< "] has "
<< inertia_issue
<< ", ["
<< static_cast<int>(_link->child_links.size())
<< "] children joints ignored.\n";
}

if (_link->parent_joint)
{
sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, "
<< "parent joint [" << _link->parent_joint->name
sdfwarn << "urdf2sdf: link[" << _link->name
<< "] has "
<< inertia_issue
<< ", parent joint [" << _link->parent_joint->name
<< "] ignored.\n";
}

sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, not modeled in sdf\n";
sdfwarn << "urdf2sdf: link[" << _link->name
<< "] has "
<< inertia_issue
<< ", not modeled in sdf\n";
return;
}

Expand Down
271 changes: 271 additions & 0 deletions src/parser_urdf_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,20 @@ std::string getMinimalUrdfTxt()
return stream.str();
}

/////////////////////////////////////////////////
/// Check that _a contains _b
static bool contains(const std::string &_a, const std::string &_b)
{
return _a.find(_b) != std::string::npos;
}

/////////////////////////////////////////////////
/// Check that _a does not contain _b
static bool not_contains(const std::string &_a, const std::string &_b)
{
return _a.find(_b) == std::string::npos;
}

/////////////////////////////////////////////////
std::string convertUrdfStrToSdfStr(const std::string &_urdf)
{
Expand Down Expand Up @@ -844,6 +858,263 @@ TEST(URDFParser, OutputPrecision)
EXPECT_EQ("0", poseValues[5]);
}

/////////////////////////////////////////////////
TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia)
{
// Capture sdferr output
std::stringstream buffer;
auto old = std::cerr.rdbuf(buffer.rdbuf());

#ifdef _WIN32
sdf::Console::Instance()->SetQuiet(false);
#endif

{
// clear the contents of the buffer
buffer.str("");

std::ostringstream stream;
stream << "<robot name='test'>"
<< " <origin xyz='0 foo 0' rpy='0 0 0'/>"
<< " <link name='link'>"
<< " <inertial>"
<< " <mass value='1.0'/>"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
<< " </inertial>"
<< " </link>"
<< "</robot>";
TiXmlDocument doc;
SDF_SUPPRESS_DEPRECATED_BEGIN
sdf::URDF2SDF parser_;
SDF_SUPPRESS_DEPRECATED_END
doc.Parse(stream.str().c_str());
TiXmlDocument sdf_result = parser_.InitModelDoc(&doc);

EXPECT_PRED2(not_contains, buffer.str(),
"urdf2sdf: link[link] has no inertia defined, not modeled in sdf");
}

{
// clear the contents of the buffer
buffer.str("");

std::ostringstream stream;
stream << "<robot name='test'>"
<< " <origin xyz='0 foo 0' rpy='0 0 0'/>"
<< " <link name='link' />"
<< "</robot>";
TiXmlDocument doc;
SDF_SUPPRESS_DEPRECATED_BEGIN
sdf::URDF2SDF parser_;
SDF_SUPPRESS_DEPRECATED_END
doc.Parse(stream.str().c_str());
TiXmlDocument sdf_result = parser_.InitModelDoc(&doc);

EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link] has no inertia defined, not modeled in sdf");
}

{
// clear the contents of the buffer
buffer.str("");

std::ostringstream stream;
stream << "<robot name='test'>"
<< " <origin xyz='0 foo 0' rpy='0 0 0'/>"
<< " <link name='link1' />"
<< " <joint name='joint1_2' type='continuous'>"
<< " <parent link='link1' />"
<< " <child link='link2' />"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 " << IGN_PI*0.5
<< "' />"
<< " </joint>"
<< " <link name='link2'>"
<< " <inertial>"
<< " <mass value='1.0'/>"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
<< " </inertial>"
<< " </link>"
<< "</robot>";
TiXmlDocument doc;
SDF_SUPPRESS_DEPRECATED_BEGIN
sdf::URDF2SDF parser_;
SDF_SUPPRESS_DEPRECATED_END
doc.Parse(stream.str().c_str());
TiXmlDocument sdf_result = parser_.InitModelDoc(&doc);

EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link1] has no inertia defined, [1] children links "
"ignored");
EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link1] has no inertia defined, [1] children joints "
"ignored");
EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link1] has no inertia defined, not modeled in sdf");
}

{
// clear the contents of the buffer
buffer.str("");

std::ostringstream stream;
stream << "<robot name='test'>"
<< " <origin xyz='0 foo 0' rpy='0 0 0'/>"
<< " <link name='link1'>"
<< " <inertial>"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
<< " <mass value='1.0'/>"
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
<< " </inertial>"
<< " </link>"
<< " <joint name='joint1_2' type='continuous'>"
<< " <parent link='link1' />"
<< " <child link='link2' />"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 " << IGN_PI*0.5
<< "' />"
<< " </joint>"
<< " <link name='link2' />"
<< "</robot>";
TiXmlDocument doc;
SDF_SUPPRESS_DEPRECATED_BEGIN
sdf::URDF2SDF parser_;
SDF_SUPPRESS_DEPRECATED_END
doc.Parse(stream.str().c_str());
TiXmlDocument sdf_result = parser_.InitModelDoc(&doc);

EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link2] has no inertia defined, parent joint [joint1_2] "
"ignored");
EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link2] has no inertia defined, not modeled in sdf");
}

{
// clear the contents of the buffer
buffer.str("");

std::ostringstream stream;
stream << "<robot name='test'>"
<< " <origin xyz='0 foo 0' rpy='0 0 0'/>"
<< " <link name='link1' />"
<< " <joint name='joint1_2' type='continuous'>"
<< " <parent link='link1' />"
<< " <child link='link2' />"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 " << IGN_PI*0.5
<< "' />"
<< " </joint>"
<< " <link name='link2' />"
<< "</robot>";
TiXmlDocument doc;
SDF_SUPPRESS_DEPRECATED_BEGIN
sdf::URDF2SDF parser_;
SDF_SUPPRESS_DEPRECATED_END
doc.Parse(stream.str().c_str());
TiXmlDocument sdf_result = parser_.InitModelDoc(&doc);

EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link1] has no inertia defined, [1] children links "
"ignored");
EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link1] has no inertia defined, [1] children joints "
"ignored");
EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link1] has no inertia defined, not modeled in sdf");

// It parses in sequence, therefore once ignored, no warnings for link2 will
// be issued.
EXPECT_PRED2(not_contains, buffer.str(),
"urdf2sdf: link[link2] has no inertia defined, parent joint [joint1_2] "
"will be ignored");
EXPECT_PRED2(not_contains, buffer.str(),
"urdf2sdf: link[link2] has no inertia defined, not modeled in sdf");
}

// Revert cerr rdbug so as to not interfere with other tests
std::cerr.rdbuf(old);
#ifdef _WIN32
sdf::Console::Instance()->SetQuiet(true);
#endif
}

/////////////////////////////////////////////////
TEST(URDFParser, WarningWhenIgnoringLinksWithSmallInertia)
{
// Capture sdferr output
std::stringstream buffer;
auto old = std::cerr.rdbuf(buffer.rdbuf());

#ifdef _WIN32
sdf::Console::Instance()->SetQuiet(false);
#endif

{
// clear the contents of the buffer
buffer.str("");

std::ostringstream stream;
stream << "<robot name='test'>"
<< " <origin xyz='0 foo 0' rpy='0 0 0'/>"
<< " <link name='link'>"
<< " <inertial>"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
<< " <mass value='1e-6'/>"
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
<< " </inertial>"
<< " </link>"
<< "</robot>";
TiXmlDocument doc;
SDF_SUPPRESS_DEPRECATED_BEGIN
sdf::URDF2SDF parser_;
SDF_SUPPRESS_DEPRECATED_END
doc.Parse(stream.str().c_str());
TiXmlDocument sdf_result = parser_.InitModelDoc(&doc);

EXPECT_PRED2(contains, buffer.str(),
"urdf2sdf: link[link] has a mass value of less than 1e-6, not modeled "
"in sdf");
}

{
// clear the contents of the buffer
buffer.str("");

std::ostringstream stream;
stream << "<robot name='test'>"
<< " <origin xyz='0 foo 0' rpy='0 0 0'/>"
<< " <link name='link'>"
<< " <inertial>"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
<< " <mass value='2e-6'/>"
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
<< " </inertial>"
<< " </link>"
<< "</robot>";
TiXmlDocument doc;
SDF_SUPPRESS_DEPRECATED_BEGIN
sdf::URDF2SDF parser_;
SDF_SUPPRESS_DEPRECATED_END
doc.Parse(stream.str().c_str());
TiXmlDocument sdf_result = parser_.InitModelDoc(&doc);

EXPECT_PRED2(not_contains, buffer.str(),
"urdf2sdf: link[link] has a mass value of less than 1e-6, not modeled "
"in sdf");
}

// Revert cerr rdbug so as to not interfere with other tests
std::cerr.rdbuf(old);
#ifdef _WIN32
sdf::Console::Instance()->SetQuiet(true);
#endif
}

/////////////////////////////////////////////////
/// Main
int main(int argc, char **argv)
Expand Down