Implement friction compensation

[PeterBowman]

TEO's joints require a lot of effort to rotate because of friction. It might be useful to have some sort of out-of-the-box compensation for this phenomenon without having to rely on external control loops.

I believe friction compensation should only target torque/current control as position and speed loops already take care of following their reference regardless of any physical downstream reactions (friction, gravity, load...) that could impede motion. Without any deeper background on control theory, it is only my guess that the acceleration feedforward component of the position PI kind of behaves like a term of the intrinsic compensator.

Regarding the friction model, I'd go first with static friction (a.k.a. Coulomb friction). Further, a damping coefficient could be considered in order to apply an additional friction term that varies with the joint's rotation speed. These two friction kinds are represented in Gazebo via <friction> and <damping> joint properties (SDF reference). Note it seems to go a bit further and even consider a spring-like behavior via the additional initial-reference and stiffness parameters. Refs: Joint::Load, ODEJoint::SetParam (regarding friction), ODEJoint::SetStiffnessDamping (regarding joint stiffness/damping, docs).

According to estimations by S. Morante & J. Victores, static friction and damping coefficients on TEO's left sagittal shoulder result in ~0.7 and ~0.001, respectively (roboticslab-uc3m/teo-gazebo-models#1 (comment)). See morante2015force-preprint.pdf, video.

See also an attempt on friction compensation in the iCub: robotology/icub-firmware#67, robotology/icub-firmware#228, PID_do_friction_comp, eo_pid_PWM_friction.

See also:

• https://github.com/robotology-playground/torque-control-params-estimation.
• https://www.mathworks.com/help/simscape/ref/rotationalfriction.html
• Gazebo: Joint.cc, ODEJoint.cc (SetParam), ODEJoint.cc (SetStiffnessDamping)
• icub-firmware: Pid.c
• icub-firmware-shared: EOpid.c

[PeterBowman]

Note iPOS drives already implement some sort of compensation by applying a current offset. Sadly, I think it doesn't care of the sign of the velocity, hence it might help or hinder motion depending on it. This is also the reason why certain joints (sagittal right hip?) move out of zero pose right after entering torque mode. @jgvictores said this offset was disabled for the sake of their tests.

[jgvictores]

I think it doesn't care of the sign of the velocity

I confirm this is my belief but I'm not 100% sure.

This is also the reason why certain joints (sagittal right hip?) move out of zero pose right after entering torque mode

I confirm I'm also pretty sure about this, which provides further proof of the above (the joint does not even wait for a reference to check the direction, it just feeds the current).

said this offset was disabled for the sake of their tests.

I'm quite positive on this one.

[PeterBowman]

The iPOS setting is available in EasySetup > Drive Setup > Current controller > Tune & Test > Test > Compensate for gravitational load > Offset value.

It is described like this in the documentation:

For applications with gravitational loads you can add an offset to the current reference to compensate the gravitational force. Check “Compensate for gravitational load” and set the “Offset value”. The maximum value of the offset is limited at motor nominal current.

Assuming the latest backup represent the current state of the drives, the only place we are using this parameter is joint 12, left sagittal ankle, with a value of 0.25 A (variable OffsetCurrent).

[PeterBowman]

yarp::dev::ITorqueControl now exposes motor friction parameters, namely Coulomb and viscous friction coefficients, via yarp::dev::MotorTorqueParameters: robotology/yarp#2805. We've got this covered at d5367fb, although currently unused.

[PeterBowman]

Now we have a custom PID implementation: #262. Following on this, I'm thinking of a nested (cascaded) torque PID capable of accounting for friction, whose output would be directly forwarded to the iPOS firmware as current commands. Problem: can I actually use the drive's current probes to obtain force feedback (using the motor constant and reduction factors in the current-to-torque conversion)?

Edit: the current sensor is placed on the motor side, hence no, it doesn't reflect the actual torque exerted by the joint.

Edit 2: see #270.

[PeterBowman]

See https://robotology.github.io/robotology-documentation/doc/html/classiCub_1_1ctrl_1_1OnlineStictionEstimator.html:

Online Stiction Estimator.

Estimate the up and down stiction values.
During the experiment, the joint is controlled by a high-level pid controller that commands directly the voltage in order to track a time varying reference position. The stiction values are estimated during the rising and falling edge transitions.

[PeterBowman]

yarp::dev::MotorTorqueParameters will probably lose its bemf and bemf_scale members in the near future, whereas a new one regarding velocity thresholds will be added in YARP 3.9 (robotology/yarp#3010).