Spnav controller does not dismiss out-of-limit poses (MOVI)

[PeterBowman]

Steps to reproduce and explanation:

• Launch streamingDeviceController with spnav. In --movi mode, the app will construct a virtual trajectory on the infinitesimal position increments exerted by the user on the joystick (ref, https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/173#issuecomment-493479201).
• Reach joint(s) limit(s).
• Keep commanding new poses in the out-of-joint-limits space. The current virtual pose will change while the robot does not.
• In case you re-gain control of the robot because of the virtual pose landing back in the robot's reachable workspace, this pose will be probably far off the one at which the robot stopped motion due to joint limits.
• The robot is commanded an instantaneous huge step in position-direct mode, hence nothing good can happen next.

[PeterBowman]

Temporary workaround:

1. Enable CSV mode (https://github.com/roboticslab-uc3m/yarp-devices/issues/221): set ivar mvar all (enableCsv 1).
2. Use twist commands in streamingDeviceController: just skip --movi (--gain becomes unused).

[PeterBowman]

How to tackle this:

1. Perform IK on the streamingDeviceController, connect to the remote joint controller device and fetch current joint values and joint limits, compare current vs commanded joint values, block commands that exceed limits. Efficient, but overkill.
2. In BCC::movi, which is currently checking against position limits, also make sure velocity limits are being considered (BUT see https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/173#issuecomment-493191279): https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/309bf577f5ffd71a26e90993f4236aa2132f7804/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp#L564-L568

The second option looks cleaner, although it requires the BCC to store previous commanded/actual cartesian values. In any case, this is only to prevent huge position command steps. In streamingDeviceController, the "virtual" TCP is not affected in any way, it's free to move beyond position limits. This also means that, in order to regain control (as allowed by BCC), the user needs to be lucky enough to move the virtual TCP exactly where it surpassed the robot's reachable space.

I'm inclined to close this as invalid. As explained in the linked issue, the velocity limitation is enforced by the low-level driver. Yes, we can really mess things up without proper care and if those limits are set to higher values. Still, it can't be really avoided in --movi mode.

[PeterBowman]

The second option looks cleaner, although it requires the BCC to store previous commanded/actual cartesian values. In any case, this is only to prevent huge position command steps.

Note to self: this might not be a bad idea after all. I'd need to check all cases where limit checks are actually expected, both joint and cartesian controller sides. At https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/173#issuecomment-493191279, delegating velocity checks onto the joint controller seemed the correct way, but are they applied everywhere? BCC could perform a second (or first) check just to make sure. Otherwise, why would we bother having position checks in BCC if they could stay at the iPOS device?

[PeterBowman]

I'm inclined to close this as invalid. As explained in the linked issue, the velocity limitation is enforced by the low-level driver. Yes, we can really mess things up without proper care and if those limits are set to higher values. Still, it can't be really avoided in --movi mode.

Closed as invalid, then. Now, passing --movi will display a warning: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/ff5092ac0fd0d9fc4bbe40f2d4678eb4d0d7bf6d. If you landed here after reading the inline comment, see workaround at https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/186#issuecomment-579706674.