Support for <fluid_added_mass> definition in URDF files

[gabrielfpacheco]

After #1077, SDFormat v1.10 introduced <fluid_added_mass> to the link's <inertial>. However, the corresponding support to set this property from a URDF file has not been created. This issue aims to discuss potential solution paths.

Desired behavior

An URDF with a <gazebo> tag defining the fluid added mass for the link

<?xml version="1.0" ?>
<robot name="added_mass_example">
  <link name="base_link">
    <inertial name="inertial">
      <origin xyz="1 2 3" rpy="-0.1 0.2 -0.3"/>
      <mass value="1"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
  </link>
  <gazebo reference="base_link">
    <fluid_added_mass>
      <xx>1</xx>
      <xy>0</xy>
      <xz>0</xz>
      <xp>0</xp>
      <xq>0</xq>
      <xr>0</xr>
      <yy>1</yy>
      <yz>0</yz>
      <yp>0</yp>
      <yq>0</yq>
      <yr>0</yr>
      <zz>1</zz>
      <zp>0</zp>
      <zq>0</zq>
      <zr>0</zr>
      <pp>1</pp>
      <pq>0</pq>
      <pr>0</pr>
      <qq>1</qq>
      <qr>0</qr>
      <rr>1</rr>
    </fluid_added_mass>
  </gazebo>
</robot>

Should generate a corresponding SDF as follows:

<sdf version='1.11'>
  <model name='added_mass_example'>
    <link name='base_link'>
      <inertial>
        <pose>1 2 3 -0.099999999999999992 0.20000000000000001 -0.29999999999999999</pose>
        <mass>1</mass>
        <inertia>
          <ixx>1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>1</iyy>
          <iyz>0</iyz>
          <izz>1</izz>
        </inertia>
        <fluid_added_mass>
          <xx>1</xx>
          <xy>0</xy>
          <xz>0</xz>
          <xp>0</xp>
          <xq>0</xq>
          <xr>0</xr>
          <yy>1</yy>
          <yz>0</yz>
          <yp>0</yp>
          <yq>0</yq>
          <yr>0</yr>
          <zz>1</zz>
          <zp>0</zp>
          <zq>0</zq>
          <zr>0</zr>
          <pp>1</pp>
          <pq>0</pq>
          <pr>0</pr>
          <qq>1</qq>
          <qr>0</qr>
          <rr>1</rr>
        </fluid_added_mass>
      </inertial>
    </link>
  </model>
</sdf>

Alternatives considered

Within a <gazebo reference="link_name"> tag:

1. Handle <fluid_added_mass> as other link-specific properties, such as mu1 and mu2, and place it directly into <inertial> where it belongs since a link is always supposed to have one, and only one, inertial element. This is the expected behavior shown in the previous section.

2. Handle the entire <inertial> block similarly to <visual>/ <collision>, i.e.: the content of the element will be inserted into each inertial (expected only one) of the SDFormat link. This is described below by using existing behavior for the <visual> element to insert the fluid added mass content into a link's visual element.

Original "test.urdf" example using "visual" capabilities

<robot name="added_mass_example">
  <link name="base_link">
    <inertial name="inertial">
      <mass value="1"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
    <visual>
      <geometry>
        <box size="1.0 1.0 1.0"/>
      </geometry>
    </visual>
  </link>
  <gazebo reference="base_link">
    <visual>
      <fluid_added_mass>
        <xx>1</xx>
        <xy>0</xy>
        <xz>0</xz>
        <xp>0</xp>
        <xq>0</xq>
        <xr>0</xr>
        <yy>1</yy>
        <yz>0</yz>
        <yp>0</yp>
        <yq>0</yq>
        <yr>0</yr>
        <zz>1</zz>
        <zp>0</zp>
        <zq>0</zq>
        <zr>0</zr>
        <pp>1</pp>
        <pq>0</pq>
        <pr>0</pr>
        <qq>1</qq>
        <qr>0</qr>
        <rr>1</rr>
      </fluid_added_mass>
    </visual>
  </gazebo>
</robot>

SDF result, inserting the desired fluid added mass into "visual". Behavior to be replicated for "inertial"

