Implement computation in ContactConstraint
Implement first version of complete force control (solver), without friction cones (to be done).