search

gazebosim / sdformat

Simulation Description Format (SDFormat) parser and description files.
http://sdformat.org
Apache License 2.0
155 stars 90 forks source link

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

Open osrf-migration opened 8 years ago

osrf-migration commented 8 years ago

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 commented 8 years ago

Original comment by Silvio Traversaro (Bitbucket: traversaro).

osrf-migration commented 8 years ago

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 commented 8 years ago

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

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

osrf-migration commented 7 years ago

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

osrf-migration commented 7 years ago

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 (d696f97e1875ee6e1107e45fea9d322b45faea9d) . 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 commented 5 years ago

Original comment by Antoine Hoarau (Bitbucket: ahoarau).

Also affected with this bug ... Any news ?

azeey commented 4 years ago

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