Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Print auto inertial values with gz sdf --print --expand-auto-inertials #1422

Merged
merged 8 commits into from
May 24, 2024

Conversation

scpeters
Copy link
Member

@scpeters scpeters commented May 20, 2024

🎉 New feature

Closes #1348

Summary

The ability to automatically compute inertial values by setting the //inertial/@auto attribute to true is very convenient, but it is difficult to see what inertial values were computed. This adds a --expand-auto-inertials argument to gz sdf --print to show the auto-computed inertial values in the printed output.

To enable this behavior, a new enum value SAVE_CALCULATION_IN_ELEMENT is added to ConfigureResolveAutoInertials in ParserConfig.hh, and the Link will store auto-computed inertial values directly in its sdf::Element when this configuration is set.

I am seeing a failing test on macOS locally but want to see if it is an issue on other platforms. EDIT: fixed in 2ffda3d.

Test it

  • Run INTEGRATION_link_dom to verify the functionality of the Link class with SAVE_CALCULATION_IN_ELEMENT
  • Run UNIT_gz_TEST to verify the --expand-auto-inertials parameter to gz sdf --print
  • If you build this in a colcon workspace with gz-tools2, then gz sdf --print test/sdf/inertial_stats_auto.sdf with and without --expand-auto-inertials should demonstrate the feature.

Checklist

  • Signed all commits for DCO
  • Added tests
  • Added example and/or tutorial
  • 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.

This adds a SAVE_CALCULATION_IN_ELEMENT value to the
ConfigureResolveAutoInertials enum, which is similar
to the SAVE_CALCULATION value but also stores the
resulting inertial data in the Link's Element so that
it can be printed.

Signed-off-by: Steve Peters <[email protected]>
If the SAVE_CALCULATION_IN_ELEMENT value is set
in ParserConfig, then store auto-computed inertial
data in the Link's Element. Confirm with a
test added to INTEGRATION_link_dom.

Signed-off-by: Steve Peters <[email protected]>
The --inertial-stats argument represents a command,
so move its help string and change indentation
to match other commands, so that it doesn't look like
a sub-parameter of --print.

Signed-off-by: Steve Peters <[email protected]>
The `--expand-auto-inertials` option to
`gz sdf --print` will print auto-computed
inertial values.

Signed-off-by: Steve Peters <[email protected]>
@scpeters scpeters mentioned this pull request May 21, 2024
8 tasks
" -p [ --print ] arg Print converted arg. Note the quaternion representation of the\n" +
" rotational part of poses and unit vectors will be normalized.\n" +
" -i [ --preserve-includes ] Preserve included tags when printing converted arg (does not preserve merge-includes).\n" +
" --degrees Pose rotation angles are printed in degrees.\n" +
" --expand-auto-inertials Prints auto-computed inertial values.\n" +
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you mention that this won't work for meshes.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

in f32393b I've added a bit to the doc-string for gz sdf --print --expand-auto-inertials that mesh inertial data isn't computed. here's the output I see (with some adjusted whitespace):

$ gz sdf --print test/sdf/mesh_auto_inertial.sdf --expand-auto-inertials
Warning [Utils.cc:115] Collision is missing a <density> child element.
 Using a default density value of 1000.000000 kg/m^3.
Warning [Utils.cc:115] Custom moment of inertia calculator for meshes
 not set via sdf::ParserConfig::RegisterCustomInertiaCalc,
 using default inertial values.
<sdf version='1.12'>
  <model name='test_model'>
    <link name='L1'>
      <inertial auto='true'>
        <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>
      <collision name='collision'>
        <geometry>
          <mesh>
            <uri>box.dae</uri>
          </mesh>
        </geometry>
      </collision>
    </link>
  </model>
</sdf>

Update doc-string for `gz sdf --print --expand-auto-inertials`
to clarify that mesh inertial data is not computed.

Signed-off-by: Steve Peters <[email protected]>
@scpeters scpeters merged commit 3f84210 into main May 24, 2024
10 checks passed
@scpeters scpeters deleted the scpeters/print_auto_inertials branch May 24, 2024 20:24
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
🏛️ ionic Gazebo Ionic
Projects
Archived in project
Development

Successfully merging this pull request may close these issues.

3 participants