Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug `MeshInertiaCalculator::CalculateMassProperties()` function #2182

jasmeet0915 · 2023-09-29T17:25:11Z

🦟 Bug fix

Previous Understanding:
The MeshInertiaCalculator.cc computes the inertia w.r.t the mesh origin. This can lead to incorrect inertia values in case the Mesh Origin was not at the Center of Mass as mentioned here.
Since many times users may tend to use meshes with origins, not at the center, this PR adds and uses a function to transform the Inertia Matrix to the COM in such cases.

Actual Scenario:
There was a bug in the center of mass and inertia matrix calculation. This PR fixes bugs in a moment of inertia matrix calculation and adds a test for the inertia calculation of a cylinder model with the origin set not at the center of mass.

Some extra functions were also added according to the previous understanding but are not needed after the bugfix:

CalculateMeshCentroid()
TransformInertiaMatrixToCOM()

Note: I have still kept the functions as a part of this PR as they might come in handy later but they can be removed if required.

Below you can see multiple cylinder meshes with origins set at different places inside or outside the mesh getting similar inertia values:

Test it

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.

… theorem Signed-off-by: Jasmeet Singh <[email protected]>

Signed-off-by: Jasmeet Singh <[email protected]>

…ormation Signed-off-by: Jasmeet Singh <[email protected]>

Signed-off-by: Jasmeet Singh <[email protected]>

codecov · 2023-09-29T22:39:36Z

Codecov Report

Merging #2182 (2a3ffdf) into gz-sim8 (18aa48f) will decrease coverage by 0.19%.
Report is 26 commits behind head on gz-sim8.
The diff coverage is 36.58%.

❗ Current head 2a3ffdf differs from pull request most recent head 7424bab. Consider uploading reports for the commit 7424bab to get more accurate results

@@             Coverage Diff             @@
##           gz-sim8    #2182      +/-   ##
===========================================
- Coverage    66.00%   65.81%   -0.19%     
===========================================
  Files          323      323              
  Lines        30719    30740      +21     
===========================================
- Hits         20275    20233      -42     
- Misses       10444    10507      +63

Files	Coverage Δ
...rc/systems/ackermann_steering/AckermannSteering.hh	`100.00% <ø> (ø)`
src/systems/breadcrumbs/Breadcrumbs.hh	`100.00% <ø> (ø)`
src/systems/buoyancy/Buoyancy.hh	`100.00% <ø> (ø)`
src/systems/detachable_joint/DetachableJoint.hh	`100.00% <ø> (ø)`
src/systems/diff_drive/DiffDrive.hh	`100.00% <ø> (ø)`
src/systems/elevator/Elevator.hh	`100.00% <ø> (ø)`
src/systems/joint_controller/JointController.hh	`100.00% <ø> (ø)`
...int_position_controller/JointPositionController.hh	`100.00% <ø> (ø)`
...stems/joint_state_publisher/JointStatePublisher.hh	`100.00% <ø> (ø)`
...ms/joint_traj_control/JointTrajectoryController.hh	`100.00% <ø> (ø)`
... and 4 more

... and 2 files with indirect coverage changes

azeey · 2023-10-04T20:53:00Z

@jasmeet0915 would it be possible to add a test for this?

Signed-off-by: Jasmeet Singh <[email protected]>

… to 0, added TODO comment Signed-off-by: Jasmeet Singh <[email protected]>

…perties() Signed-off-by: Jasmeet Singh <[email protected]>

Signed-off-by: Jasmeet Singh <[email protected]>

jasmeet0915 · 2023-10-20T09:37:21Z

@azeey I have added a test in a161374

iche033 · 2023-10-27T18:49:07Z

src/MeshInertiaCalculator.hh

+          gz::math::Pose3d &_centreOfMass,
+          gz::math::Pose3d &_inertiaOrigin);


can these be const ref?

Done in 01aa539

Signed-off-by: Jasmeet Singh <[email protected]>

iche033 · 2023-10-30T20:20:44Z

code looks good to me. Maybe @azeey can take a one final look before merging.

scpeters · 2023-10-30T20:46:50Z

is there documentation of the algorithm / integrals used in MeshInertiaCalculator? In particular I'm not sure where the terms in CalculateMassProperties are coming from

jasmeet0915 · 2023-10-31T08:35:05Z

is there documentation of the algorithm / integrals used in MeshInertiaCalculator? In particular I'm not sure where the terms in CalculateMassProperties are coming from

@scpeters The method used was referred from this doc: https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
You can find the documentation of all the integrals in the same.

azeey · 2023-11-08T17:12:10Z

src/MeshInertiaCalculator.cc

@@ -101,12 +107,61 @@ void MeshInertiaCalculator::CalculateMeshCentroid(
  _centreOfMass.SetZ(centroid.Z());
 }

