Working version with Talos and dynamical filter.
Computation time needs to be checked.

Maximum speed reach with TALOS when setting SS phase to 1.0.
Divergence otherwise.

Current status: The sign of the orientation seems to be wrong.
TODO: Check where does this comes from.

and faster debugging.

Enforce 80 columns policy
Indentation to 2 spaces.

Remove warnings

+ enforce the 80 columns rule.

Pb with pinocchio v2.1.0 and retrieveCollisionGeometrey.
It works only if this method is inline.

Eigen is by default column-major, but dgesv is working on column major.

Not tested yet.