cleaning
Correct the computation of the desired position of the inverted pendulum
Update gains definitions