Observe joint limits on both iPOS controller implementations

[PeterBowman]

As a follow-up to https://github.com/roboticslab-uc3m/yarp-devices/issues/262, I'm going to decide on the safety mechanisms we should enforce on the new "external" controller regarding its behavior around joint limits, and also review the ones enabled in the "embedded" implementation.

The "embedded" (classic, default) controller behaves as follows:

• On position profile mode: don't exceed limits even if commanded further, print warnings.
• On velocity profile mode: same as above.
• On CSP mode: same as above.
• On torque/current mode: print warnings, but still apply desired reference torque/current even if limits are exceeded.
• On idle mode: motors are free to rotate to any extent, no warnings are printed.

The warnings we are getting look like these lines:

[INFO] |rl.TechnosoftIpos|ID26| [MSR] Limit switch negative event / interrupt triggered.
[WARNING] |rl.TechnosoftIpos|ID26| [DER] Negative software limit switch is active.
[WARNING] |rl.TechnosoftIpos|ID26| [EMCY] Negative software position limit triggered

The "external" controller heavily relies on the torque/current drive mode, also when it's closing a position control loop. Perhaps encoder reads should be permanently watched and acted upon if we try to go beyond limits?

Relevant iPOS manual entry: 5.3.2 Software limit switches functionality

The software limit switches work just like the hardware limit switches (LSP, LSN) in terms of functionality. An individual position value is chosen for the negative and positive limits and when those values are reached, the drive will quick stop. A new motion will be accepted only if the motion is opposite the active software or hardware limit switch.

The software limit switches can also be configured through Object 607Dh: Software position limit.

If the positive software limit switch is activated, the emergency error code 0xFF06 will be sent automatically and object 2002h bit 6 will be 1.

If the negative software limit switch is activated, the emergency error code 0xFF07 will be sent automatically and object 2002h bit 7 will be 1.

When a limit switch becomes inactive, the emergency error code 0x0000 will be sent automatically and object 2002h bit 6 or 7 will return to 0.

Even though the manual states that a quick stop is performed, it does not mean that the drive transitions into the "Quick stop active" state. In fact, it is still in "Operation enabled".

[PeterBowman]

• On torque/current mode: print warnings, but still apply desired reference torque/current even if limits are exceeded.

I can't really tell since the synchronization thread keeps sending new torque/current references that might be overwriting something. Perhaps a single reference would be indeed zeroed upon hitting a joint limit. Section 5. Drive control and status states in Table 5.1.2 Drive States:

Quick Stop Active: Drive has been stopped with the quick stop deceleration. The power stage is enabled. If the drive was operating in position control when quick stop command was issued, the motor is held in position. If the drive was operating in speed control, the motor is kept at zero speed. If the drive was operating in torque control, the motor is kept at zero torque.

It might be interesting to check this (out of sheer curiosity), but it's actually worthless since I'm not going to change the way torques or currents are synced.

Edit: I have checked this out anyway. The reference torque is not reset upon triggering a limit switch.

[PeterBowman]

I'm considering one of three possible ways to handle emergency stops upon reaching a joint limit (signalled via EMCY):

• Update the target command, send either zero torques/currents/speeds or re-send the current position (depending on the current control mode). Disable further commands unless they make the joint move within limits.
• Transition into Quick Stop state. Set object 605Ah ("Quick stop option code") accordingly. Transition back to Operation Enabled if moving again within limits.
• Perform a halt command. Set object 605Dh ("Halt option code") accordingly. Reset halt bit if moving again within limits. In external reference torque mode, this requires sending a rising edge in controlword's bit 4.

The first option is easy, but the actual stop (via RPDO3) is not processed until the next SYNC. The two remaining options rely on a controlword command, hence it is a high priority message (via RPDO1). Transitioning into quick stop must be done without blocking as it is called from within the CAN read thread (because of attending to an EMCY signal). Its main con is having to transition back to Operation Enabled, which can be slow (and would block). The halt command seems a good solution, although a bit tougher to handle than option 1.

[PeterBowman]

So option 1 has been implemented and merged at https://github.com/roboticslab-uc3m/yarp-devices/commit/0cd1dbd3b52f23b74e024a06c244d74ea1d32c2b. I'm still considering option 3 (halt command) and it would look pretty much like this: https://github.com/roboticslab-uc3m/yarp-devices/commit/23ed1f95913a54192e657355966cb1cda82caed7.