Fix initialization of world components when enabling velocity checks #2777

gabrielfpacheco · 2025-02-14T15:19:24Z

🦟 Bug fix

Summary

When using the link API to enable velocity checks for a link that does not yet have the world ECM components (WorldPose, WorldLinearVelocity, WorldAngularVelocity), typically before the physics system plugin does its first update loop, such components will be initialized with their default constructor, zeroing values that might not be zero. One more evident example is the link world pose that could have already been initialized.

Add the same unit test added by this MR to gz-sim/src/Link_TEST.cc
Build the package: colcon build --packages-select gz-sim9
Execute the test from your build folder: ./bin/UNIT_Link_TEST --gtest-filter=*EnableVelocityChecks*
You should verify the tests will fail because all world components are zero

Checklist

Signed all commits for DCO
Added tests
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.

gabrielfpacheco · 2025-02-18T20:45:55Z

Friendly ping :)

@mjcarroll, @scpeters, @azeey. Could any of you guys take a quick look at this PR to see if I'm in the right track?

Signed-off-by: Gabriel Pacheco <[email protected]>

* Not all models of test/worlds/mesh_inertia_calculation.sdf are initialized with zero pose * Bug gazebosim#2774 was forcing all `link.WorldPose` to be zero after calling `link.EnableVelocityChecks` Signed-off-by: Gabriel Pacheco <[email protected]> # Conflicts: # test/integration/mesh_inertia_calculation.cc

codecov · 2025-02-28T20:27:19Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 68.94%. Comparing base (036515f) to head (b928b4a).
Report is 6 commits behind head on gz-sim9.

Additional details and impacted files

@@             Coverage Diff             @@
##           gz-sim9    #2777      +/-   ##
===========================================
+ Coverage    68.93%   68.94%   +0.01%     
===========================================
  Files          345      345              
  Lines        33296    33319      +23     
===========================================
+ Hits         22952    22972      +20     
- Misses       10344    10347       +3

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

scpeters

this makes sense to me; thanks for the contribution!

I just have a few small comments on some test expectations that I think can be removed

scpeters · 2025-02-28T22:05:36Z

test/integration/mesh_inertia_calculation.cc

-  EXPECT_EQ(link.WorldInertialPose(*ecm).value(), gz::math::Pose3d::Zero);
+  // Check the Inertial Pose and Link Pose. Their world poses should be the
+  // same since the inertial pose relative to the link is zero.
+  EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));


Suggested change

EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));

this expectation seems unnecessary based on the comments; I would remove it

scpeters · 2025-02-28T22:06:26Z

test/integration/mesh_inertia_calculation.cc

@@ -219,13 +222,12 @@ void cylinderColladaMeshWithNonCenterOriginInertiaCalculation(
    link.WorldInertiaMatrix(*ecm).value().Equal(inertiaMatrix, 0.005));

  // Check the Inertial Pose and Link Pose
-  EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
+  EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));


Suggested change

EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));

this expectation seems unnecessary based on the comments; I would remove it

scpeters · 2025-02-28T22:07:25Z

test/integration/mesh_inertia_calculation.cc

@@ -318,7 +320,7 @@ TEST(MeshInertiaCalculationTest, CylinderColladaOptimizedMeshInertiaCalculation)
  EXPECT_TRUE(actualIxyxzyz.Equal(
              meshInertial.MassMatrix().OffDiagonalMoments(), 3.5));
  // Check the Inertial Pose and Link Pose
-  EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
+  EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));


Suggested change

EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));

this expectation seems unnecessary based on the comments; I would remove it

scpeters

thanks for the contribution! I'll address my comments in a follow-up PR

Follow-up to #2777. Signed-off-by: Steve Peters <[email protected]>

gabrielfpacheco · 2025-03-06T12:44:42Z

@scpeters, thank you for your review. Do you think it would be possible to backport that change to Harmonic?

scpeters · 2025-03-06T16:58:55Z

https://github.com/Mergifyio backport gz-sim8

mergify · 2025-03-06T16:59:07Z

backport gz-sim8

✅ Backports have been created

#2811 Fix initialization of world components when enabling velocity checks (backport #2777) has been created for branch gz-sim8 but encountered conflicts

…2777) Retain already initialized world component values when enabling velocity checks and adjust expectations accordingly in mesh_inertia_calulation integration tests. Signed-off-by: Gabriel Pacheco <[email protected]> Co-authored-by: Gabriel Pacheco <[email protected]> (cherry picked from commit 4ae75e5) # Conflicts: # test/integration/mesh_inertia_calculation.cc

