Fix bug in init() of SimpleLeggedOdometry

[prashanthr05]

THis PR fixes the following,

• One of the overloaded instances init() methods in SimpleLeggedOdometry was using link_H_frame is computed by passing the link index instead of the frame index.
  

  idyntree/src/estimation/src/SimpleLeggedOdometry.cpp

  Line 71 in ba43b00

  Transform initalFixedFrame_H_fixedLink = m_model.getFrameTransform(m_fixedLinkIndex).inverse();

  
  This was fixed to use the frame index.

Experiment description for the Identification of the bug

A legged odometry experiment was done in Gazebo simulation using the iCubGazeboV2_5 URDF model and the results were compared with ground truth measurements coming from Gazebo.
The following steps were followed to set up and run SimpleLeggedOdometry,

• l_sole was used as an initial reference frame with the following transformation with world

>> ref_H_w

ref_H_w =

    1.0000   -0.0050    0.0003   -0.0341
    0.0050    0.9999   -0.0118    0.0082
   -0.0002    0.0118    0.9999   -0.0108
         0         0         0    1.0000

• Right after the initialization and updating kinematics, calling the getWorldFrameTransform on the l_sole resulted in,

>> w_H_lsole

ans =

    1.0000    0.0050   -0.0002    0.0341
   -0.0050    0.9999    0.0118   -0.0090
    0.0003   -0.0118    0.9999   -0.0536
         0         0         0    1.0000

while the expected was

>> w_H_lsole

ans =

    1.0000    0.0050   -0.0002    0.0340
   -0.0050    0.9999    0.0118   -0.0082
    0.0003   -0.0118    0.9999    0.0108
         0         0         0    1.0000

With some investigation, it was identified that during initialization the w_H_refframe was wrongly calculated only up to the link associated with the refframe and not upto the refframe itself.

Results

Before the fix suggested in this PR,

l_sole position z(m)

After the fix suggested in this PR,

l_sole position z(m)

It can be observed that the initial offset in the estimate has vanished.
Note the drift in the estimate is not relevant to the issue in hand. So it can be ignored for the scope of this PR.

[CLAassistant]

All committers have signed the CLA.

[prashanthr05]

cc @traversaro

[traversaro]

Can you base the PR on the master branch? As the bug fix is quite critical, did you verified it works fine with the fix?

[prashanthr05]

Can you base the PR on the master branch?

Done.

As the bug fix is quite critical, did you verified it works fine with the fix?

Yes, this has been verified. Should I add some details regarding this?

[traversaro]

As the bug fix is quite critical, did you verified it works fine with the fix?

Yes, this has been verified. Should I add some details regarding this?

Yes, if you have few lines about this it would be great!

[traversaro]

It would be also great if you could be a bit more verbose in the changelog. " Fix bug in init() of SimpleLeggedOdometry" is not really informative for the user. A similar phrase "Fix bug in init() of SimpleLeggedOdometry that used an incorrect initial world frame location if used with an additional frame of a link" is more informative without a lot of additional effort.

[prashanthr05]

Yes, if you have a few lines about this it would be great!

Updated the PR description. Feel free to edit or remove details, if you think they are not relevant.

It would be also great if you could be a bit more verbose in the changelog.

Updated to your suggestion. Thank you for the suggestion!