Add position feedback to BCC::twist

[PeterBowman]

BasicCartesianControl::twist performs open-loop velocity control, which is unsufficient if a steady, linear path is desired. Any attempts to control TEO's arms via joystick (https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/173#issuecomment-493400560) will cause said arm to bend due to gravity.

• Position-based control (via movi cartesian command and yarp::dev::IPositionDirect joint commands) does the trick (https://github.com/roboticslab-uc3m/yarp-devices/issues/231#issuecomment-575821842), but is somewhat shaky.

• A closed-loop setup as implemented in movl takes advantage of the position feedback and tries to compensate the bend-down reaction of the arm in motion. By setting an acceptable gain value, this command yields nice results (https://github.com/roboticslab-uc3m/yarp-devices/issues/221#issuecomment-575843920).

So, I'd like to add position feedback in the twist command in case it renders beneficial.

[PeterBowman]

Besides, ICartesianControl::pose might no longer make sense, anymore, as movi is now the preferred way to accept position-based targets.

[PeterBowman]

Proposed control scheme:

• Input 1: desired instantaneous cartesian velocity.
• Input 2: expected pose (cartesian position) on the previous twist command.
• Input 3 (feedback): current cartesian pose.

Similarly to movl, we combine inputs 2 and 3 in an integral control scheme (differentiate input2-input3 and apply gain). Then, add input 1 and perform differential IK to obtain joint velocities.

Every step needs to store the expected pose (input 2 of the next iteration) resulting from input 1, expressed in terms of infinitesimal distance, added to input 3.

Tested at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/8d8dca02f56d8cff9e4af5b41063ab13b12034f1, not working as expected. Alternative solution is even worse: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/dddd23e44fadbc734170879b997554a6d6f1fa36.

[jgvictores]

Tested at 8d8dca0, not working as expected. Alternative solution is even worse: dddd23e.

That's a pity. The proposed control scheme LGTM.

[PeterBowman]

Not doing. This has been superseded by CSV mode (https://github.com/roboticslab-uc3m/yarp-devices/issues/221). Also, the reason why the arm was bending down could've been that the default IVelocityControl implementation (i.e. "velocity profile" at the firmware level instead of CSP/CSV) accelerates/decelerates prior to reaching each velocity reference. It is of course best to apply instantaneous velocities as in CSV mode, which will probably become the new default.

Cf. the ftCompensation app in which CSV is also the preferred method: https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/191.