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

URDF->SDF handle links with no inertia or small mass #1238

Merged
merged 47 commits into from
Mar 22, 2023
Merged

URDF->SDF handle links with no inertia or small mass #1238

Show file tree
Hide file tree
Changes from 44 commits
Commits
Show all changes
47 commits
Select commit Hold shift + click to select a range
a7d8a24
Adding tests that catch warnings when urdf have no inertia element, o…
aaronchongth Feb 8, 2023
15845d8
Promote link inertia and mass related urdf2sdf sdfdbg to sdfwarn with…
aaronchongth Feb 9, 2023
bcd9459
Added flag for converting urdf links with small or no mass to frames
aaronchongth Feb 10, 2023
90bfe5f
Converts urdf links with small or no mass to frames, added tests
aaronchongth Feb 10, 2023
09e148e
Adding warning when conversion happens, added tests for small masses,…
aaronchongth Feb 10, 2023
053f70e
Fix cpplint errors
aaronchongth Feb 10, 2023
c5f757d
Adding integration test
aaronchongth Feb 14, 2023
9b0aca3
Fix lint
aaronchongth Feb 14, 2023
d829d50
Use camelCase
aaronchongth Feb 16, 2023
089b8a7
Added URDFMinimumAllowedLinkMass
aaronchongth Feb 16, 2023
4887eae
Fix tests expecting warnings when converting to frames
aaronchongth Feb 17, 2023
cb6d540
Adding inline contains and notContains to test_utils
aaronchongth Feb 17, 2023
d4de275
Using RedirectConsoleStream and ScopeExit
aaronchongth Feb 17, 2023
ecdad41
Refactor to use Root::LoadSdfString
aaronchongth Feb 17, 2023
2d01243
Removing debug message when converted frame is from a root link
aaronchongth Feb 17, 2023
1e5a2da
Added attached_to for frames during conversion, using < instead of ma…
aaronchongth Feb 21, 2023
27bf7b7
Update brief of URDFSetConvertLinkWithNoMassToFrame to mention case o…
aaronchongth Feb 21, 2023
d0a2770
Remove stale commentted code
aaronchongth Feb 21, 2023
2f86ca7
Update comment about the errors we are expecting
aaronchongth Feb 21, 2023
e1b488b
Convert to frame by default, remove minimal mass option, refactor imp…
aaronchongth Feb 28, 2023
a880d4f
Only convert to frame when parent joint is fixed, attaches and transf…
aaronchongth Mar 1, 2023
bfb68c8
Rephrased conversion error, modified unit tests
aaronchongth Mar 1, 2023
dff421c
prints zero mass errors as well when conversion to frame fails
aaronchongth Mar 2, 2023
dd687c3
integration tests that mimic the unit tests
aaronchongth Mar 2, 2023
3d2bf35
Added integration test with valid and invalid use of force torque sen…
aaronchongth Mar 2, 2023
c53243c
Merge branch 'sdf12' into aaron/urdf-inertia-handling
aaronchongth Mar 2, 2023
c9a7f4b
Fix lint
aaronchongth Mar 2, 2023
d29328b
Fix joint reduction logic, more specific error messages, more targete…
aaronchongth Mar 6, 2023
a575e1c
Convert joints to frames when converting links, attach them to conver…
aaronchongth Mar 6, 2023
99db618
Integration tests with child fixed links as well
aaronchongth Mar 6, 2023
0e8c8fb
Change sdferr to sdfwarn, no way to use ParserConfig::WarningsPolicy …
aaronchongth Mar 6, 2023
86ac8a6
Merge branch 'sdf12' into aaron/urdf-inertia-handling
aaronchongth Mar 6, 2023
5d2d26c
sdferr to sdfwarn for the case where conversion to frame is attempted
aaronchongth Mar 7, 2023
b495dd9
Removing case within converting to frame, where parent joint turns in…
aaronchongth Mar 7, 2023
a53f9c7
Reduced scope of implementation, more specific warning messages
aaronchongth Mar 14, 2023
0479650
Remove mention of parser config in warning, since that is not typical…
aaronchongth Mar 14, 2023
432a1cb
Integration tests revisited and modified, removed printouts
aaronchongth Mar 14, 2023
d9c9cb4
Test case showing ParserConfig::URDFSetPreserveFixedJoint(true) overr…
aaronchongth Mar 15, 2023
8976110
Merge branch 'sdf12' into aaron/urdf-inertia-handling
aaronchongth Mar 15, 2023
774d86b
Use sdf::testing::contains instead of local contains functions in tes…
aaronchongth Mar 15, 2023
b290786
Remove unused InitSDF, added TODO for warning when joints are convert…
aaronchongth Mar 16, 2023
bb9fa63
Cleaned up if else cases
aaronchongth Mar 16, 2023
509bb50
Using << operator of Errors
aaronchongth Mar 16, 2023
a0d457a
Replacing placeholder url with expected URL for the documentation fro…
aaronchongth Mar 16, 2023
ba52e6f
URL to tutorial as a constant, removing checking URL from test, just …
aaronchongth Mar 17, 2023
94382d7
Refactored fixed child joint logic as it is never reached
aaronchongth Mar 20, 2023
6aa625c
Reduce number of if statements, renaming to only check if parent join…
aaronchongth Mar 20, 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
131 changes: 63 additions & 68 deletions src/ign_TEST.cc
View file
Open in desktop

