Issues related to the imuBosch_BNO055

[traversaro]

I found several issues related to the imuBosch_BNO055 driver, but given that they are touching different parts of the code I prefer to document all of them here before actually try to fixing something.

BNO055 datasheet: https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST_BNO055_DS000_14.pdf .

Quaternion ordering

The quaternion is published by the sensor using the real/imaginary serialization (see section 4.3.33 of the datashet) , but is then saved by the driver in a 4 element vector using the imaginary/real serialization (see [1]). The same vector is then passed to the quat2dcm, but that function is expecting quaternion ordered with the real/imaginary serialization.

Proposed solution

Store quaternion directly with real/imaginary serialization, as provided by the device.

Conversion from quaternion to RPY

The conversion from quaternion to RPY is done using the quat2dcm [2] method, that however has been debugged to be using a convention different from the one used in this IMU sensors and in robotics (see https://github.com/robotology/yarp/issues/786).

Proposed solution

As in https://github.com/robotology/yarp/issues/786 , implement the quat2rotm method that follows the robotics convention and use that one.

Conversion from RPY back to quaternion in ServerInertial

In ServerInertial, the RPY readed from the IMU is converted back to a quaternion for publishing it on a ROS topic. However the used function is defined in [3] , but I did some tests and is not consistent at all with the RPY returned by dcm2rpy(quat2rotm(quat) (where quat2rotm is used for the problems discussed in https://github.com/robotology/yarp/issues/786). However that output was validated by @barbalberto , so I wonder if there is a coupled bug between this function and the previous bugs that cancel one another. Where did you took that function?

Proposed solution

Copy the implementation of rotm2quat(rpy2dcm(rpy) in a YARP_dev friendly header, or implement a consistent function.

References

[1] : https://github.com/robotology/yarp/blob/75c52edc335c7cbf3f9a0075bf105904fb6315d6/src/modules/imuBosch_BNO055/imuBosch_BNO055.cpp#L562 [2] : https://github.com/robotology/yarp/blob/75c52edc335c7cbf3f9a0075bf105904fb6315d6/src/modules/imuBosch_BNO055/imuBosch_BNO055.cpp#L568 [3] : https://github.com/robotology/yarp/blob/master/src/libYARP_dev/src/modules/msgs/ros/include/yarpRosHelper.h#L72

[traversaro]

cc @DanielePucci

[traversaro]

I added the test for consistency of the conversions between quat and rpy in YARP, KDL and iDynTree in https://github.com/robotology/idyntree/commit/84a5269315338e0674a6ea9bac678fb9ecf94a12 . Clearly I commented out the test relative to convertEulerAngleYXZrads_to_quaternion for the reason discussed in this issue.

[randaz81]

pull request #959 should fix points 1 and 2, i.e:

• Quaternion ordering
• Conversion from quaternion to RPY

Without the real device I'm currently unable to check point 3, i.e. Conversion from RPY back to quaternion in ServerInertial

[randaz81]

Probably fixed. Feel free to reopen if needed.