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

[aaronchongth]

🦟 Bug fix

Related to #199, and #1007.

Summary

(edited)

Current behavior

When a URDF link has no <inertial> block or has a mass of less than or eqaul to 1e-6 it is considered a link without mass, and if joint lumping does not help convert it to a frame, it will be ignored during the conversion to sdf, with debug messages instead of warnings or errors, which is easily missed and users might be surprised when a large part of their model disappears.

---

Changes

• Promotes sdfdbg to sdfwarn for when links with no block or mass values of 0 or less are ignored, parent joint, and their child joints and links
• More specific warnings during conversion from URDF to SDF, or when links and joints are ignored, as well as suggestions to remove <gazebo> tags to allow joint lumping and reduction to resolve massless link issues
• Fix a bug in joint lumping and reducing, https://github.com/gazebosim/sdf_tutorials/pull/87/files#r1128571697, tested here, https://github.com/gazebosim/sdformat/pull/1238/files#diff-ba763ab70ecd0b5b139acc603e4a4f2861453b0480d80ae5ff9e6d8ed6d4398cR115-R193
• use > 0 for mass requirement, instead of gz::math::equals and the epsilon of 1e-6
• Added more unit tests
• Started integration tests, with most cases from unit tests, added a special case of the torque force sensor, when the joint targeted by the sensor becomes invalid due to joint lumping, therefore this is another special case that needs documentation

Tests

./build/sdformat12/bin/UNIT_parser_urdf_TEST
./build/sdformat12/bin/INTEGRATION_urdf_to_sdf

Example of suggesting removal of <gazebo> tag

Intermediate joint with zero mass, fixed parent link, but joint lumping and reduction disabled with gazebo tag,

<robot name='test_robot'>
  <link name='link1'>
    <inertial>
      <mass value='0.1' />
      <origin rpy='1.570796326794895 0 0' xyz='0.123456789123456 0 0.0' />
      <inertia ixx='0.01' ixy='0' ixz='0' iyy='0.01' iyz='0' izz='0.01' />
    </inertial>
  </link>
  <link name='link2'/>
  <joint name='joint1_2' type='fixed'>
    <parent link='link1' />
    <child  link='link2' />
    <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 1.57'/>
  </joint>
  <link name='link3'>
    <inertial>
      <mass value='0.1' />
      <origin rpy='1.570796326794895 0 0' xyz='0.123456789123456 0 0.0' />
      <inertia ixx='0.01' ixy='0' ixz='0' iyy='0.01' iyz='0' izz='0.01' />
    </inertial>
  </link>
  <joint name='joint2_3' type='continuous'>
    <parent link='link2' />
    <child  link='link3' />
    <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 1.57'/>
  </joint>
  <gazebo reference='joint1_2'>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
</robot>

Warnings expected:

urdf2sdf: link[link3] has no <inertial> block defined.
urdf2sdf: allowing joint lumping by removing the <disableFixedJointLumping> tag or setting it to false
on fixed parent joint[joint1_2] could help resolve this warning.
urdf2sdf: parent joint[joint2_3] ignored
urdf2sdf: link[link3] is not modeled in sdf

Resulting sdf will have everything below joint1_2 ignored,

<sdf version="1.7">
    <model name="test_robot">
        <link name="link1">
            <inertial>
                <pose>0.123456789123456 0 0 1.570796326794895 0 0</pose>
                <mass>0.1</mass>
                <inertia>
                    <ixx>0.01</ixx>
                    <ixy>0</ixy>
                    <ixz>0</ixz>
                    <iyy>0.01</iyy>
                    <iyz>0</iyz>
                    <izz>0.01</izz>
                </inertia>
            </inertial>
        </link>
    </model>
</sdf>

Removing it, we get

<robot name='test_robot'>
  <link name='link1'>
    <inertial>
      <mass value='0.1' />
      <origin rpy='1.570796326794895 0 0' xyz='0.123456789123456 0 0.0' />
      <inertia ixx='0.01' ixy='0' ixz='0' iyy='0.01' iyz='0' izz='0.01' />
    </inertial>
  </link>
  <link name='link2'/>
  <joint name='joint1_2' type='fixed'>
    <parent link='link1' />
    <child  link='link2' />
    <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 1.57'/>
  </joint>
  <link name='link3'>
    <inertial>
      <mass value='0.1' />
      <origin rpy='1.570796326794895 0 0' xyz='0.123456789123456 0 0.0' />
      <inertia ixx='0.01' ixy='0' ixz='0' iyy='0.01' iyz='0' izz='0.01' />
    </inertial>
  </link>
  <joint name='joint2_3' type='continuous'>
    <parent link='link2' />
    <child  link='link3' />
    <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 1.57'/>
  </joint>