+//////////////////////////////////////////////////
+void MeshInertiaCalculator::TransformInertiaMatrixToCOM(


Since CalculateMeshCentroid and TransformInertiaMatrixToCOM are unused and untested, I think we should remove them. I'd be in favor of adding TransformInertiaMatrixToCOM to gz::math::Inertial with tests.

Removed the unused functions in 7424bab. Would create a PR over at gz-math as soon as possible for the TransformInertiaMatrixToCOM() function.

azeey

Looks good! Thanks for the fix. My only suggestion is we remove unused code.

Signed-off-by: Jasmeet Singh <[email protected]>

jasmeet0915 · 2023-11-13T19:07:28Z

Looks good! Thanks for the fix. My only suggestion is we remove unused code.

@azeey I have removed the unused code in this commit: 7424bab

azeey

Thanks @jasmeet0915!!

github-actions bot added the 🎵 harmonic Gazebo Harmonic label Sep 29, 2023

jasmeet0915 added 5 commits September 30, 2023 02:10

Added function to transform inertia matrix to COM using parallel axis…

9803a6f

… theorem Signed-off-by: Jasmeet Singh <[email protected]>

Corrected mismatch of values in ixz and iyz calculation from integral

63ddac2

Signed-off-by: Jasmeet Singh <[email protected]>

Added doc for TransformInertiaMatrixToCOM() function

1cb4f78

Signed-off-by: Jasmeet Singh <[email protected]>

Added correction for rotational offset of inertia matrix after transf…

8e0741a

…ormation Signed-off-by: Jasmeet Singh <[email protected]>

Completed codecheck

fcda45f

Signed-off-by: Jasmeet Singh <[email protected]>

jasmeet0915 force-pushed the jasmeet/inertia_tensor_transformation branch from 9c0eed4 to fcda45f Compare September 29, 2023 21:02

jasmeet0915 added 5 commits October 20, 2023 14:19

Added cylinder model with origin at the bottom

f6c71db

Signed-off-by: Jasmeet Singh <[email protected]>

Added cylinder with origin at bottom to test world

b02bf99

Signed-off-by: Jasmeet Singh <[email protected]>

Updated transformation function to directly set off diagonal elements…

4d17fcc

… to 0, added TODO comment Signed-off-by: Jasmeet Singh <[email protected]>

Fixed center of mass & inertia matrix calculation in CalculateMassPro…

42b2562

…perties() Signed-off-by: Jasmeet Singh <[email protected]>

Added test for inertia calculation of cylinder with origin at the bottom

a161374

Signed-off-by: Jasmeet Singh <[email protected]>

jasmeet0915 changed the title ~~Added function for Transformation of Inertia Matrix to COM in MeshInertiaCalculator~~ Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug MeshInertiaCalculator::CalculateMassProperties() function Oct 20, 2023

jasmeet0915 added 2 commits October 20, 2023 14:58

Completed codecheck

7795580

Signed-off-by: Jasmeet Singh <[email protected]>

Added newline at end of collada file

d2c780e

Signed-off-by: Jasmeet Singh <[email protected]>

jasmeet0915 marked this pull request as ready for review October 20, 2023 09:30

jasmeet0915 requested a review from mjcarroll as a code owner October 20, 2023 09:30

iche033 reviewed Oct 27, 2023

View reviewed changes

jasmeet0915 added 2 commits October 28, 2023 21:05

Updated input params of TransformInertiaMatrixToCOM() to const ref

01aa539

Signed-off-by: Jasmeet Singh <[email protected]>

Updated tutorial doc

9a7e24d

Signed-off-by: Jasmeet Singh <[email protected]>

jasmeet0915 requested a review from mabelzhang as a code owner October 28, 2023 15:37

mjcarroll requested a review from iche033 October 30, 2023 18:42

iche033 approved these changes Oct 30, 2023

View reviewed changes

azeey reviewed Nov 8, 2023

View reviewed changes

Removed unused functions from the code

7424bab

Signed-off-by: Jasmeet Singh <[email protected]>

azeey approved these changes Nov 13, 2023

View reviewed changes

azeey merged commit 9db8061 into gazebosim:gz-sim8 Nov 13, 2023
3 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug `MeshInertiaCalculator::CalculateMassProperties()` function #2182

Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug `MeshInertiaCalculator::CalculateMassProperties()` function #2182

jasmeet0915 commented Sep 29, 2023 •

edited

Loading

codecov bot commented Sep 29, 2023 •

edited

Loading

azeey commented Oct 4, 2023

jasmeet0915 commented Oct 20, 2023

iche033 Oct 27, 2023

jasmeet0915 Oct 28, 2023

iche033 commented Oct 30, 2023

scpeters commented Oct 30, 2023

jasmeet0915 commented Oct 31, 2023

azeey Nov 8, 2023 •

edited

Loading

jasmeet0915 Nov 13, 2023

azeey left a comment

jasmeet0915 commented Nov 13, 2023

azeey left a comment

		gz::math::Pose3d &_centreOfMass,
		gz::math::Pose3d &_inertiaOrigin);

Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug MeshInertiaCalculator::CalculateMassProperties() function #2182

Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug MeshInertiaCalculator::CalculateMassProperties() function #2182

Conversation

jasmeet0915 commented Sep 29, 2023 • edited Loading

🦟 Bug fix

Test it

Checklist

codecov bot commented Sep 29, 2023 • edited Loading

Codecov Report

azeey commented Oct 4, 2023

jasmeet0915 commented Oct 20, 2023

iche033 Oct 27, 2023

Choose a reason for hiding this comment

jasmeet0915 Oct 28, 2023

Choose a reason for hiding this comment

iche033 commented Oct 30, 2023

scpeters commented Oct 30, 2023

jasmeet0915 commented Oct 31, 2023

azeey Nov 8, 2023 • edited Loading

Choose a reason for hiding this comment

jasmeet0915 Nov 13, 2023

Choose a reason for hiding this comment

azeey left a comment

Choose a reason for hiding this comment

jasmeet0915 commented Nov 13, 2023

azeey left a comment

Choose a reason for hiding this comment

Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug `MeshInertiaCalculator::CalculateMassProperties()` function #2182

Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug `MeshInertiaCalculator::CalculateMassProperties()` function #2182

jasmeet0915 commented Sep 29, 2023 •

edited

Loading

codecov bot commented Sep 29, 2023 •

edited

Loading

azeey Nov 8, 2023 •

edited

Loading