Add Inverse dynamics class using TSID

[EtienneAr]

This PR is still in progress, here is my todo list before asking for any review. Skip this comment to have a clearer summary of this PR.

• 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 ? (for later PR)
• 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 (returns acceleration already, for later PR)
• 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

[EtienneAr]

This PR is (almost) ready for review. The only thing left is to wait for a new release of TSID (hopefully coming soon) in order to have the fix I made regarding adding and removing rigid contacts.

Here is a overview of the proposed changes :

• Refactor RobotModelHandler and RoboDataHandler (mostly for feet anagment) :
  • Now all the function concerning the feet are only indexed using the feet nb and not the feet name. This is for homogeneity concerns and efficiency (for the cases it is needed). The method getFootNb(std::string name) provide the index of the foot knowing it's name, if necessary.
  • There are now 2 types of foot, point foot (which can be used to model a quadruped foot, for instance) and quad foot (which can be used to model a humanoid foot). The latest require passing 4 points defining the shape/size of the foot.
• inverse Dynamics :
  • I added two classes to perform inverse dynamics, one for kinodynamics settings and another one for a centroidal setting.
  • Both class are extensively tested (each cost/task is tested separately and one test integrate everything together, both on quadruped and biped robots)
  • Design of ID classes:
    • Each class contains a Settings struct containing all the necessary params to initialize it. Mostly costs and weight for each of the tasks. (Note: if a weight is set to a negative value, which is the default value, the the task is ignored)
    • Each class then have a:
      • setTarget member that takes as arguments pieces of the MPC output, and compute/set all the setpoints of the tasks
      • solve member, that takes the current state as argument and return the torque
  • All the python examples have been updated to include those ID as demonstration
    • Note: while I have been able to tune the centroidal ID in a static setting for the Talos humanoid robot, I haven't been able to tune it to work with the MPC, so the example explode... However everything has been tested in this ID so I am confident it is not due to a bug of the ID.
    • I tried to clean / homogenize a bit the examples in the meantime

[edantec]

First draft of review, pointing some mistakes (mainly wrong signedness for int)
Also pointing bugs in test and examples