Support passing a additional frame (instead of just a link) to KinDynComputations::setFloatingBase

[traversaro]

The setFloatingBase method of KinDynComputations only supports specifying a link frame as the floating base frame of the floating structure. This can complicate expressing inverse kinematics problems, so it would be convenient to support also passing a additional frame as a floating base of a system.

[traversaro]

Some additional details for @lrapetti and other possible interested users (@diegoferigo @paolo-viceconte).

Test for the feature

First of all, I think we should write a basic test to exert the future setFloatingBase method, for example adding it in https://github.com/robotology/idyntree/blob/d5bbceab9e8ae52943b31199f6e024d12ff52159/src/high-level/tests/KinDynComputationsUnitTest.cpp . In particular, I think we use an existing iCub model (for example the one in V) and run the methods such as getWorldTransform, getFrameVel, getFrameFreeFloatingJacobian, getFrameBiasAcc, getCenterOfMassPosition, getFreeFloatingMassMatrix and getFreeFloatingMassMatrix for two cases:

• setFloatingBase("l_foot"), and setting the state with arbitrary world_H_base, base velocity, base acceleration and gravity
• • setFloatingBase("l_sole"), and setting the state with world_H_base, base velocity and base acceleration computed to be consistent with the arbitary one set when the base was l_foot, for example world_H_l_sole = world_H_l_foot * l_foot_H_l_sole, etc etc .

As the input data is the same even if expressed with different choice of base frame, the quantity computed need to be exactly the same, and so we will have a good test for starting working on implementing the feature.

Feature implementation

When the test is ready, it is possible to start working on the implementation. To avoid changing a lot of the logic of the class, we need to understand clearly where we need to modify the class to account for the additional transformation due to the use of an additional frame rather then a link frame as a base. Fortunately, in KinDynCompuations we already do a base conversions for supporting the different representation of base velocity, so we can check already were we are converting the velocity to have an idea or where to do the conversions. Furthermore, we already are normalizing the internal buffers to always contain velocity expressed in "body" or left-trivialized representation, and that representation is fortunately does not change depending on the base.

To make you an example, I think we should modify for example the setRobotState to correctly take into account the additional transform between the base frame and the relative link in
https://github.com/robotology/idyntree/blob/devel/src/high-level/src/KinDynComputations.cpp#L748, and similarly change the getRobotState https://github.com/robotology/idyntree/blob/master/src/high-level/src/KinDynComputations.cpp#L779, getWorldBaseTransform https://github.com/robotology/idyntree/blob/master/src/high-level/src/KinDynComputations.cpp#L779 and getBaseTwist .
Whenever we are checking for the velocity representation used (access to m_frameVelRepr attribute) we will need to take into account the fact that we are using an additional frame instead of a base link directly.

I am not sure what is the best way of dealing with this, but take in consideration that the existing KinDynComputations is already a bit messy, so if you think it can be better organized feel free to le me know. Most of the math should be contained in Chapter 3 of my PhD thesis (see https://github.com/traversaro/phd-thesis), but if you think that some part of the code need better docs, let me know and I can happily add it, thanks.

[traversaro]

Today we spent some time dealing with some workaround code that dealt with this limitation.