</robot>

Resulting in no warnings or errors, and a valid sdf,

<sdf version="1.7">
    <model name="test_robot">
        <link name="link1">
            <inertial>
                <pose>0.123456789123456 0 0 1.570796326794895 0 0</pose>
                <mass>0.1</mass>
                <inertia>
                    <ixx>0.01</ixx>
                    <ixy>0</ixy>
                    <ixz>0</ixz>
                    <iyy>0.01</iyy>
                    <iyz>0</iyz>
                    <izz>0.01</izz>
                </inertia>
            </inertial>
        </link>
        <joint type="revolute" name="joint2_3">
            <pose relative_to="link1">0 0 0 0 0 3.14</pose>
            <parent>link1</parent>
            <child>link3</child>
            <axis>
                <xyz>1 0 0</xyz>
                <limit/>
                <dynamics/>
            </axis>
        </joint>
        <link name="link3">
            <pose relative_to="joint2_3"/>
            <inertial>
                <pose>0.123456789123456 0 0 1.570796326794895 0 0</pose>
                <mass>0.1</mass>
                <inertia>
                    <ixx>0.01</ixx>
                    <ixy>0</ixy>
                    <ixz>0</ixz>
                    <iyy>0.01</iyy>
                    <iyz>0</iyz>
                    <izz>0.01</izz>
                </inertia>
            </inertial>
        </link>
        <frame name="joint1_2" attached_to="link1">
            <pose>0 0 0 0 -0 1.57</pose>
        </frame>
        <frame name="link2" attached_to="joint1_2"/>
    </model>
</sdf>

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[codecov]

Codecov Report

Merging #1238 (32da52c) into sdf12 (d9d5d75) will increase coverage by 0.10%.
The diff coverage is 100.00%.

❗ Current head 32da52c differs from pull request most recent head 6aa625c. Consider uploading reports for the commit 6aa625c to get more accurate results

@@            Coverage Diff             @@
##            sdf12    #1238      +/-   ##
==========================================
+ Coverage   92.03%   92.14%   +0.10%     
==========================================
  Files          79       79              
  Lines       13090    13120      +30     
==========================================
+ Hits        12047    12089      +42     
+ Misses       1043     1031      -12     

Impacted Files	Coverage Δ
src/parser_urdf.cc	88.63% <100.00%> (+0.82%)	⬆️

... and 1 file with indirect coverage changes

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

[azeey]

The main issue that we have to resolve is what to assign the attached_to attribute of the created frame. In thinking about that I've come to realize there will be some use cases we can't/shouldn't support as they will cause more confusion. My conclusion so far is, for the link->frame conversion, the link has have a parent or child joint with a "fixed" type.

[aaronchongth]

Per VC,

• added a separate test to show the behavior of setting ParserConfig::URDFSetPreserveFixedJoint to true, overriding gazebo tags <disableFixedJointLumping> and <preserveFixedJoint>, https://github.com/gazebosim/sdformat/pull/1238/files#diff-fc2484863e209b4368da90c08381488ec4b010f386c642b1ea1e8bcf82cf6f02R703-R803
• urging users to fix links without masses in warning, https://github.com/gazebosim/sdformat/pull/1238/files#diff-2bc5ca23bcfc66fe173f513399bec8065b1cb4e607d741566517d7368e6bce0fR2688-R2689
• suggesting to allow joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tags, and making sure ParserConfig::URDFSetPreserveFixedJoint is false. Adding a url to tutorial with more information about the these tags' and flag's behavior (it is currently just a placeholder), https://github.com/gazebosim/sdformat/pull/1238/files#diff-2bc5ca23bcfc66fe173f513399bec8065b1cb4e607d741566517d7368e6bce0fR2707-R2714

[azeey]

Just a few minor comments, otherwise, this looks great! Thanks a lot for iterating and for the through tests. This was a doozy!

[aaronchongth]

I've also committed the expected URL of gazebosim/sdf_tutorials#88, we can wait for that to be merged before hitting the green button then

[azeey]

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

[aaronchongth]

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

[azeey]

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?

[aaronchongth]

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.

[aaronchongth]

@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.