…2777) Retain already initialized world component values when enabling velocity checks and adjust expectations accordingly in mesh_inertia_calulation integration tests. Signed-off-by: Gabriel Pacheco <[email protected]> Co-authored-by: Gabriel Pacheco <[email protected]> (cherry picked from commit 4ae75e5)

Follow-up to #2777. Signed-off-by: Steve Peters <[email protected]>

gabrielfpacheco requested a review from mjcarroll as a code owner February 14, 2025 15:19

github-actions bot added the 🏛️ ionic Gazebo Ionic label Feb 14, 2025

gabrielfpacheco force-pushed the gp-world_components_fix branch 5 times, most recently from 369ac76 to f7e26fb Compare February 18, 2025 18:42

Gabriel Pacheco and others added 2 commits February 26, 2025 16:15

Fix initialization of world components when enabling velocity checks

0a9563e

Signed-off-by: Gabriel Pacheco <[email protected]>

gabrielfpacheco force-pushed the gp-world_components_fix branch from f7e26fb to b928b4a Compare February 26, 2025 19:33

scpeters reviewed Feb 28, 2025

View reviewed changes

scpeters approved these changes Mar 4, 2025

View reviewed changes

github-project-automation bot moved this from Inbox to In review in Core development Mar 4, 2025

scpeters merged commit 4ae75e5 into gazebosim:gz-sim9 Mar 4, 2025
13 checks passed

github-project-automation bot moved this from In review to Done in Core development Mar 4, 2025

scpeters added a commit that referenced this pull request Mar 4, 2025

Remove unneeded mesh_inertia_calculation test expectations

b3511b2

Follow-up to #2777. Signed-off-by: Steve Peters <[email protected]>

scpeters mentioned this pull request Mar 4, 2025

Remove unneeded mesh_inertia_calculation test expectations #2808

Merged

11 tasks

gabrielfpacheco deleted the gp-world_components_fix branch March 6, 2025 16:20

mergify bot mentioned this pull request Mar 6, 2025

Fix initialization of world components when enabling velocity checks (backport #2777) #2811

Merged

8 tasks

scpeters added a commit that referenced this pull request Apr 4, 2025

Remove unneeded mesh_inertia_calculation test expectations (#2808)

f78df6d

Follow-up to #2777. Signed-off-by: Steve Peters <[email protected]>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Fix initialization of world components when enabling velocity checks #2777

Fix initialization of world components when enabling velocity checks #2777

Uh oh!

gabrielfpacheco commented Feb 14, 2025 •

edited

Loading

Uh oh!

gabrielfpacheco commented Feb 18, 2025

Uh oh!

codecov bot commented Feb 28, 2025 •

edited

Loading

Uh oh!

scpeters left a comment

Uh oh!

scpeters Feb 28, 2025

Uh oh!

scpeters Feb 28, 2025

Uh oh!

scpeters Feb 28, 2025

Uh oh!

scpeters left a comment

Uh oh!

Uh oh!

gabrielfpacheco commented Mar 6, 2025

Uh oh!

scpeters commented Mar 6, 2025

Uh oh!

mergify bot commented Mar 6, 2025 •

edited

Loading

Uh oh!

Uh oh!

Fix initialization of world components when enabling velocity checks #2777

Fix initialization of world components when enabling velocity checks #2777

Uh oh!

Conversation

gabrielfpacheco commented Feb 14, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🦟 Bug fix

Summary

Checklist

Uh oh!

gabrielfpacheco commented Feb 18, 2025

Uh oh!

codecov bot commented Feb 28, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

scpeters left a comment

Choose a reason for hiding this comment

Uh oh!

scpeters Feb 28, 2025

Choose a reason for hiding this comment

Uh oh!

scpeters Feb 28, 2025

Choose a reason for hiding this comment

Uh oh!

scpeters Feb 28, 2025

Choose a reason for hiding this comment

Uh oh!

scpeters left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

gabrielfpacheco commented Mar 6, 2025

Uh oh!

scpeters commented Mar 6, 2025

Uh oh!

mergify bot commented Mar 6, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

✅ Backports have been created

Uh oh!

Uh oh!

gabrielfpacheco commented Feb 14, 2025 •

edited

Loading

codecov bot commented Feb 28, 2025 •

edited

Loading

mergify bot commented Mar 6, 2025 •

edited

Loading