Implement properly simulated joint torque control mode

[traversaro]

Since when gazebo-yarp-plugins was created, the YARP's torque control model (see http://wiki.icub.org/wiki/File:ICub_Control_Modes_1_1.pdf) has been implemented in gazebo_yarp_controlboard by passing directly the reference torque to the Joint::SetForce method of Gazebo. However, that methods sets the input torque to the simulation, i.e. in formulas, it permits to set $\tau_{sim}$

$$ \tau_{sim} = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + J^T f_{ext} - \tau_{friction}(q, \dot{q}) $$

On the other hand, the torque control mode on real robots used by impedance controllers and whole-body-controllers typically aims to control (using feed-forward terms and feedback control laws, if some kind of torque feedback is available) the following quantity:

$$ \tau_{control} = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + J^T f_{ext} $$

Depending on the amount of friction on the simulated model, this two quantities can be change quite a lot, so it is important to keep this in mind when using gazebo-yarp-plugins

This is not a new issue, but I prefer to open it and state the problem clearly so that uses of gazebo-yarp-plugins are aware of the problem and can have a reference if they want to solve this issue. If anyone is interested in solving this issue, please ping me and I would be happy to provide more details.

Related comments:

• https://github.com/robotology/gym-ignition/pull/158/files#r395493760
• https://github.com/osrf/gazebo/issues/1321