Add support for kinematic constaints.

[matthieuvigne]

I wish to simulate a segway-like system: basically, a wheel turning on the ground. This cannot be exactly simulated in Jiminy, which only supports punctual contacts with an elastic ground: the 'hack' one could do would be to place many points along the rim of the wheel, and set them all as contact points.

Rather, I want to simulate the contact as an analytical constraint: i.e. the wheel is in perfect contact with the ground and does not slip. This can be done by adding the constraint in the dynamics equation as Lagrange multipliers.

Note that I'm only interested in having a constant contact throughout the simulation: I do not mean to implement 'analytical contact' for a walking robot, that would require an added contact detection framework. What I have in mind is a constant contact throughout the simulation. However, this is already a significant first step in that direction if we ever want to do that.

This is how I see the implementation:

• at a 'low-level', contact kinematics is implemented in pinocchio as pinocchio::forwardDynamics(..., J, drift) where J is the jacobian of the constraint, and drift the associated drift. This implementation of pinocchio::forwardDynamics would replace the aba algorithm in Engine.computeSystemDynamics. In practice, pinocchio::forwardDynamics is likely to be much slower than aba (because it requires computing the inertia matrix, angument it with the jacobian, and do the inverse), thus aba would remain the default option, and Engine would switch to using forwardDynamics only if a contact constraint is being used.

• like we did in aba, we will need a custom implementation of forwardDynamics to support armature inertia.

• While I do not need this for my use case, supporting several constraints at once seems a nice feature to have. I think the best way to handle this from the API standpoint is to give a name to each constraint, so adding and removing contact constraints would be like adding and removing sensors or motors.

• As API, to simulate a contact analytical we need a jacobian, with signature matrixN_t jacobian(q), and the corresponding drift term with signature vectorN_t drif(q, v). The main usage would probably be to have a specific frame in contact, however, for a wheel, I want the wheel rim to be in contact, not a fixed point. This requries a 'custom' jacobian to be implemented. So I'm thinking about having two ways to define a constaint: by a simple frame name, or by a callback function.

In the end, here is an overall architecture of what I plan to do:

• Define signatures for jacobian and drift callback functions:

typedef matrixN_t (*JacobianFct_t) (model, data, q);
typedef vectorN_t (*DriftFct_t) (model, data, q, v);

• Add class representing the contact:

class ContactConstraint:
    ContactConstraint(std::string contactFrameName);
    ContactConstraint(JacobianFct_t J, DriftFct_t drift);

• Add add/remove methods to robot, like add / remove of motors, sensors, contact frames... work:
  • Robot::setContactConstraint(std::string name, ContactConstraint)

The engine would then switch between both implements of the dynamics depending of the presence of not of ContactContraint. Note that both contact are not mutually exclusive: you could imagine having both intermitent, elastic contacts, and constant constraints (for instance, fix you robot foot on the ground analytically, then make its hand hit a wall).