Idyntree: Add the possibility to force the height of the contact link in the `LeggedOdometry`
In the SimpleLeggedOdometry class the position of the fixed foot is evaluated by solving a forward kinematics problem.
In details:
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
GiulioRomualdi
All 14 comments
Can you think about a possible change in the SimpleLeggedOdometry API to support this use case?
traversaro
on 13 Mar 2019
Yes, sure!
GiulioRomualdi
on 13 Mar 2019
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.
S-Dafarra
on 13 Mar 2019
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.
GiulioRomualdi
on 13 Mar 2019
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.
S-Dafarra
on 13 Mar 2019
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.
GiulioRomualdi
on 13 Mar 2019
In principle we could improve that method, but maybe we can achieve the same effect by calling init and changeFixedFrame in sequence.
S-Dafarra
on 13 Mar 2019
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.
prashanthr05
on 13 Mar 2019
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.
traversaro
on 13 Mar 2019
A possibility could be to add an optional parameter to the changeFixedFrame method to force the the fixed link to a specified transform.
S-Dafarra
on 13 Mar 2019
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.
GiulioRomualdi
on 13 Mar 2019
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.
prashanthr05
on 13 Mar 2019
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.
S-Dafarra
on 13 Mar 2019
traversaro
on 16 Apr 2019
Related issues
GiulioRomualdi
路
10Comments
traversaro
路
4Comments
traversaro
路
3Comments
traversaro
路
7Comments
gabrielenava
路
7Comments
Most helpful comment
A possibility could be to add an optional parameter to the
changeFixedFramemethod to force the the fixed link to a specified transform.