iCub >= V2.6: investigate orientation of the head imu frame

[martinaxgloria]

During the test on the IMU mountend on icub-head, @Nicogene and I found out that head_imu_0, which is the imu frame in the urdf, was not properly oriented. After the additional transformation for iCubs > v2_5 that was added here:

• Include the additionalTransformation when the flag exportFrameInURDF is set to true for the sensors simmechanics-to-urdf#60

we saw that something was already wrong with the orientation data retrieved from the sensor. This is the result of the test on iCubGenova11:

test-icub.webm

We tried to understand if the imu frame is oriented in the same way in both iCub > v2_5 and ergoCub models and so it is, as reported in the figures below.

• ergoCubGazeboV1:

• iCubGazeboV2_7:

However, running the same test for the imu on the head on ergoCub1.1, instead, the result reported below shows that the expected and measured orientation traces follow each other, as expected:

test_imu_ergocub.webm

cc @traversaro

[traversaro]

Could this be something at the firwmare level? The version of the firmware in the related boards of iCubGenova11 and ergoCubSN001 is the same?

[martinaxgloria]

Could this be something at the firwmare level? The version of the firmware in the related boards of iCubGenova11 and ergoCubSN001 is the same?

Both on iCubGenova11 and ergoCub1.1 the rfe firmware version is v1.4.0.

[traversaro]

Did you double checked if the board are mounted in the same way in the two heads?

[Nicogene]

Did you double checked if the board are mounted in the same way in the two heads?

They seems mounted differently on the two robots

ergoCub

iCub

I don't know why we didn't mount it in the same way, maybe @Mick3Lozzo knows it.

Important

We need to re-check the csys of the imu in the iCub simulation model also at iKin level

cc @traversaro @fiorisi @martinaxgloria @pattacini

[traversaro]

Then I guess one of the two models is wrong?

[Nicogene]

Then I guess one of the two models is wrong?

I would say yes because give this mounting in rviz we should see a different frame orientation between ergocub and iCub

[Nicogene]

On ergoCub the cad of real robot and simulation model seems consistent

so it is iCub the faulty one

[martinaxgloria]

I confirm that the two rfe are differently oriented (I was checking with the help of @AntonioConsilvio). I highlighted the two imu frame on the cad and I obtained this:

• ergoCub

• iCub

Making a comparison with what we saw on rviz, it seems that the robot that has a correspondence between the urdf and the cad is ergoCub, right?

cc @Nicogene @traversaro

[Nicogene]

Alongside @Lawproto, we recomputed the transformation between the frame of the "old" mtx to the bno055 of the rfe and here is the result:

SCSYS_HEAD_MTX_IMU -> BN0055_M-FRAME

0.0000000000	1.00000	0.0000000000	6.65401
1.00000	0.0000000000	0.0000000000	4.00169
0.0000000000	0.0000000000	-1.00000	59.2493

Looking at this @traversaro comment

• Handle software modifications necessary to support the bno055 IMU mounted on the RFE (Robot Face Expression) board icub-main#570 (comment)

The only difference is in the traslation, probably in the past we took the wrong starting frame, but the rotations are the same.

Maybe @traversaro we used the wrong euler convention while transforming the rotation matrix to euler angles? I remember we looked which was the correct one to be used, I think we adopted the XYZ

[Nicogene]

In the sdf specification it is stated:

This means that we need transform to euler angles using the sequence ZYX?

Maybe this explain why we have the frame rotated of 180 deg around the z axis?

[traversaro]

To transform from Rotation matrix to URDF's rpy I would just use the related iDynTree::Rotation function, it has been extensively tested as part of URDF Import/Export functionality of iDynTree, example code:

import idytree.bindings as idt
import numpy as np

angle_radians = np.radians(30)

# Create rotation matrix
rotation_matrix_x = np.array([
    [1, 0, 0],
    [0, np.cos(angle_radians), -np.sin(angle_radians)],
    [0, np.sin(angle_radians), np.cos(angle_radians)]
])

rot_idyn = idt.Rotation(rotation_matrix_x).asRPY().toNumPy()

[martinaxgloria]

Hi @traversaro. I followed your suggestion and, starting from the rotation matrix retrieved with CREO (the one posted by @Nicogene a few comments above), I obtained the correspondent RPY orientation.

>>> import idyntree.bindings as idt
>>> import numpy as np
>>> rotMat = np.array([
... [0, 1, 0],
... [1, 0, 0],
... [0, 0, -1]
... ])
>>> rot_eul = idt.Rotation(rotMat).asRPY().toNumPy()
>>> rot_eul
array([ 3.14159265, -0.        ,  1.57079633])

I regenerated the urdf model by inserting this orientation within the CMakeLists.txt and I visualized the transformation on rviz2, which now is the same of the one on the CAD model:

I'm unable to visualize the meshes on rviz, maybe a problem in the model generation, but I checked the orientation with all the TFs enabled (the one on the top is the head_imu_0 frame:

[martinaxgloria]

I tried to do the same as iDynTree but with Matlab to obtain the same Euler angles values and I found out that the used convention is the YXZ:

>> R = [0 1 0; 1 0 0; 0 0 -1]

R =

     0     1     0
     1     0     0
     0     0    -1

>> R_eul = rotm2eul(R, "YXZ")

R_eul =

    3.1416         0    1.5708

maybe, this information could be useful.

cc @Nicogene @traversaro @pattacini

[traversaro]

Just to be sure, I would try this with a couple more rotations to be sure that the two methods are equivalent, otherwise we may be just lucky/unlucky that the two methods match in that specific case but not in general.

[Lawproto]

@Nicogene on a side note, I discovered it is possible for Creo to export a transform matrix between two CSYS's to a .trf text file. Then, use it to define a new CSYS from a starting one.

This way, it is possible to store new CSYS's in our Creo sim_model all the way up to the Pulsantone, avoiding the bloodbath of recreating Simp Reps and low-poly models. It may come in handy for the future.

[martinaxgloria]

Just to be sure, I would try this with a couple more rotations to be sure that the two methods are equivalent, otherwise we may be just lucky/unlucky that the two methods match in that specific case but not in general.

You're right, I've tried with some rotation matrices and they didn't match in all the examined cases. So, we could keep on with the iDynTree::Rotation method to transform the rotation matrix into rpy angles.

cc @Nicogene @traversaro

[martinaxgloria]

Today, @Nicogene and I tested the new head imu frame after robotology/icub-models-generator#258 on iCubGenova11 using the IMU test. This was the result:

imu-test-icubgenova11.mp4

Now the test passes! I think we can close this issue.

cc @traversaro @pattacini

[traversaro]

Now the test passes! I think we can close this issue.

Great! I want to do a quick release of icub-models to release this (fundamental) fix, then I will close it, thanks again!

[traversaro]

Now the test passes! I think we can close this issue.

Great! I want to do a quick release of icub-models to release this (fundamental) fix, then I will close it, thanks again!

Fixed in https://github.com/robotology/icub-models/releases/tag/v2.5.0, thanks to @martinaxgloria and @Nicogene !