Handle software modifications necessary to support the bno055 IMU mounted on the RFE (Robot Face Expression) board

[traversaro]

More details will come in the following.

[traversaro]

Similar issue from the past: #285 .

[traversaro]

@Nicogene is now working on this issue, that has some non-trivial ramification .

[marcoaccame]

[Nicogene]

I started working on iKinGazeCtrl (#592) , but in order to use the bno055 mounted on the rfe board we need to add a new version of iCubInertialSensor in iKin

This is the transformation between the "old" imu bno055 and the one mounted on the rfe board

@pattacini it is ok for you if the new version of iCubInertialSensor is 2.6?

[pattacini]

@Nicogene @traversaro
Is this new IMU going to replace the standard one in the head or represent only a further added device?

• In the first case, we can certainly update iKin with the new version.
• In the second case, I would rather keep iKin unchanged and let the downstream modules build the proper chain to use the new device internally, as iKin is meant to expose only the kinematics of the main devices.

⚠️ If the first case holds, then we need to be careful: we have already version 2.5 and 3.0. Is it correct to term the proposed version 2.6? Moreover, I would compact the matrix HN in one shot instead of accumulating transformations.

[Nicogene]

Is this new IMU going to replace the standard one in the head or represent only a further added device?

Good question, actually a couple of icub mount this new board(rfe).
I am not sure (maybe @marcoaccame knows better) but, one day, it could replace the imus attached via serial/i2c to the icub-head.

In case we proceed for the second option, the idea is to add an option like --useRFE in order to handle the required transformation in the software downstream?

[traversaro]

This is the transformation between the "old" imu bno055 and the one mounted on the rfe board

A small suggestion: phrases such as "transformation between" are typically ambiguous, and whenever you have some screenshot, for future reference is always better to include it unedited, especially as the full screenshot in this case provide you with a lot of context info:

I am not sure (maybe @marcoaccame knows better) but, one day, it could replace the imus attached via serial/i2c to the icub-head.

According to @maggia80 (verbal communication) ideally in the future we would like to ship iCub heads with just the RFE, so the idea is that indeed it could replace the imus attached via serial/i2c.

[pattacini]

Ok guys, I've made the requested changes in the referenced PR.
@Nicogene you ought to rebase your development branch on iKinGazeCtrl onto this PR.

Keep in mind that I'll be off at least for one week 🏖

[Nicogene]

Thanks @pattacini , I will rebase onto it, test on iCubGenova02 and then merge your PR.

[Nicogene]

The translation column in which measurement unit is expressed?

[traversaro]

The translation column in which measurement unit is expressed?

In CREO it is millimeters, in iKin meters.

[Nicogene]

Today I compared the dynamics of the imu mounted on the rfe board with the imu Bosch attached to the pc104 of iCubGenova02.
I used this simple script bash for dumping the data:

#!/bin/bash

configure_vel() {
    echo "set vel $1 $2" | yarp rpc /$3/head/rpc:i
    echo "- velocities configured for joint $1 to $2 deg/s"
    sleep 1.0
}

home() {
    echo "-- homing"
    echo "set pos 0 0" | yarp rpc /$1/head/rpc:i
    sleep 0.5
    echo "set pos 1 0" | yarp rpc /$1/head/rpc:i
    sleep 0.5
    echo "set pos 2 0" | yarp rpc /$1/head/rpc:i
    sleep 2.0
}

move() {
    echo "-- moving joint $1"
    echo "set pos $1 $2" | yarp rpc /$3/head/rpc:i
    sleep 4.0
}

cleanup() {
    killall yarpdatadumper
}

run_test () {
    echo "Running test with deltaAng = $2 and vel = $3 joint = $4"
    if [ -d deltaAng$2 ]; then
      cd deltaAng$2
    else
      mkdir deltaAng$2 && cd deltaAng$2
    fi

    if [ -d vel$3joint$4 ]; then
      cd vel$3joint$4
    else
      mkdir vel$3joint$4 && cd vel$3joint$4
    fi

    configure_vel $4 $3 $1
    home $1
    echo "Start recording..."
    yarpdatadumper --connect /$1/head/state:o --name pos &>/dev/null &
    yarpdatadumper --connect /$1/inertial --name imu &>/dev/null &
    yarpdatadumper --connect /$1/head/imurfe/measures:o --name imuMAS &>/dev/null &
    sleep 1.0
    move $4 $2 $1

    echo "Closing dumpers"
    killall yarpdatadumper
    sleep 1.0
    home $1
    sleep 2.0
    cd ../../
}

if [ $1 == '--help' ]; then
  echo "Usage"
  echo "testImuMAS.sh <robot_name> <deltaAng> <velocity>"
  exit 0
fi

if [ "$#" -ne 3 ]; then
  echo "Illegal number of parameters, 3 needed"
  echo "Usage"
  echo "testImuMAS.sh <robot_name> <deltaAng> <velocity>"
  exit 1
fi

echo "Setup of the environment"
robot_name=$1
test_dir="TestIMU$(date +'%Y%m%d')"
echo "Test dir ${test_dir}"
cd $HOME

if [ -d $test_dir ]; then
  cd $test_dir
else
  mkdir $test_dir &&  cd $test_dir
fi

cleanup

sleep 0.3

run_test $robot_name $2 $3 0
run_test $robot_name $2 $3 1
run_test $robot_name $2 $3 2

cleanup

echo "Test finished"

The first thing I noticed was that the data coming from the imu mounted on the rfe was more constant.

See this picture as example, from the top the second plot is the data of the old imu, the third is the data coming from the one mounted on rfe:

I investigated and I discovered that the period of the imu on the rfe was 50ms instead of 10ms (see here).

@marcoaccame why it has been set to 50? The param acquisitionRate is it actually acquisitionPeriod and it is expressed in ms?

Changing the period to 10 ms the dynamic improved:

Finally I checked that the rotation matrix introduced in #594 was correct multiplying the data from the imurfe and comparing with the old one.

And as you can see the transformation is correct.

Using the imurfe the movement of the neck using the iKinGazeCtrl were fine.
I experienced issues in eyes movements probably because of this.

cc @pattacini @traversaro

[Nicogene]

The issues of the eyes has been fixed by b216c5b . The iKinGazeCtrl works also with the imurfe

[Nicogene]

This is blocked by #595

[pattacini]

Nice verification @Nicogene 👍

Yep, the rate the IMU uses to stream data needs to be as high as possible, thus 10 ms is a more appropriate rate than 50 ms.

[Nicogene]

Another related step : robotology/robots-configuration#116

[Nicogene]

Yet Another Related Step: #596

[Nicogene]

Other ramifications: #605, robotology/yarp#2098

[Nicogene]

Other ramification robotology/gazebo-yarp-plugins#443

[Nicogene]

It seems that all the software modifications and tests have been done.
This has been a long trip, but finally it ended 😄
Thanks to everyone who helped.

Closing.

[pattacini]

We landed! ✈️