Large diffs are not rendered by default.

75 changes: 38 additions & 37 deletions src/parser_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include "sdf/Console.hh"
#include "sdf/Filesystem.hh"
#include "test_config.h"
#include "test_utils.hh"

/////////////////////////////////////////////////
TEST(Parser, initStringTrim)
Expand Down Expand Up @@ -277,13 +278,6 @@ TEST(Parser, NameUniqueness)
}
}

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

/////////////////////////////////////////////////
TEST(Parser, SyntaxErrorInValues)
{
Expand All @@ -301,10 +295,11 @@ TEST(Parser, SyntaxErrorInValues)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Unable to set value [bad 0 0 0 0 0] for key [pose]");
EXPECT_PRED2(contains, buffer.str(), "bad_syntax_pose.sdf:L5");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"bad_syntax_pose.sdf:L5");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/pose:");
}
{
Expand All @@ -315,10 +310,11 @@ TEST(Parser, SyntaxErrorInValues)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Unable to set value [bad ] for key[linear]");
EXPECT_PRED2(contains, buffer.str(), "bad_syntax_double.sdf:L7");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"bad_syntax_double.sdf:L7");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/"
"link[@name=\"link\"]/velocity_decay/linear:");
}
Expand All @@ -330,10 +326,11 @@ TEST(Parser, SyntaxErrorInValues)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Unable to set value [0 1 bad ] for key[gravity]");
EXPECT_PRED2(contains, buffer.str(), "bad_syntax_vector.sdf:L4");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"bad_syntax_vector.sdf:L4");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/gravity:");
}

Expand Down Expand Up @@ -363,15 +360,15 @@ TEST(Parser, MissingRequiredAttributesErrors)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Error Code " +
std::to_string(
static_cast<int>(sdf::ErrorCode::ATTRIBUTE_MISSING)));
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Required attribute[name] in element[link] is not specified "
"in SDF.");
EXPECT_PRED2(contains, buffer.str(), "box_bad_test.world:L6");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(), "box_bad_test.world:L6");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/model[@name=\"box\"]/link:");
}

Expand Down Expand Up @@ -401,14 +398,15 @@ TEST(Parser, IncludesErrors)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Error Code " +
std::to_string(
static_cast<int>(sdf::ErrorCode::ATTRIBUTE_MISSING)));
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"<include> element missing 'uri' attribute");
EXPECT_PRED2(contains, buffer.str(), "includes_missing_uri.sdf:L5");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"includes_missing_uri.sdf:L5");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/include[0]:");
}
{
Expand All @@ -421,14 +419,15 @@ TEST(Parser, IncludesErrors)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Error Code " +
std::to_string(
static_cast<int>(sdf::ErrorCode::URI_LOOKUP)));
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Unable to find uri[missing_model]");
EXPECT_PRED2(contains, buffer.str(), "includes_missing_model.sdf:L6");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"includes_missing_model.sdf:L6");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/include[0]/uri:");
}
{
Expand All @@ -446,15 +445,16 @@ TEST(Parser, IncludesErrors)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Error Code " +
std::to_string(static_cast<int>(sdf::ErrorCode::URI_LOOKUP)));
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Unable to resolve uri[box_missing_config]");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"since it does not contain a model.config");
EXPECT_PRED2(contains, buffer.str(), "includes_model_without_sdf.sdf:L6");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"includes_model_without_sdf.sdf:L6");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/include[0]/uri:");
}
{
Expand All @@ -472,15 +472,16 @@ TEST(Parser, IncludesErrors)
sdf::init(sdf);

sdf::readFile(path, sdf);
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Error Code " +
std::to_string(
static_cast<int>(sdf::ErrorCode::ELEMENT_MISSING)));
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"Failed to find top level <model> / <actor> / <light> for "
"<include>\n");
EXPECT_PRED2(contains, buffer.str(), "includes_without_top_level.sdf:L6");
EXPECT_PRED2(contains, buffer.str(),
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"includes_without_top_level.sdf:L6");
EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"/sdf/world[@name=\"default\"]/include[0]/uri:");
}

