Add Inverse dynamics class using TSID

[EtienneAr]

This PR is still in progress, here is my todo list before asking for any review (also, by the time I finish, I might split it in several smaller PRs to make it more legible) :

• Member function of the ID class (like setTarget, solve) should be templated on the vector type, and use const Eigen::Ref<VectorType> &

• sampleBase, samplePosture and poseBase do not require to be kept as member variable

  • [Edit: sampleBase cannot be removed, because the setter actually does more stuff than just copying. To be consistent, samplePosture will be kept]

• foot_forces should be better typed than a flattened list, probably Matrixtype<3, -1>

  • Should interpolator be templated to account for matrices now ?
  • Check that examples are still working

• RobotModelHandler should avec methods to extract confguration and velocity from the state getConfiguration getVelocity

  • Do a global search of .head(nq) .tail(nv) and replace when appropriate

• Are all tsid tasks tested individually ?

  • Test joint limits
  • Test actuation limit
  • ... ?

• Does solver need a resize after adding/removing contacts ?

• bindings

  • UpdateInternalData
  • KinodynamicsID, KinodynamidsID::Settings
  • getFootPose
  • Any other added function ?

• Should the data handler keep a copy of the state ?

• getFootPose should take the foot index as argument, not the name (more effcient solution by default)

  • Add a deprecation warning ?

• BaseTask : use data handler

• Make suer TSID PR is merged, and set minimum version in CMake accordingly

• Add asserts everywhere to check dimensions

• ID should also give the q and v after interpollation, to feed forward to the robot

• getAccelerations shouldn't be returning a const cast

• Take into account 6D contact forces

  • Add point vs 6D semantic in ModelHandler
  • Test AddFootFrames
  • Rename feet_ids_ ? (confusing with nb)

• Do a subclass for centroidal

• Make ID work for talos kinodynamics

  • Add Angular Momentum task --> Not needed for now (maybe for centroidal)
    • Which sample value & derivative to set ?
    • Add unit test
  • Double check that Com task value, derivative is good --> Not needed for now (maybe for centroidal)
    • How to test derivative (in unittests ?) -> Speed of convergence ??
    • Compute task second derivative ?? --> is it worth ? Test it !
  • Tune Talos example to work

• Contact pose should be indexed by foot number not foot name

Necessary ?

• getContactForces return a MatrixN3
• interpolateLinear must be templated on matrix ? (because vectors or matrices ??)