[core] Leveraging new features introduced by Pinocchio 2.9

[duburcqa]

• aba now computes the acceleration and forces, no need to do it manually anymore when there is no constraint
• armature are taking into account into algorithms, through model.armature. No need to overload pinocchio anymore.
• RigidContraintModel and RigidContraintData are now provided to handle fixed frame constraints.

It is not straightfoward to use the new optimized algorithms for constraint computation implemented in Pinocchio. The complexity comes from the inability to define custom jacobian for the constraints in Pinocchio:

• First approach [Try hard to use provided algorithm] The easiest way to get around this limitation is to abuse the data.J (which is the concatenation in column of the joint jacobian in world frame) by swapping the memory with the constraint Jacobian computed by Jiminy. Then, each constraint would feature an extra attributes corresponding to the associated RigidContraintModel and RigidContraintData, always assuming 6 fixed DoFs in WORLD frame whatever the actual reference frame. Jiminy Model would provide new methods getConstraintModel and getConstraintData to get the ordered vectors of RigidContraintModel and RigidContraintData attributes for all active constraints respectively. This way, it is not necessary to modify the core algorithms of Pinocchio. However, it would add empty rows in the jacobian, that may break the algorithm.
• Second apporach [Reimplement the sparse decomposition algorithm] The main issue would be code duplication.

[duburcqa]

After looking more closely at the implementation of the cholesky decomposition in Pinocchio, it is quite far from the current architecture of Jiminy. Jiminy is providing the whole jacobian as a block matrix, and the connection with individual constraints is already lost at this point.

I suggest adopting the non-aggregated formulation in Jiminy. It would be much easier to take advantage of the sparsity in computations as in Pinocchio, correct PGS bias, and handle friction and non-penetration in solver itself instead of doing confusing index manipulations in PGS itself.

[duburcqa]

Jiminy has been refactored as planned. Now it would be possible to implement the Cholesky decomposition of JMinvJt as proposed in the paper.