Integrate desired accelerations to get references for position-controlled joints

[andreadelprete]

As it can be seen in the project wiki, the position-controlled joints use as references the output of the joint-trajectory-generator. This means that the CoM reference trajectory is completely neglected.

A better strategy could be to integrate the desired accelerations computed by the balance controller, and use the resulting positions as references for the joint position controller. To do that, the only missing block is a double integrator, which should also handle the switch between torque and position control (i.e. reset the integral when a joint switches to position control).

Do you see anything wrong with this approach @thomasfla ?