Wrong data streamed by the accelerometer on the iCub3 head

[isorrentino]

Here is what I see logging data received on the yarp port /icubSim/head/inertials.

I expect to see the gravity in the z direction instead of $10^{-4}$.

I tried with two models: iCubGazeboV3_fixed (no hands) and iCubGazeboV3 (no hands).

cc @prashanthr05 @traversaro

[traversaro]

Can you post a few lines of the dump of /icubSim/head/inertials ? For reference, the sensor name in the URDF is head_imu_acc_1x1, see https://github.com/robotology/icub-models/blob/102edcbc141cd024c42165a2adfcf003bc0258b5/iCub/robots/iCubGazeboV3/model.urdf#L1652 .

[isorrentino]

I dumped data by using the YarpRobotLoggerDevice in BipedalLocomotionFramework (https://github.com/ami-iit/bipedal-locomotion-framework/tree/master/devices/YarpRobotLoggerDevice).
Here is the .mat file:
yarprobot_dataset_2021_12_03_17_34_55.zip

The mas remapper is defined in https://github.com/ami-iit/bipedal-locomotion-framework/blob/master/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/interface/mas-remapper.xml and the multipleanalogsensorsclient is https://github.com/ami-iit/bipedal-locomotion-framework/blob/master/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/interface/head_imu.xml

[traversaro]

Can you read/dump at least a few lines of the port that starts with /icubSim/head/inertials with yarp read? So that we can at least understand at which point of the stack the problem is.

[isorrentino]

Here is a dump of yarp read ... /icubSim/head/inertials/measures:o

port.txt

[traversaro]

fyi @gabrielenava that also reported strage results from a gazebo_yarp_imu plugin recently.

[gabrielenava]

@FabioBergonti suggested me to have a look at @prashanthr05 comment in ironcub-estimation repo (https://github.com/ami-iit/element_floating-base-estimation/issues/61#issuecomment-651695040)

We need to understand how the Gazebo IMU considers the sensor transform to generate the measurements. Given the CAD frame associated with the root IMU, the measurements from the Gazebo should reflect the gravity only along the zy-plane, more inclined towards the y-axis.

Related to this,
In simulation, it was noticed that the measurements were not being published in the sensor local frame and it was always published with respect to the inertial frame. This was because the root_link_imu_acc sensor was referenced to root_link which is a massless link attached to the actual link base_link (base_link is the link actually identified by Gazebo). So this had lead to the measurements/orientations being published with respect to the root_link frame which had almost the same orientation as the Gazebo world except for the heading.

This bug was identified and fixed in,
prashanthr05/icub-models@d7e09b4

Now the root_link_imu_acc publishes measurements (accelerometer and gyroscope) in its local frame.

cc @MiladShafiee also faced similar issues in simulation. Seems like the gazebo measurements coming from the port /icubSim/xsens_inertial is wrong.
It streams orientation rpy =0.0, 0.0, 0.0 in the case of static home position, instead of streaming the rootlink_R_imu orientation.

However, the orientation still seems to be a problem.
The documentation here https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1sensors_1_1ImuSensor.html#a66db2fd83d1d832184d888adc367115b says,

ignition::math::Quaterniond Orientation ( ) const
get orientation of the IMU relative to a reference pose Initially, the reference pose is the boot up pose of the IMU, but user can call either SetReferencePose to define current pose as the reference frame, or call SetWorldToReferencePose to define transform from world frame to reference frame.
returns the orientation quaternion of the IMU relative to the imu reference pose.

This might be related to gazebosim/gazebo-classic#1959

I am investigating the comment to understand if this is the problem I am facing

[traversaro]

I have the impression that is related to orientation, while the problem here is on accelerometer and in your case is on angular velocity, but the problem can be related in an indirected way I guess.

[prashanthr05]

The problem reported in the comment #592 (comment) is due to a corner-case unhandled in YarpSensorBridge while dealing with multiple analog sensors' interface inertial sensors.

Apparently, YarpSensorBridge uses a single unordered_map std::unordered_map<std::string, StampedYARPVector> wholeBodyInertialMeasures; to store different sensor measures with unique names which is used when an user calls getLinearAcceleromterMeasurement(), getGyroscopeMeasurement() here.

This problem occurs when we specify the same ids for different sensors as specified here which is the case only for simulation.

This must be fixed in YarpSensorBridge side by handling separate buffers for each sensor type in MAS inertials.

The changes will be,

YarpSensorBridgeImpl

diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h
index 2c2f41af..6cc0f294 100644
--- a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h
+++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h
@@ -158,7 +158,10 @@ struct YarpSensorBridge::Impl
     std::unordered_map<std::string, StampedYARPVector> wholeBodyFTMeasures;
 
     /**< map holding three axis inertial sensor measures */
-    std::unordered_map<std::string, StampedYARPVector> wholeBodyInertialMeasures;
+    std::unordered_map<std::string, StampedYARPVector> wholeBodyInertialAccMeasures;
+    std::unordered_map<std::string, StampedYARPVector> wholeBodyInertialGyroMeasures;
+    std::unordered_map<std::string, StampedYARPVector> wholeBodyInertialOrientMeasures;
+    std::unordered_map<std::string, StampedYARPVector> wholeBodyInertialMagMeasures;
 
     /**< map holding cartesian wrench measures */
     std::unordered_map<std::string, StampedYARPVector> wholeBodyCartesianWrenchMeasures;
@@ -1786,7 +1789,7 @@ struct YarpSensorBridge::Impl
 
         return readAllMASSensors(wholeBodyMASInertialsInterface.accelerometers,
                                  masSensorIndexMaps.accelerometer,
-                                 wholeBodyInertialMeasures,
+                                 wholeBodyInertialAccMeasures,
                                  failedSensorReads,
                                  checkForNAN);
     }
@@ -1801,7 +1804,7 @@ struct YarpSensorBridge::Impl
 
         return readAllMASSensors(wholeBodyMASInertialsInterface.gyroscopes,
                                  masSensorIndexMaps.gyroscopes,
-                                 wholeBodyInertialMeasures,
+                                 wholeBodyInertialGyroMeasures,
                                  failedSensorReads,
                                  checkForNAN);
     }