Expand Down
135 changes: 111 additions & 24 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,9 @@
#include <urdf_model/link.h>
#include <urdf_parser/urdf_parser.h>

#include "sdf/Error.hh"
#include "sdf/sdf.hh"
#include "sdf/Types.hh"

#include "XmlUtils.hh"
#include "SDFExtension.hh"
Expand Down Expand Up @@ -2663,48 +2665,133 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference)
void CreateSDF(tinyxml2::XMLElement *_root,
urdf::LinkConstSharedPtr _link)
{
// must have an <inertial> block and cannot have zero mass.
// allow det(I) == zero, in the case of point mass geoms.
// Links without an <inertial> block will be considered to have zero mass.
const bool linkHasZeroMass = !_link->inertial || _link->inertial->mass <= 0;

// link must have an <inertial> block and cannot have zero mass, if its parent
// joint and none of its child joints are reduced.
// allow det(I) == zero, in the case of point mass geoms.
// @todo: keyword "world" should be a constant defined somewhere else
if (_link->name != "world" &&
((!_link->inertial) || gz::math::equal(_link->inertial->mass, 0.0)))
if (_link->name != "world" && linkHasZeroMass)
{
if (!_link->child_links.empty())
Errors nonJointReductionErrors;
std::stringstream errorStream;
errorStream << "urdf2sdf: link[" << _link->name << "] has ";
if (!_link->inertial)
{
errorStream << "no <inertial> block defined. ";
}
else
{
sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, ["
<< static_cast<int>(_link->child_links.size())
<< "] children links ignored.\n";
errorStream << "a mass value of less than or equal to zero. ";
}
errorStream << "Please ensure this link has a valid mass to prevent any "
<< "missing links or joints in the resulting SDF model.";
nonJointReductionErrors.emplace_back(
ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
errorStream.str(std::string());

// If joint reduction happens and resolves the issue of this link with no
// mass, no warnings will be emitted
bool jointReductionHappens = _link->parent_joint &&
FixedJointShouldBeReduced(_link->parent_joint) &&
g_reduceFixedJoints;

if (!_link->child_joints.empty())
// check if parent joint can be lumped and reduced
if (!jointReductionHappens && _link->parent_joint)
{
sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, ["
<< static_cast<int>(_link->child_links.size())
<< "] children joints ignored.\n";
if (_link->parent_joint->type == urdf::Joint::FIXED)
{
errorStream << "urdf2sdf: allowing joint lumping by removing any "
<< "<disableFixedJointLumping> or <preserveFixedJoint> "
<< "gazebo tag on fixed parent joint["
<< _link->parent_joint->name
<< "], as well as ensuring that "
<< "ParserConfig::URDFPreserveFixedJoint is false, could "
<< "help resolve this warning. See http://sdformat.org/tuto"
<< "rials?tut=sdformat_urdf_extensions&cat=specification& "
<< "for more information about this behavior.";
nonJointReductionErrors.emplace_back(
ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
errorStream.str(std::string());
}

errorStream << "urdf2sdf: parent joint["
<< _link->parent_joint->name
<< "] ignored.";
nonJointReductionErrors.emplace_back(
ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
errorStream.str(std::string());
}

if (_link->parent_joint)
// check child joints for any possibility of joint lumping and reduction
if (!jointReductionHappens && !_link->child_joints.empty())
{
sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, "
<< "parent joint [" << _link->parent_joint->name
<< "] ignored.\n";
for (const auto &cj : _link->child_joints)
{
if (FixedJointShouldBeReduced(cj) && g_reduceFixedJoints)
{
jointReductionHappens = true;
break;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Codecov says these two lines are not covered. Is it a false negative?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe so! This test will cause the break, https://github.com/gazebosim/sdformat/pull/1238/files#diff-fc2484863e209b4368da90c08381488ec4b010f386c642b1ea1e8bcf82cf6f02R2141-R2190

Could this be due to the code only checking !jointReductionHappens and not do anything if jointReductionHappens is true?

I'm not too sure how to see if this gets reported again, is it just https://app.codecov.io/gh/gazebosim/sdformat/pull/1238? Looks alright at the moment

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tested with gdb, and looks like it really isn't covered. Apparently, a massless link with a non-fixed parent joint and a fixed child joint is reduced before CreateSDF is ever called :

sdformat/src/parser_urdf.cc

Lines 3385 to 3398 in ba52e6f

if (g_reduceFixedJoints)
{
ReduceFixedJoints(robot, urdf::const_pointer_cast<urdf::Link>(rootLink));
}
if (rootLink->name == "world")
{
// convert all children link
for (std::vector<urdf::LinkSharedPtr>::const_iterator
child = rootLink->child_links.begin();
child != rootLink->child_links.end(); ++child)
{
CreateSDF(robot, (*child));
}

This means, the massless link now has mass from the lumped child links. So, I guess if we encounter a massless link here, it would only be if:

  • it's parent joint is reduced (i.e, the link has been lumped into it's parent link) or
  • it's child joint is fixed, but could not be reduced because joint lumping is disabled.

Can you verify my logic? Does that maybe simplify anything?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yup, I can recreate it with gdb too, it doesn't even allow me to set a breakpoint there, probably got optimized away by the compiler. Looking at the code, I can verify that is the intended behavior too.

94382d7, 6aa625c, I've refactored the fixed child joint logic and reduced the number of checks we are doing overall (only if parent joint is reduced)

I kept the use of Errors, instead of using sdfwarn << Error(...) directly, to make easier when we do introduce passing Errors into these functions in the future.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@azeey, per VC, regarding whether or not a similar situation for reduced parent joints is a bug, I don't believe it is a bug, just that checks happen after . Before this PR, joints are lumped recursively onto their parents with L3387 (shared by you above), and relies on https://github.com/gazebosim/sdformat/pull/1238/files#diff-2bc5ca23bcfc66fe173f513399bec8065b1cb4e607d741566517d7368e6bce0fL2705 to not create the link (whose's inertia has been lumped) if the parent joint is reduced, but the urdf link still contains the mass values

so there will be cases where CreateSDF is called on a massless link, https://github.com/gazebosim/sdformat/pull/1238/files#diff-2bc5ca23bcfc66fe173f513399bec8065b1cb4e607d741566517d7368e6bce0fL2713, where the parent has already been reduced, hence the need to check if the parent has been reduced.

on the otherhand, as you surmised, a massless link A with a fixed child joint, and child link B with mass, will already have B's mass lumped into it in CreateSDF, so it should always have mass unless joint lumping was turned off.

}
else if (cj->type == urdf::Joint::FIXED)
{
errorStream << "urdf2sdf: allowing joint lumping by removing any "
<< "<disableFixedJointLumping> or <preserveFixedJoint> "
<< "gazebo tag on fixed child joint["
<< cj->name
<< "], as well as ensuring that "
<< "ParserConfig::URDFPreserveFixedJoint is false, could "
<< "help resolve this warning. See http://sdformat.org/tu"
<< "torials?tut=sdformat_urdf_extensions&cat=specificatio"
<< "n& for more information about this behavior.";
nonJointReductionErrors.emplace_back(
ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
errorStream.str(std::string());
}
}

// none of the child joints can be lumped and reduced
if (!jointReductionHappens)
{
errorStream << "urdf2sdf: ["
<< static_cast<int>(_link->child_joints.size())
<< "] child joints ignored.";
nonJointReductionErrors.emplace_back(
ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
errorStream.str(std::string());
errorStream << "urdf2sdf: ["
<< static_cast<int>(_link->child_links.size())
<< "] child links ignored.";
nonJointReductionErrors.emplace_back(
ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
errorStream.str(std::string());
}
}

sdfdbg << "urdf2sdf: link[" << _link->name
<< "] has no inertia, not modeled in sdf\n";
return;
// there is no way to resolve this link with no mass, all warnings
// accumulated will be emitted
if (!jointReductionHappens)
{
sdfwarn << nonJointReductionErrors;

errorStream << "urdf2sdf: link[" << _link->name
<< "] is not modeled in sdf.";
sdfwarn << Error(ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
return;
}
}

// create <body:...> block for non fixed joint attached bodies
// create <body:...> block for non fixed joint attached bodies that have mass
if ((_link->getParent() && _link->getParent()->name == "world") ||
!g_reduceFixedJoints ||
(!_link->parent_joint ||
!FixedJointShouldBeReduced(_link->parent_joint)))
{
CreateLink(_root, _link, gz::math::Pose3d::Zero);
if (!linkHasZeroMass)
{
CreateLink(_root, _link, gz::math::Pose3d::Zero);
}
}

// recurse into children
Expand Down
Loading