Fix Inaccuracy of Body/Link View

[Nbot07]

The roll pitch and yaw positions and possibly other values are sometimes inaccurate in the Body/Link view. It seems that the conversion from axis angle format to Euler angles is inaccurate in certain cases.

[s-nakaoka]

Really?
Can you show me some examples where the values are inaccurate.

[Nbot07]

Here are some examples:
Category : Expected : Actual (x, y, z) or (roll, pitch, yaw)
Translation : 0 0 0 : 0 0 0.16
Translation: -98 15 43 : -98 15 43.16
Translation: -145 25 2 : -99.99 25 2.16
Translation: -135 37 87 : -99.99 37 87.16
Translation: -170 5 125 : -99.99 5 99.99
Rotation: 180 0 -90 : 180 0 90
Rotation: 180 0 -80 : 180 0 80
Rotation: 180 0 95 : 180 0 -95
Rotation: -180 -10 -90 : -170 0 90
Rotation: 0 10 90 : 10 0 90
Rotation: -180 0 -90 : 180 0 90
Rotation: -180 0 -90 : -180 0 90

[s-nakaoka]

The range of the translation value in the Body/Link view is limited to the range from -99.9999 to 99.9999. If the actual value is over the range, the value is shown as the minimu or maximum value within the range.
However, I'm not sure why the values below the decimal point are not shown. It is set to show up to four digits after the decimal point and it is actually shown in my environment.

As for the rotation values, I think that the kind of value you think as roll pitch yaw is different from the one we employ. In Choreonoid, Z-Y-X Euler Angles is used.

[Nbot07]

I rounded to two decimal places to make posting the values easier in the message. All four decimal places show for me as well. Why does the rootLink set at 0 0 0 have an offset of ~ 0.16 in the z axis?

Also, about my confusion regarding euler angles and row pitch and yaw, would you mind explaining how a row pitch and yaw input of -180 -10 -90 results in the euler representation of -170 0 90? Likewise, there appears to be something similar happening with the input 0 10 90 and the output 10 0 90. In addition, I am particularly concerned that the same input -180 0 -90 had two outputs {180 0 90, -180 0 90}.

[s-nakaoka]

Why does the rootLink set at 0 0 0 have an offset of ~ 0.16 in the z axis?

How did you set the translation of the root link before checking it in the Body/Link view?

would you mind explaining how a row pitch and yaw input of -180 -10 -90 results in the euler representation of -170 0 90?

Choreonoid's Python console view can be used for calculating matrices of Euler angles.
For example, please input the following code in the console view:

from cnoid.Util import *
from math import *
angleAxis(radians(90), [0,0,1]).dot(angleAxis(radians(0), [0,1,0])).dot(angleAxis(radians(-170), [1,0,0]))

This code calculates the rotation matrix of (-170, 0, 90) in Z-Y-X Euler angles. The result is:

array([[  6.12323400e-17,   9.84807753e-01,  -1.73648178e-01],
       [  1.00000000e+00,  -6.03020831e-17,   1.06328842e-17],
       [  0.00000000e+00,  -1.73648178e-01,  -9.84807753e-01]])

This is the rotation matrix in the roll-pitch-yaw expression.

Then, please input the following expression:

angleAxis(radians(-180), [1,0,0]).dot(angleAxis(radians(-10), [0,1,0])).dot(angleAxis(radians(-90), [0,0,1]))

This is the rotation matrix of (-180, -10, -90) in X-Y-Z Euler angles. The result is:

array([[  6.03020831e-17,   9.84807753e-01,  -1.73648178e-01],
       [  1.00000000e+00,  -3.99665715e-17,   1.20604166e-16],
       [  1.11831796e-16,  -1.73648178e-01,  -9.84807753e-01]])

This is almost same as the former result. Very small differences are just the errors in numerical computation.

I am particularly concerned that the same input -180 0 -90 had two outputs {180 0 90, -180 0 90}.

In Choreonoid, the angle values of the roll-pitch-yaw expression is limited to the range from -180 degree to +180 degree. The above difference can be caused by very subtle numerical value differences in the input matrix elements.

[Nbot07]

Thank you for the information.
To set the rootLink I created a model file and typed in [0, 0, 0] as the translation of the rootLink.

[Nbot07]

The rootPosition field in the data section of my .cnoid file was set to [0, 0, 0.1605] and by changing that field to [0, 0, 0] choreonoid now has 0 0 0 displayed in the Body/Link view for the rootLink. So, it seems that the rootPosition field of the data field of the cnoid file overwrites the value set in the translation field of the rootLink in the .body file.