$ gz sdf -p test.urdf
Warning [Utils.cc:132] [/sdf/model[@name="added_mass_example"]/link[@name="base_link"]/visual[@name="base_link_visual"]/fluid_added_mass:/home/ksim_harmonic/dev/ksim_harmonic/sdformat/test.urdf:L0]: XML Element[fluid_added_mass], child of element[visual], not defined in SDF. Copying[fluid_added_mass] as children of [visual].
<sdf version='1.11'>
  <model name='added_mass_example'>
    <link name='base_link'>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>1</mass>
        <inertia>
          <ixx>1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>1</iyy>
          <iyz>0</iyz>
          <izz>1</izz>
        </inertia>
      </inertial>
      <visual name='base_link_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
        <fluid_added_mass>
          <xx>1</xx>
          <xy>0</xy>
          <xz>0</xz>
          <xp>0</xp>
          <xq>0</xq>
          <xr>0</xr>
          <yy>1</yy>
          <yz>0</yz>
          <yp>0</yp>
          <yq>0</yq>
          <yr>0</yr>
          <zz>1</zz>
          <zp>0</zp>
          <zq>0</zq>
          <zr>0</zr>
          <pp>1</pp>
          <pq>0</pq>
          <pr>0</pr>
          <qq>1</qq>
          <qr>0</qr>
          <rr>1</rr>
        </fluid_added_mass>
      </visual>
    </link>

Implementation suggestion

As the URDF specification does not define fluid added mass matrix, this inertial property is only necessary for Gazebo and the CreateInertial method would be kept the same.

sdformat/src/parser_urdf.cc

Line 2970 in f360776

void CreateInertial(tinyxml2::XMLElement *_elem,

Depending on the preferred alternative, update ParseSDFExtension to either include <fluid_added_mass> directly into SDF <inertial> or change the whole inertial behavior to be such as <<visual> which might require the creation of a InsertSDFExtensionInertial similar to InsertSDFExtensionCollision and InsertSDFExtensionVisual.

sdformat/src/parser_urdf.cc

Line 1273 in f360776

void URDF2SDF::ParseSDFExtension(tinyxml2::XMLDocument &_urdfXml)

Additional context

If there is a clear solution path which would best fit the package design, I would be more than happy to contribute with a PR.

[gabrielfpacheco]

Friendly ping :)

@azeey do you have any thoughts on this?

[azeey]

Hi @gabrielfpacheco, we're pretty tied up with the Gazebo Ionic feature freeze, so I won't be able to give you much guidance at the moment. My only thought is if it would be possible to use "blob insertion" for this (http://sdformat.org/tutorials?tut=sdformat_urdf_extensions&cat=specification&#gazebo-elements-for-links)

Any tag that is not listed in the table below will be directly inserted into the corresponding element in the SDFormat output. This direct insertion is sometimes referred to as blob insertion.

So would this work?

<?xml version="1.0" ?>
<robot name="added_mass_example">
  <link name="base_link">
  </link>
  <gazebo reference="base_link">
    <inertial>
      <pose>1 2 3   -0.1 0.2 -0.3</pose>
      <mass>1</mass>
      <inertia>...</inertia>

      <fluid_added_mass>
      ...
      </fluid_added_mass>
    </inertial>
  </gazebo>
</robot>

[gabrielfpacheco]

Thank you for your response, especially considering the high workload related to the Gazebo Ionic feature freeze.

So would this work?

I have previously tried this approach, but <inertial> is a required element for a link during URDF validation:

$ gz sdf -p added_mass_example.urdf 
Warning [parser_urdf.cc:2774] Error Code 19: Msg: urdf2sdf: link[base_link] has no <inertial> block defined. Please ensure this link has a valid mass to prevent any missing links or joints in the resulting SDF model.
Warning [parser_urdf.cc:2777] Error Code 19: Msg: urdf2sdf: link[base_link] is not modeled in sdf.<sdf version='1.11'>
  <model name='added_mass_example'/>
</sdf>

While blob insertion works with the visual and collision tags by merging their contents into existing visual/collision tags, it does not work similarly for inertial. Instead of merging, it generates a second (undesirable) inertial element. For instance:

<?xml version="1.0" ?>
<robot name="added_mass_example">
  <link name="base_link">
    <inertial name="inertial">
      <origin xyz="1 2 3" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
  </link>
  <gazebo reference="base_link">
    <inertial>
      <fluid_added_mass>
        <xx>1</xx>
        <yy>1</yy>
        <zz>1</zz>
        <pp>1</pp>
        <qq>1</qq>
        <rr>1</rr>
      </fluid_added_mass>
    </inertial>
  </gazebo>
</robot>

Running gz sdf -p added_mass_example.urdf results in:

<sdf version='1.11'>
  <model name='added_mass_example'>
    <link name='base_link'>
      <inertial>
        <pose>1 2 3 0 0 0</pose>
        <mass>1</mass>
        <inertia>
          <ixx>1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>1</iyy>
          <iyz>0</iyz>
          <izz>1</izz>
        </inertia>
      </inertial>
      <inertial>
        <fluid_added_mass>
          <xx>1</xx>
          <yy>1</yy>
          <zz>1</zz>
          <pp>1</pp>
          <qq>1</qq>
          <rr>1</rr>
        </fluid_added_mass>
      </inertial>
    </link>
  </model>
</sdf>