search

gazebosim / sdformat

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

Support for <fluid_added_mass> definition in URDF files #1467

Open gabrielfpacheco opened 3 months ago

gabrielfpacheco commented 3 months ago

After https://github.com/gazebosim/sdformat/pull/1077, SDFormat v1.10 introduced 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 ```xml 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1 ```
SDF result, inserting the desired fluid added mass into "visual". Behavior to be replicated for "inertial" ```console $ 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]. 0 0 0 0 0 0 1 1 0 0 1 0 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1 ```

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.

https://github.com/gazebosim/sdformat/blob/f3607761d3846dba0b660880484ff1e5eb1e9425/src/parser_urdf.cc#L2970

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.

https://github.com/gazebosim/sdformat/blob/f3607761d3846dba0b660880484ff1e5eb1e9425/src/parser_urdf.cc#L1273

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 commented 3 months ago

Friendly ping :)

@azeey do you have any thoughts on this?

azeey commented 3 months ago

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 commented 3 months ago

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>