Trajectory optimizer with contact invariance

The first version of the trajectory optimisation software developed able to handle hard equality and inequality constraints (T2.2) by incorporating also contact invariance techniques (T2.3)