Overcome lack of backdrivability through external forces and teaching (proper GCMP)

[PeterBowman]

The ftCompensation app introduced in https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/191 converts the forces and torques measured by a FT sensor into joint velocities (via differential IK) to be applied on the motors, that is, F = k * dx/dt.

Why not stick to classical mechanics and Newton? Said sensor measurements could be translated into accelerations instead according to F = m * a, where the mass here is parameterized (in fact, such application would perform mass compensation). Actually, the kinematics involved are already handled under the hood by KDL and our cartesian command FORC (input: cartesian forces, output: joint torques). This makes much more sense: tell KDL that the forces measured by the FT sensors should be interpreted as external forces (as in fact they are), obtain joint torques, send command.

By using forces in this algorithm, it could be easier to implement spring-like behavior (i.e. impedance), too.

Blockers:

• https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/162
• https://github.com/roboticslab-uc3m/yarp-devices/issues/261

What is the point of this? As we already know, our GCMP cartesian command is severely affected by the lack of backdrivability in TEO (thanks, @jgvictores). That is, force exerted on the motor side can be easily propagated to the joint side, but not otherwise (think of a worm drive). Ideally, forces exerted by users while in GMCP mode would produce unhindered (as much as friction allows) motion and the limb would not fall on release (since gravity is compensated). In reality, it is too hard to move a joint from the joint side due to said preferred direction of propagation. Here is where FT sensors would vastly help: any force registered on the tip would be translated into joint torques via inverse dynamics and produce the desired motion.

Reminder: GMCP works the same as FORC, but assuming no external forces are present (force on the tip equals zero).

[PeterBowman]

A handy command for printing actual torques measured by the control board:

yarp read ... /teo/leftArm/stateExt:o | awk '{split($0,a,/ \[(ok|fail)\]/); print a[7]}'

The index 7 can be deduced from here.

[PeterBowman]

Done at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/f5010f0e4ee430ff50424d31bceb83521e446bd8. A new ICartesianControl::wrench command has been added as the streaming counterpart of forc. Note there is still some friction to be compensated on the low (joint) level: https://github.com/roboticslab-uc3m/yarp-devices/issues/261.

[smcdiaz]

I like the definition of backdrivability from this paper: https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=4018747