Create a templated interface that mimicks KinDynComputations

[S-Dafarra]

No description provided.

[traversaro]

I am not sure, but I guess that for the conversion necessary to go from pinocchio to KinDynComputations it could be useful to combine equations 3.59, 3.60 with equations 3.75, 3.76 of https://traversaro.github.io/phd-thesis/traversaro-phd-thesis.pdf .

[traversaro]

Related issue: robotology/idyntree#674 .

[traversaro]

stack-of-tasks/pinocchio#1308 is an example of an issue of having something that mimicks KinDynComputations on the top of pinocchio (so with the setFloatingBase method to change the floating base of the model) could be useful.

[traversaro]

The related PR stack-of-tasks/pinocchio#1310 also contains some interesting details: in particular, as you can see the joint ordering in pinocchio will change if you change the root body. This is due to the tightly coupling between the chosen numbering (i.e. the mapping between the set of joints and links and the indexes used to represent them compactly) and the chosen base link, as discussed in Remark 3.4 of my PhD thesis (https://traversaro.github.io/phd-thesis/traversaro-phd-thesis.pdf):

Remark 3.4. In classical approaches this numbering is also used to encode some of the hierarchical properties of a spanning directed tree induced by the choice of an arbitrary floating base link. In particular the regular numbering is usually adopted [Featherstone, 2008, Jain, 2010], in which every parent link has a number lower then the ones of its children. This aid the description of some algorithms for multibody dynamics but has the downside of coupling the chosen numbering with the chosen base link. As in this chapter we explore and use in depth the concept of changing the base frame of the system, we opted to keep the numbering and the topology of the system separated, such that internal quantities such the robot shape are independent of the choice of the base frame

iDynTree follow the formalism of my PhD thesis, so even if you change the base link, the numbering of links and joints will not change, because the information on the order of visit of links in RNEA algorithms is not encoded in the link and joint numbering, but rather in a separate data structure called iDynTree::Traversal (https://github.com/robotology/idyntree/blob/c8bf721b771fa4b1e7c3a940632e121060719a19/src/model/include/iDynTree/Model/Traversal.h#L25) that is generated from a Model and a chosen base link (https://github.com/robotology/idyntree/blob/c8bf721b771fa4b1e7c3a940632e121060719a19/src/model/include/iDynTree/Model/Model.h#L433). The RNEA algorithms then do not visit the joint/links from 0 to N (forward) and from N to 0 (backward) as done in the Featherstone book and in Pinocchio source code (https://github.com/stack-of-tasks/pinocchio/blob/063ee519073718c37196fea73c595f54afb65fa6/src/algorithm/rnea.hxx#L116), but rather iterate on the link in the order provided by the iDynTree::Traversal (see https://github.com/robotology/idyntree/blob/c8bf721b771fa4b1e7c3a940632e121060719a19/src/model/src/ForwardKinematics.cpp#L77). Decoupling the joint ordering from the choice of the base link and in general from the hierarchy of the model also has the additional advantage of permit to the user to specify its own preferred joint serialization, as we done for example in the loadReducedModel functions (https://github.com/robotology/idyntree/blob/114671fb1a012f6214b35b8462d463fea99781fd/src/model_io/urdf/include/iDynTree/ModelIO/ModelLoader.h#L126) that is used extensively in our software.

For this reason, to keep the property of keeping a consistent (and arbitrary) joint order in the KinDynComputations
shim it is probably necessary to mantain a kind of mapping between the joint and link ordering used in the KinDynComputations API (that is specified by the user and independent from the topology of the model) and the one used internally by Pinocchio (that instead depends on the topology of the model and on the choice of the base link).

[jcarpent]

In fact, it may have some advantages of being independent. In our cases, using a dedicated ordering for the CRBA for instance allows a better and faster implementation. The question is would it possible to keep a universal Model without given ordering and then produce a resulting Model with an optimized ordering?

[traversaro]

Decoupling the joint ordering from the choice of the base link and in general from the hierarchy of the model also has the additional advantage of permit to the user to specify its own preferred joint serialization, as we done for example in the loadReducedModel functions (https://github.com/robotology/idyntree/blob/114671fb1a012f6214b35b8462d463fea99781fd/src/model_io/urdf/include/iDynTree/ModelIO/ModelLoader.h#L126) that is used extensively in our software.

This is related to our today's discussion @Andrea8Testa .