Implement yarpWholeBodyModel without depending on KDL

[traversaro]

Not a priority at the moment, but given that we did a lot of improvement to be the algorithms implemented using only iDynTree data structures with natural bindings in Matlab, I think it would be nice to track what we miss to be able to drop dependency on KDL in yarpWholeBodyInterface .

• iDynTree :
  • [ ] Migrate DynamicsComputationsto use the new iDynTree data structures and algorithms :
  • [ ] Implement function to compute link Jacobians
  • [ ] Implement function to compute centroidal momentum
  • [ ] Implement function to compute the centroidal momentum matrix / com jacobian
  • [ ] Implement function to compute the inverse dynamics regressor
  • [ ] For all the new algorithms, write consistency test with the old implementations as in https://github.com/robotology/idyntree/tree/master/tests/kdl_consistency
  • [ ] In general, find a clean way to address the relation between featherstone-style algorithms, in which the base velocity/acceleration/wrench and the link velocity/acceleration/wrench are expressed in a local frame and with spatial accelerations, and the DynamicsComputations/ yarpWholeBodyInterface convention in which quantities are expressed with respect to the local origin, but with the world orientation, and this accelerations are classical.
  • [x] Implement a function to "block" the DOF of a model, for example by transforming all the joints not in the list of DOFs in fixed joints. This is necessary because a key feature of the yarpWholeBodyInterface is the fact that you can add/remove joints from the model at configuration time. Ideally the link attached to DOFs "removed" should be lumped on their parents and transformed in frames, to avoid wasting computation, but the difference is probably negligible. Done with the createReducedModel function in https://github.com/robotology/idyntree/pull/103.
• yarp-wholebodyinterface:
  • [ ] Migrate yarpWholeBodyModel class to use the DynamicsComputations class

cc @nunoguedelha @naveenoid

[traversaro]

Regarding the second point:

In general, find a clean way to address the relation between featherstone-style algorithms, in which the base velocity/acceleration/wrench and the link velocity/acceleration/wrench are expressed in a local frame and with spatial accelerations, and the DynamicsComputations/ yarpWholeBodyInterface convention in which quantities are expressed with respect to the local origin, but with the world orientation, and this accelerations are classical.

The good old @ahoarau implemented a solver in KDL in https://github.com/orocos/orocos_kinematics_dynamics/pull/64 in which the used convention can be changed with call to a specific method. This is indeed a good idea and we could to the same thing in the DynamicsComputations class (in our case this will change also the convention used for the floating base velocity/acceleration).

[traversaro]

Final switch happening in https://github.com/robotology/yarp-wholebodyinterface/pull/63 . The mechanism to switch between different velocity representation is done via the setFrameVelocityRepresentation method.