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
• Proper joint torque feedback gazebosim/gazebo-classic#1321

[DanielePucci]

CC @robotology/iit-dynamic-interaction-control

[gabrielenava]

CC @robotology/iit-dynamic-interaction-control

Note that this filter may not be visible to everybody in the dynamic-interaction-control as it was not mentioned in the list of "default" filters for that organization.

[diegoferigo]

Thanks @traversaro for reporting this mismatch to everyone. I am having a look and, just to make it more clear, I wanted to check if the following statements are correct.

Let's assume that we know how the physics engine is dealing with friction. Let's say that, as often happens, we can define the coulomb (static) and viscous friction. Let's also imagine that our robot model has motors associated to joints, with no harmonic drives (i.e. joint angle = motor angle). Assuming also that the motors do not have any inertia, we can say that the physics engine is applying the following model:

$$ \tau_{friction}(\dot{q}) = K_v \dot{q} + K_c sign(\dot{q}) $$

This means that, when a controller is calling Joint::SetForce($ \tau_{control} $), a fraction of this torque is used to fight friction, and the remaining part is the real net torque sent to the motor. This is what you denoted as $ \tau_{sim} $. Therefore, we have: $ \tau_{sim} = \tau_{motor} $. In other words:

$$ \tau_{motor} = \tau_{control} - \tau_{friction} $$

As you reported, impedance and whole-body controllers aim to control the second equation you wrote. This means that they want the motor to actuate the right-hand side of that equation. However, due to friction, a fraction of it is lost.

Therefore, in order to make these controller to deal with friction, we should add a feedforward term when we set the force: Joint::SetForce($ \tau_{control} + \tau_{friction}(\dot{q})$). In this way, the simulated controllers should work exactly as before even under the presence of friction.

[traversaro]

Therefore, in order to make these controller to deal with friction, we should add a feedforward term when we set the force: Joint::SetForce($ \tau_{control} + \tau_{friction}(\dot{q})$). In this way, the simulated controllers should work exactly as before even under the presence of friction.

That is correct. In practice it may be impossible to perfectly compensate the $\tau_{friction}(\dot{q})$ as it is possible that the physics engine is actually implementing viscous friction using implicit integration, so actually using the joint velocity of the future. Furthermore, it is possible that perfectly compensating the viscous friction makes the system more difficult to integrate (i.e. more stiff) from the numerical point of view, but that does not change that what you said is correct.

[diegoferigo]

I'm currently doing some experiments with a simple pendulum using Ignition Gazebo (that uses DART), and the viscous friction is correctly compensated. I can see indeed a small error in the output joint position and it could be related to the implicit integration scheme you correctly reported.

I noticed that the coulomb friction compensation has a caveat. Let's consider a swinging pendulum that is already swinging (i.e. $ \dot{q} \neq 0 $), and an applied sinusoidal torque that could mimic the effect of a controller. As soon as the velocity goes to zero, and the swinging is supposed to change its direction, the feedforward term becomes $ K_c sign(\dot{q}) = 0 $. This means that in this situation the compensation is not active since the feedforward correction does not know if applying +Kc or -Kc. Do you have any suggestion about how to deal with this singular point?

[traversaro]

This means that in this situation the compensation is not active since the feedforward correction does not know if applying +Kc or -Kc. Do you have any suggestion about how to deal with this singular point?

Most (real) Coulomb friction compensation scheme typically avoid to use an actual discontinuous signal, bur rather a continuous approximation, using for example a linear or a cubic signal around the zero. In this case, if you want to exactly compensate the Coulomb friction in the simulator I would check how the Coulomb i s actually implemented in the simulator.

[diegoferigo]

Thanks for the input @traversaro. Even using a continuous compensation signal, the problem with zero velocity would still be there, wouldn't it? In fact, even if the curve is smoother, it will pass through (0, 0) nonetheless.

[traversaro]

Thanks for the input @traversaro. Even using a continuous compensation signal, the problem with zero velocity would still be there, wouldn't it? In fact, even if the curve is smoother, it will pass through (0, 0) nonetheless.

Yes, in reality no one ever perfectly compensate for friction, as you never perfectly know the exact friction model and you don't have perfect (with no-delay) position and velocity feedback. In practice the case of being around zero is handled either via closed-loop torque control, or compensating the friction of the desired velocity, rather then the measured one.

[diegoferigo]

Ok, it makes sense. I think these information could be useful both for people coming here and for whom will address this issue. Thanks again!