@@ -1816,7 +1819,7 @@ struct YarpSensorBridge::Impl
 
         return readAllMASSensors(wholeBodyMASInertialsInterface.orientationSensors,
                                  masSensorIndexMaps.orientationSensors,
-                                 wholeBodyInertialMeasures,
+                                 wholeBodyInertialOrientMeasures,
                                  failedSensorReads,
                                  checkForNAN);
     }
@@ -1831,7 +1834,7 @@ struct YarpSensorBridge::Impl
 
         return readAllMASSensors(wholeBodyMASInertialsInterface.magnetometers,
                                  masSensorIndexMaps.magnetometers,
-                                 wholeBodyInertialMeasures,
+                                 wholeBodyInertialMagMeasures,
                                  failedSensorReads,
                                  checkForNAN);
     }

YarpSensorBridge.cpp

diff --git a/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp b/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp
index 44eea181..3720c19b 100644
--- a/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp
+++ b/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp
@@ -466,13 +466,13 @@ bool YarpSensorBridge::getLinearAccelerometerMeasurement(const std::string& accN
                                                          OptionalDoubleRef receiveTimeInSeconds)
 {
     if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getLinearAccelerometerMeasurement ",
-                                          m_pimpl->wholeBodyInertialMeasures,
+                                          m_pimpl->wholeBodyInertialAccMeasures,
                                           accName))
     {
         return false;
     }
 
-    auto iter = m_pimpl->wholeBodyInertialMeasures.find(accName);
+    auto iter = m_pimpl->wholeBodyInertialAccMeasures.find(accName);
     accMeasurement = yarp::eigen::toEigen(iter->second.first);
     if (receiveTimeInSeconds)
         receiveTimeInSeconds.value().get() = iter->second.second;
@@ -484,13 +484,13 @@ bool YarpSensorBridge::getGyroscopeMeasure(const std::string& gyroName,
                                            OptionalDoubleRef receiveTimeInSeconds)
 {
     if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getGyroscopeMeasure ",
-                                          m_pimpl->wholeBodyInertialMeasures,
+                                          m_pimpl->wholeBodyInertialGyroMeasures,
                                           gyroName))
     {
         return false;
     }
 
-    auto iter = m_pimpl->wholeBodyInertialMeasures.find(gyroName);
+    auto iter = m_pimpl->wholeBodyInertialGyroMeasures.find(gyroName);
     gyroMeasurement = yarp::eigen::toEigen(iter->second.first);
     if (receiveTimeInSeconds)
         receiveTimeInSeconds.value().get() = iter->second.second;
@@ -502,13 +502,13 @@ bool YarpSensorBridge::getOrientationSensorMeasurement(const std::string& rpyNam
                                                        OptionalDoubleRef receiveTimeInSeconds)
 {
     if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getOrientationSensorMeasurement ",
-                                          m_pimpl->wholeBodyInertialMeasures,
+                                          m_pimpl->wholeBodyInertialOrientMeasures,
                                           rpyName))
     {
         return false;
     }
 
-    auto iter = m_pimpl->wholeBodyInertialMeasures.find(rpyName);
+    auto iter = m_pimpl->wholeBodyInertialOrientMeasures.find(rpyName);
     rpyMeasurement = yarp::eigen::toEigen(iter->second.first);
     if (receiveTimeInSeconds)
         receiveTimeInSeconds.value().get() = iter->second.second;
@@ -520,13 +520,13 @@ bool YarpSensorBridge::getMagnetometerMeasurement(const std::string& magName,
                                                   OptionalDoubleRef receiveTimeInSeconds)
 {
     if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getMagnetometerMeasurement ",
-                                          m_pimpl->wholeBodyInertialMeasures,
+                                          m_pimpl->wholeBodyInertialMagMeasures,
                                           magName))
     {
         return false;
     }
 
-    auto iter = m_pimpl->wholeBodyInertialMeasures.find(magName);
+    auto iter = m_pimpl->wholeBodyInertialMagMeasures.find(magName);
     magMeasurement = yarp::eigen::toEigen(iter->second.first);
     if (receiveTimeInSeconds)
         receiveTimeInSeconds.value().get() = iter->second.second;

[prashanthr05]

Nevertheless, what I see is the accelerometer measurements streamed by gazebo_imu do not seem to be rotated in the sensor frame.

For example, the below plot is from the accelerometer on the root link of iCubGazeboV2_5 (Xsens IMU), in which the IMU z-axis is tilted and pointing downwards, ideally on the robot we observe in stationary condition the value [0.09; -5.72; -7.98], here it doesn't seem to be the case.

Data collected after fixing BLF with changes suggested in #592 (comment)

Rotating the accelerometer using,

imu_R_b = iDynTree.Rotation.RPY(-2.09439521059, -3.88578058619e-16, -1.57079632679).inverse().toMatlab();

where the RPY values are obtained from the root_link_imu_acc sensor in the URDF, we get expected values.

[isorrentino]

This problem has been fixed. Closing.