Implement a dry run mode for MOVL commands

[PeterBowman]

From https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/135#issuecomment-359866382:

Idea: perform a dry run on the desired trajectory (usually movl) and check for any limit violations, e.g. maximum joint velocity might be exceeded at some point or the operation would take too long (the latter does not apply to current strategies since trajectory duration is one of our input parameters). On success, perform the trajectory; on failure, return immediately and notify callers.

[jgvictores]

This idea is nice. It's like an "offline" checker. It would be great if the JMC could do this (kind of like movj vs inv), but our JMC are unfortunately unaware of trajectories. I understand this is what it pushes it out of the JMC, and, to not end up with another layer in between, goes to the catch-all BCC.

1. From what I'm seeing, maybe BasicCartesianControl should be renamed BasicTrajectoryContol or something.
2. Can't completely discard the joint-space offline checker idea... feels kind of like a collision checker that works with the trajectory before it's launched....

[PeterBowman]

Regarding offline cartesian/joint trajectories:

• https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/124#issuecomment-488339423
• https://github.com/roboticslab-uc3m/yarp-devices/issues/208

[PeterBowman]

Renaming issue since movj is not affected by position/velocity limits being hit on runtime: it already performs IK under the hood and fails fast if the target (joint) pose is not achievable.

[PeterBowman]

This is going to be a WONTFIX. The MOVL command loop, after initial trajectory configuration, performs the following steps:

1. Query the trajectory generator for cartesian pose and velocity at current instant t.
2. Perform FK on last encoder reads (position feedback).
3. Close the control loop and apply gain.
4. Perform differential IK.
5. Check if joint velocities are within limits.
6. Send the resulting joint velocity vector to the robot for instant execution.

So, what can we do instead of these steps as a fail-fast pre-check? Stemming from 1.:

• either use queried cartesian pose at instant t to obtain (via IK) joint configuration at instant t, then check position limits straight away and velocity limits via differentiation, or
• use queried cartesian velocity to emulate at instant t to obtain (via differential IK) joint velocities at instant t, then check velocity limits straight away and position limits via differentiation.

Neither method takes into account the underlying control loop of the real system. In addition, it is important to note that joint velocity commands on TEO perform badly and it is almost imposible to obtain a nice linear motion. Whatever we conclude from a pre-check, it won't apply to what is going to happen on real TEO. Does it make sense in simulation, then? No, in that case the benefits of a dry-run mode are minimal.

Summing up, a fail-fast mode could make sense if we got rid of the control loop and replace it with position commands via IPositionDirect. Until then, MOVL is useless anyway (outside simulation).

[PeterBowman]

Summing up, a fail-fast mode could make sense if we got rid of the control loop and replace it with position commands via IPositionDirect. Until then, MOVL is useless anyway (outside simulation).

Reopening, we can configure CSV mode at the joint controller and thus work with velocity commands as if they were position ones: https://github.com/roboticslab-uc3m/yarp-devices/issues/221.

[PeterBowman]

Done at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/26f8c35f4253cc984bc326cbb8ee9fe4b515f4ae. To enable this, launch BCC with enableFailFast. Internally, IK is performed first, then joint position limits are checked (somewhat innecessarily since the IK should fail earlier), then joint velocity limits.