Add velocity feedback in motor control

[andreadelprete]

Currently the motor controller is a pure torque control, meaning that the desired current is a function of the desired and estimated joint torques only (plus friction compensation, which of course depends on velocity). A simple trick used by many people at the DRC to improve tracking was to add a velocity feedback term. The reference motor velocities dq_mot were computed by integrating the desired accelerations computed by the inverse-dynamics controller ddq_des. However, to avoid the drift of dq_mot, they used a leaky integrator expressed by this first-order ODE:

ddq_mot = a * (dq - dq_mot) + ddq_des

where dq_mot is the integral of ddq_mot. The leaking rate a determines the rate at which the in-
tegral drifts towards the estimated velocity dq. In [1] they have set a to 75 for the lower body joints and 0 for the upper body joints to account for the different actuator impedances.

[1] "Optimization-Based Whole-Body Control of a Series Elastic Humanoid Robot", Hopkins, Leonessa, Lattimer, and Hong, http://www.worldscientific.com/doi/pdf/10.1142/S0219843615500346