Add the possibility to force the height of the contact link in the `LeggedOdometry`

[GiulioRomualdi]

In the SimpleLeggedOdometry class the position of the fixed foot is evaluated by solving a forward kinematics problem. In details:

https://github.com/robotology/idyntree/blob/114671fb1a012f6214b35b8462d463fea99781fd/src/estimation/src/SimpleLeggedOdometry.cpp#L250-L254

In the case of flat ground, the z coordinate of the foot in contact should be constant. However by propagating the FK the z element increases. Because of this, it would be nice to have the possibility to force the z coordinate when the fixed frame is changed.

@prashanthr05 @traversaro @S-Dafarra

[traversaro]

Can you think about a possible change in the SimpleLeggedOdometry API to support this use case?

[GiulioRomualdi]

Yes, sure!

[S-Dafarra]

Can you call the init method again? The idea is to filter the z component from the outside (even if I am not sure about which kind of side effects this would have). If you want to do this at library level you first need to specify what is the normal to the ground and set to zero the corresponding height component. Suppose you are keeping the z parallel to gravity, but you are walking on an inclined plane: in this case forcing the height to zero would not be a good idea. Another advantage of doing it from the "outside" is that you know the moment in which you are going to have a discontinuity.

[GiulioRomualdi]

Another advantage of doing it from the "outside" is that you know the moment in which you are going to have a discontinuity.

As far as I know changeFixedFrame(const FrameIndex newFixedFrame) is called from "outside" when, for example, a new contact is detected. So you know when you will have the discontinuity.

[S-Dafarra]

So in principle you could also reinitialize the odometry by setting the "modified" transform of the fixed link. I don't know how this affects the performances of the base estimator though.

[GiulioRomualdi]

So in principle you could also reinitialize the odometry by setting the "modified" transform of the fixed link. I don't know how this affects the performances of the base estimator though.

Actually no because the SimpledLeggedOdometry class has a KinDynComputation object and it only wants the index of the frame that is considered as a fixed frame.

[S-Dafarra]

In principle we could improve that method, but maybe we can achieve the same effect by calling init and changeFixedFrame in sequence.

[prashanthr05]

We could follow these steps from the "outside",

• change fixed frame,
• get transform
• modify z-component of position
• init odometry with the reference frame and the modified transform However the final step will have extra computations and we must be careful to inverse the transform we get, since we set the world with respect to the base frame.

[traversaro]

Some of the steps described by @prashanthr05 seems to be something that it could make sense to have inside the API, even without anything specific to Z.

[S-Dafarra]

A possibility could be to add an optional parameter to the changeFixedFrame method to force the the fixed link to a specified transform.

[GiulioRomualdi]

A possibility could be to add an optional parameter to the changeFixedFrame method to force the fixed link to a specified transform.

Yes, exactly and the desired value of the z can be set by another method.

However, this approach is suitable only for walking in flat terrain. As soon as we will walk in a slope or, even worst, in an uneven terrain this hypothesis will not hold anymore.

[prashanthr05]

To be part of the library, we can add an option that assumes flat terrain and allows us to use the method described by @GiulioRomualdi

Yes, exactly and the desired value of the z can be set by another method.

only when the option is true.

[S-Dafarra]

To be part of the library, we can add an option that assumes flat terrain and allows us to use the method described by @GiulioRomualdi

Yes, exactly and the desired value of the z can be set by another method.

only when the option is true.

To be honest I don't like this too much since you are also assuming that the "z" axis is pointing upwards.

[traversaro]

Fixed by https://github.com/robotology/idyntree/pull/525 .