URDF parser fails if two consecutive fixed joints are present in the model.

[osrf-migration]

Original report (archived issue) by Silvio Traversaro (Bitbucket: traversaro).

The original report had attachments: singleFixed.urdf, doubleFixed.urdf

---

If two consecutive fixed joints are present in a URDF model, and both joints have a massless child, the conversion to SDF is broken. In particular the COM of the resulting link is reset to zero (observed in Linux) or set to NaN (observed on OS X).

To reproduce the issue, call the gz sdf -p program in the attached urdf files:

Working model, with a single fixed joint:

pegua@pareto:~$ cat singleFixed.urdf 
<?xml version="1.0" encoding="utf-8"?>
<robot name="exampleRobot">
	<link name="link1">
		<inertial>
			<mass value="25.49"/>
			<origin xyz="10.0 20.0 30.0" rpy="0 -0 0"/>
			<inertia ixx="0.50297" ixy="0" ixz="0" iyy="0.332368" iyz="0" izz="0.479884"/>
		</inertial>
	</link>
	<joint name="joint1" type="fixed">
		<origin xyz="0 0 0" rpy="0 -0 0"/>
		<axis xyz="0 0 1"/>
		<parent link="link1"/>
		<child link="link2"/>
		<limit effort="-1" velocity="-1" lower="0" upper="0"/>
	</joint>
    <link name="link2">
		<inertial>
			<mass value="0.0"/>
			<origin xyz="0 0 0" rpy="0 -0 0"/>
			<inertia ixx="0.0" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.0"/>
		</inertial>
	</link>
    <!--
    <joint name="joint2" type="fixed">
		<origin xyz="0 0 0" rpy="1.57079637 -8.74227766e-08 3.14159274"/>
		<axis xyz="0 0 1"/>
		<parent link="link2"/>
		<child link="link3"/>
		<limit effort="-1" velocity="-1" lower="0" upper="0"/>
	</joint>
    <link name="link3">
		<inertial>
			<mass value="0.0"/>
			<origin xyz="0 0 0" rpy="0 -0 0"/>
			<inertia ixx="0.0" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.0"/>
		</inertial>
	</link>
    -->
</robot>
pegua@pareto:~$ gz sdf -p singleFixed.urdf 
<sdf version='1.5'>
  <model name='exampleRobot'>
    <link name='link1'>
      <pose>0 0 0 0 -0 0</pose>
      <inertial>
        <pose>10 20 30 0 -0 0</pose>
        <mass>25.49</mass>
        <inertia>
          <ixx>0.50297</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.332368</iyy>
          <iyz>0</iyz>
          <izz>0.479884</izz>
        </inertia>
      </inertial>
    </link>
  </model>
</sdf>

Broken model, with two fixed joints (note the COM pose):

pegua@pareto:~$ cat doubleFixed.urdf 
<?xml version="1.0" encoding="utf-8"?>
<robot name="exampleRobot">
	<link name="link1">
		<inertial>
			<mass value="25.49"/>
			<origin xyz="10.0 20.0 30.0" rpy="0 -0 0"/>
			<inertia ixx="0.50297" ixy="0" ixz="0" iyy="0.332368" iyz="0" izz="0.479884"/>
		</inertial>
	</link>
	<joint name="joint1" type="fixed">
		<origin xyz="0 0 0" rpy="0 -0 0"/>
		<axis xyz="0 0 1"/>
		<parent link="link1"/>
		<child link="link2"/>
		<limit effort="-1" velocity="-1" lower="0" upper="0"/>
	</joint>
    <link name="link2">
		<inertial>
			<mass value="0.0"/>
			<origin xyz="0 0 0" rpy="0 -0 0"/>
			<inertia ixx="0.0" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.0"/>
		</inertial>
	</link>
    <joint name="joint2" type="fixed">
		<origin xyz="0 0 0" rpy="1.57079637 -8.74227766e-08 3.14159274"/>
		<axis xyz="0 0 1"/>
		<parent link="link2"/>
		<child link="link3"/>
		<limit effort="-1" velocity="-1" lower="0" upper="0"/>
	</joint>
    <link name="link3">
		<inertial>
			<mass value="0.0"/>
			<origin xyz="0 0 0" rpy="0 -0 0"/>
			<inertia ixx="0.0" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.0"/>
		</inertial>
	</link>
</robot>
pegua@pareto:~$ gz sdf -p doubleFixed.urdf
Error [Param.cc:462] Unable to set value [-nan -nan -nan 0 -0 0] for key[pose]
<sdf version='1.5'>
  <model name='exampleRobot'>
    <link name='link1'>
      <pose>0 0 0 0 -0 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>25.49</mass>
        <inertia>
          <ixx>nan</ixx>
          <ixy>nan</ixy>
          <ixz>nan</ixz>
          <iyy>nan</iyy>
          <iyz>nan</iyz>
          <izz>nan</izz>
        </inertia>
      </inertial>
    </link>
  </model>
</sdf>

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

• Edited issue description

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

This happens because ReduceFixedJoints() in parser_urdf.cc recurses to the children of the tree and adds them up gradually until it gets back to the top. The inertial lumping is using code copied from ODE. Specifically, dMassAdd doesn't check for division by zero if both masses are zero, which is causing the nan values.

I'll see if I can replace the ODE inertial code with the ignition::math::Inertial class that was recently written.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

I've started work in d696f97 (branch issue_105), but there's currently test failures, so it's not quite working yet.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

• set assignee_account_id to "557058:5de38267-b118-494c-aa76-4fab35448816"
• set assignee to "scpeters (Bitbucket: scpeters, GitHub: scpeters)"

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

Hi @scpeters ,
I am affected by this bug on a project and I would be happy to continue your work fixing this bug, starting from https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/commits/bec67123f1b0 (d696f97) .
It would be convenient for me if this fix was released for a Gazebo 7 or 8 . Do you know if there is any minor release already planned for the near future, so I can plan the timing of the fix? Thanks a lot.

[osrf-migration]

Original comment by Antoine Hoarau (Bitbucket: ahoarau).

---

Also affected with this bug ... Any news ?

[azeey]

@scpeters I wonder if the test failures you mentioned is related to the recent fix in ign-math