Account for part/link rotation when saving inertia matrix

[traversaro]

This is a completely blind attempt to solve icub-tech-iit/ergocub-software#224 . I did not tested it and not even compile it as I do not have Creo at the moment on my PC, but I guess it was the most efficient way to suggest a fix.

As I need to do some matrix computations I explicitly added the Eigen3 dependency, that however was still necessary as a workaround for a CMake/iDynTree bug robotology/idyntree#1065 .

[Nicogene]

Thanks @traversaro for submitting the fix!

I think that your reasoning is right, I think that all measures taken in Creo are related to the CSYS, the default coordinate system that unfortunately is not the link frame.

I substituted in your changes root_H_link with csys_H_link in this commit;

• d534b42 (#83)

And I tested both the solutions

root_H_link

csys_H_link

The patch seems to fix

• Incorrect inertias generated in the ergoCubSN001 model ergocub-software#224

The only difference is strangely only for the index fingers both right and left, in this sense csys_H_link solution seems fit more.

@mfussi66 have you any opinion on this solution?

cc @giotherobot

[traversaro]

The thing that surprises me is that know we are also changing the translation of the com with the translation of root_H_csys, and that is quite surprising as I would expect that offset to be non-negligibile, but perhaps this is my wrong assumptions. Do you have some way to print root_H_csys, for example on a file?

[traversaro]

I think that your reasoning is right, I think that all measures taken in Creo are related to the CSYS, the default coordinate system that unfortunately is not the link frame.

There is any synonym of CSYS that we could use in the code? Also the link and the root are kind of coordinate systems (i.e. CSYS), so it may be confusing. Could we use partCsys or defaultPartCsys` or something similar?

[Nicogene]

The thing that surprises me is that know we are also changing the translation of the com with the translation of root_H_csys, and that is quite surprising as I would expect that offset to be non-negligibile, but perhaps this is my wrong assumptions. Do you have some way to print root_H_csys, for example on a file?

We just realized that root_H_csys (yes the name is confusing) have all zeros in the translation:

���Link SIM_ECUB_1-1_ROOT_LINK root_H_csys 0 0 0
���Link SIM_ECUB_1-1_TORSO_1 root_H_csys 2.2389e-18 5.55112e-20 -1.11022e-16
���Link SIM_ECUB_1-1_TORSO_2 root_H_csys 2.2389e-18 5.55112e-20 -1.07465e-16
���Link SIM_ECUB_1-1_CHEST root_H_csys 2.07559e-18 -5.81892e-21 -4.16334e-17
���Link SIM_ECUB_1-1_L_SHOULDER_1 root_H_csys 2.77556e-17 -4.16334e-17 -1.38778e-16
���Link SIM_ECUB_1-1_R_SHOULDER_1 root_H_csys 6.59195e-17 -1.38778e-17 -2.77556e-17
���Link SIM_ECUB_1-1_L_SHOULDER_2 root_H_csys -1.35724e-05 4.67557e-05 -1.13886e-05
���Link SIM_ECUB_1-1_R_SHOULDER_2 root_H_csys 1.35724e-05 4.67557e-05 1.13886e-05
���Link SIM_ECUB_1-1_R_SHOULDER_3 root_H_csys 1.35724e-05 4.67557e-05 1.13886e-05
���Link SIM_ECUB_1-1_L_SHOULDER_3 root_H_csys -1.35724e-05 4.67557e-05 -1.13886e-05
���Link SIM_ECUB_1-1_L_UPPERARM root_H_csys -1.35724e-05 4.67557e-05 -1.13886e-05
���Link SIM_ECUB_1-1_HEAD_NECK_2 root_H_csys 8.67362e-19 1.16673e-20 0
���Link SIM_ECUB_1-1_HEAD_NECK_3 root_H_csys -8.67362e-19 -6.43382e-17 2.22045e-16
���Link SIM_ECUB_1-1_HEAD root_H_csys 4.25256e-11 -7.88184e-17 -5.60829e-13
���Link SIM_ECUB_1-1_REALSENSE root_H_csys 3.46945e-16 -3.5141e-16 2.77556e-16
���Link SIM_ECUB_1-1_L_FOREARM root_H_csys -1.35724e-05 4.67557e-05 -1.13886e-05
���Link SIM_ECUB_1-1_L_WRIST_1 root_H_csys -1.92237e-11 -6.43913e-12 -3.49162e-12
���Link SIM_ECUB_1-1_L_WRIST_2 root_H_csys -1.92237e-11 -6.43932e-12 -3.49165e-12
���Link SIM_ECUB_1-1_L_HAND_PALM root_H_csys -1.92236e-11 -6.43915e-12 -3.49165e-12
���Link SIM_ECUB_1-1_L_HAND_THUMB_1 root_H_csys 1.78427e-11 1.14415e-11 1.03162e-12
���Link SIM_ECUB_1-1_L_HAND_THUMB_2 root_H_csys -1.77503e-11 -3.68799e-12 1.17814e-11
���Link SIM_ECUB_1-1_L_HAND_THUMB_3 root_H_csys -2.1594e-11 -1.36274e-11 1.16008e-11
���Link SIM_ECUB_1-1_L_HAND_INDEX_1 root_H_csys -1.94286e-11 -9.18154e-12 2.33752e-12
���Link SIM_ECUB_1-1_L_HAND_INDEX_2 root_H_csys -0.00658656 -1.04271e-11 -0.00124007
���Link SIM_ECUB_1-1_L_HAND_INDEX_3 root_H_csys -0.014903 1.56153e-11 -0.000365977
���Link SIM_ECUB_1-1_L_HAND_MIDDLE_1 root_H_csys -1.92633e-11 -9.92312e-12 -3.43947e-12
���Link SIM_ECUB_1-1_L_HAND_MIDDLE_2 root_H_csys -3.4743e-11 -1.1254e-11 -4.23273e-12
���Link SIM_ECUB_1-1_L_HAND_RING_1 root_H_csys -1.91304e-11 -8.5954e-12 -5.1209e-12
���Link SIM_ECUB_1-1_L_HAND_RING_2 root_H_csys -3.41941e-11 -9.36273e-12 -4.34558e-12
���Link SIM_ECUB_1-1_L_HAND_PINKIE_1 root_H_csys -1.91272e-11 -7.87195e-12 -5.10386e-12
���Link SIM_ECUB_1-1_L_HAND_PINKIE_2 root_H_csys -3.32153e-11 -8.52576e-12 -2.06107e-12
���Link SIM_ECUB_1-1_R_UPPERARM root_H_csys 1.35724e-05 4.67557e-05 1.13886e-05
���Link SIM_ECUB_1-1_R_FOREARM root_H_csys 1.35724e-05 4.67557e-05 1.13886e-05
���Link SIM_ECUB_1-1_R_WRIST_1 root_H_csys -1.92146e-11 6.4295e-12 -3.49215e-12
���Link SIM_ECUB_1-1_R_WRIST_2 root_H_csys -1.92148e-11 6.42925e-12 -3.49226e-12
���Link SIM_ECUB_1-1_R_HAND_PALM root_H_csys -1.92147e-11 6.42925e-12 -3.49229e-12
���Link SIM_ECUB_1-1_R_HAND_THUMB_1 root_H_csys -1.95636e-11 8.12556e-12 1.02873e-12
���Link SIM_ECUB_1-1_R_HAND_THUMB_2 root_H_csys 1.77435e-11 3.68772e-12 -1.17694e-11
���Link SIM_ECUB_1-1_R_HAND_THUMB_3 root_H_csys -2.1586e-11 -1.36229e-11 -1.15895e-11
���Link SIM_ECUB_1-1_R_HAND_INDEX_1 root_H_csys 2.7211e-11 -9.1698e-12 2.17693e-12
���Link SIM_ECUB_1-1_R_HAND_INDEX_2 root_H_csys 0.00658656 -1.04145e-11 -0.00124007
���Link SIM_ECUB_1-1_R_HAND_INDEX_3 root_H_csys 0.014903 -1.17545e-11 -0.000365977
���Link SIM_ECUB_1-1_R_HAND_MIDDLE_1 root_H_csys 1.92544e-11 -9.90816e-12 -3.44536e-12
���Link SIM_ECUB_1-1_R_HAND_MIDDLE_2 root_H_csys 3.47281e-11 -1.1161e-11 -4.43462e-12
���Link SIM_ECUB_1-1_R_HAND_RING_1 root_H_csys 1.91214e-11 -8.58225e-12 -5.12074e-12
���Link SIM_ECUB_1-1_R_HAND_RING_2 root_H_csys 3.41787e-11 -9.34919e-12 -4.34419e-12
���Link SIM_ECUB_1-1_R_HAND_PINKIE_1 root_H_csys 2.93523e-11 -7.60028e-12 -2.0971e-12
���Link SIM_ECUB_1-1_R_HAND_PINKIE_2 root_H_csys 3.32004e-11 -8.50567e-12 -2.06868e-12
���Link SIM_ECUB_1-1_R_HIP_1 root_H_csys -3.46945e-18 0 0
���Link SIM_ECUB_1-1_R_HIP_2 root_H_csys 7.66748e-16 0 -4.02456e-15
���Link SIM_ECUB_1-1_R_HIP_3 root_H_csys -5e-05 1.38778e-17 -9.99201e-16
���Link SIM_ECUB_1-1_R_UPPER_LEG root_H_csys -5e-05 6.0966e-11 -8.88178e-16
���Link SIM_ECUB_1-1_R_LOWER_LEG root_H_csys 4.57273e-15 -6.03684e-15 -5.55112e-16
���Link SIM_ECUB_1-1_R_ANKLE_1 root_H_csys -5e-05 -1.23512e-15 -3.33067e-16
���Link SIM_ECUB_1-1_R_ANKLE_2 root_H_csys -5e-05 -1.26288e-15 -6.66134e-16
���Link SIM_ECUB_1-1_R_FOOT_FRONT root_H_csys 1.35586e-14 -1.9762e-14 1.55431e-15
���Link SIM_ECUB_1-1_R_FOOT_REAR root_H_csys 1.35898e-14 -1.99285e-14 -1.11022e-15
���Link SIM_ECUB_1-1_L_HIP_1 root_H_csys 0 -1.38778e-17 -2.77556e-17
���Link SIM_ECUB_1-1_L_HIP_2 root_H_csys 0 -1.38778e-17 2.77556e-17
���Link SIM_ECUB_1-1_L_HIP_3 root_H_csys -5.37764e-17 2.77556e-17 5.55112e-17
���Link SIM_ECUB_1-1_L_UPPER_LEG root_H_csys -1.38778e-17 1.38778e-17 0
���Link SIM_ECUB_1-1_L_LOWER_LEG root_H_csys -1.73472e-17 2.498e-16 -4.44089e-16
���Link SIM_ECUB_1-1_L_ANKLE_1 root_H_csys -2.42861e-17 2.91434e-16 -5.55112e-16
���Link SIM_ECUB_1-1_L_ANKLE_2 root_H_csys -1.17961e-16 2.91434e-16 -4.44089e-16
���Link SIM_ECUB_1-1_L_FOOT_FRONT root_H_csys 1.44329e-15 4.02456e-16 -3.33067e-16
���Link SIM_ECUB_1-1_L_FOOT_REAR root_H_csys 1.43635e-15 3.747e-16 -4.44089e-16

[Nicogene]

I think that your reasoning is right, I think that all measures taken in Creo are related to the CSYS, the default coordinate system that unfortunately is not the link frame.

There is any synonym of CSYS that we could use in the code? Also the link and the root are kind of coordinate systems (i.e. CSYS), so it may be confusing. Could we use partCsys or defaultPartCsys` or something similar?

I updated the comment and change some variable names, awaiting @mfussi66 review

[mfussi66]

That seems fine to me 👍