The embObjIMU device output the RPY data in the wrong order

[S-Dafarra]

Lately, I have been investigating the output of the orientations sensors embedded in the FT sensors attached to the robot feet. In particular, I have been using the getOrientationSensorMeasureAsRollPitchYaw method of the MultipleAnalogSensorsInterfaces. By displaying its output using an iCub model, I noticed the following output:

Here you can see two frames attached to the robot: one short and thick that is the one obtained through forward kinematics from the joint values (the robot is on the pole). The other, long and thin, is the one whose orientation is computed from the RPY data following the corresponding YARP documentation. As you can see, the two frames rotate in a completely different way.

By considering the RPY data as Yaw-InvertedPitch-Roll, the result is the following:

(The magenta line is the scaled accelerometer measurements)

This problem does not occur when the IMU is read from i2c for example. I tested the same algorithm also on the head, without applying any shuffling and this is the output:

cc @traversaro @pattacini @DanielePucci

[pattacini]

Thanks @S-Dafarra for reporting 👍🏻
We'll be dealing with this starting the next week.

[Nicogene]

As I was mentioning during the last meeting the way the data is read from the SW(yarp device) and FW(embobjIMU), i.e. they both read 32 bytes starting from the ACC_REG.

• SW:

https://github.com/robotology/yarp/blob/0481f994c6e03897d038c5f1d1078145646a1772/src/devices/imuBosch_BNO055/imuBosch_BNO055.cpp#L631

https://github.com/robotology/yarp/blob/0481f994c6e03897d038c5f1d1078145646a1772/src/devices/imuBosch_BNO055/imuBosch_BNO055.h#L88

• FW:

https://github.com/robotology/icub-firmware/blob/b6c0a716bb75105fd1d62cfce6ae05c7d649df9b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_bno055.cpp#L253-L271

https://github.com/robotology/icub-firmware/blob/b6c0a716bb75105fd1d62cfce6ae05c7d649df9b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_bno055.h#L56-L57

The difference (that I missed, thanks @traversaro for pointing it out) is how the euler angles are retrieved.
In the FW it takes the data as it is, on yarp we get it from the quaternion representation(https://github.com/robotology/yarp/blob/0481f994c6e03897d038c5f1d1078145646a1772/src/devices/imuBosch_BNO055/imuBosch_BNO055.cpp#L647-L676)

This PR robotology/icub-firmware#154 should fix the problem.

cc @marcoaccame @pattacini @S-Dafarra @DanielePucci

[pattacini]

PR merged ✔

[S-Dafarra]

With a lot of delay, I finally managed to test it again on the robot and it works as expected 🎉

Data:
masImuTestOutput.zip