Add iCubGazeboV2_7 model

[traversaro]

Fix for robotology/icub-models#49 . For now, the iCubGazeboV2_7 is just a iCubGazeboV2_5 with an additional frame and sensor to represent the XSens IMU mounted in the waist.

cc @GiulioRomualdi

[prashanthr05]

This should be base_link.

Please see the fix in prashanthr05/icub-models@d7e09b4

and a detailed comment in https://github.com/dic-iit/element_floating-base-estimation/issues/61#issuecomment-651695040

Posting the comment here if someone cannot access the private repository,

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

[traversaro]

Mhh, I am a bit confused here. base_link and root_link share the same orientation, so I don't understand why there should be a difference. I typically prefer to attach the sensor to root_link as the fixed link lumping works in a different way in iDynTree/URDF/Rviz, in which "root_link" is the real link, while "base_link" is the massless additional frame.

[traversaro]

However, note that the problem is already there for the iCubGazeboV2_5_plus in https://github.com/robotology/icub-model-generator/blob/6e6ac9f5ecad68771f33c73257a9b02040a3f2d8/simmechanics/CMakeLists.txt#L145 .

[prashanthr05]

Mhh, I am a bit confused here. base_link and root_link share the same orientation, so I don't understand why there should be a difference. I typically prefer to attach the sensor to root_link as the fixed link lumping works in a different way in iDynTree/URDF/Rviz, in which "root_link" is the real link, while "base_link" is the massless additional frame.

Possibly because the sensor is not considering the massless link as its parent and the relative pose we specify is not used, thereby making the world become its parent?

[prashanthr05]

However, note that the problem is already there for the iCubGazeboV2_5_plus in

Indeed, I think I copied the changes to iCubGazeboV2_5 on my fork from the iCubGazeboV2_5_plus. (or maybe @GiulioRomualdi did, and I started making modifications over it)

[traversaro]

Mhh, I am a bit confused here. base_link and root_link share the same orientation, so I don't understand why there should be a difference. I typically prefer to attach the sensor to root_link as the fixed link lumping works in a different way in iDynTree/URDF/Rviz, in which "root_link" is the real link, while "base_link" is the massless additional frame.

Possibly because the sensor is not considering the massless link as its parent and the relative pose we specify is not used, thereby making the world become its parent?

I think there is a bug in the URDF to SDF conversion on this, see #140 .

[prashanthr05]

So this might explain the problem with orientation as well described in #139 (comment). Possibly not.

[traversaro]

So this might explain the problem with orientation as well described in #139 (comment). Possibly not.

Why not? I think it is exactly this the problem.

[prashanthr05]

Because even after changing the sensor's parent link to a lumped mass link (base_link), I remember having the problem with orientation measurements from the sensor. So I was thinking if it was more related to the initialization of IMU pose?

[traversaro]

Sorry, I referred indeed to the problem that was fixed by switching to using base_link as the reference. That seems indeed another problem that maybe is related to gazebosim/gazebo-classic#1959 .

[Nicogene]

I rebased this PR and fixed the model generation, is it ready to be merged?

[traversaro]

I rebased this PR and fixed the model generation, is it ready to be merged?

Ok for me, once the CI is happy.

[Nicogene]

@prashanthr05 is it ok also for you?

[prashanthr05]

@prashanthr05 is it ok also for you?

Ok for me too.

I think if the problem of Gazebo IMU with root_link/base_link confusion still persists, we can open a new PR since that might require changes in other models as well.