Unbreak high-frequency IPositionControl commands

[PeterBowman]

Developers might sometimes misuse YARP control modes. Recent developments on the CanBusControlboard+CANopen stack have paved the way for those little sins to cause regressions on previously (apparently) working code.

We (@jmgarciah) observed this at https://github.com/roboticslab-uc3m/WBPCwaiter. Despite its original intention, yarp::dev::IPositionControl commands were being used to command a high-frequency online-generated position-based trajectory. Please note the yarp::dev::IPositionDirect interface suits vastly better.

I presume the necessary handshake between node and application (introduced at https://github.com/roboticslab-uc3m/yarp-devices/commit/0d104f68cedff3eb2cb5095098af882d757cc5b0) cannot be accomplished at high transmission rates. Ideally, this mode could be made fail-safe in said scenarios.

[EliseiFrederic]

Hi,

Not sure if it applies to the TEO robot, but on iCub IPositionControl generates a trajectory that starts with 0 speed, so it cannot be used in a rapide sequence to track/reach a moving target (it continuously slow downs and restart) As you said, IPositionDirect can cope with the HF update. BUT in our experience, it can break cables if you ask too much for a step. Combining IPositionControl for the initial move, and then IPositionDirect for the retargeting is not trivial. And how much is "the biggest small step" than does not break something ? If you want acceleration and speed plateau, you end up writting a full controller :-(

Would be fine if IPositionControl could cope with the present joint speeds to correct/adapt the already started trajectory !

Best regards, Fréderic

[PeterBowman]

Hi, @EliseiFrederic, thank you for your input!

It really depends on the device motion control your firmware adheres to, which in turn tells you what modes of operation are allowed. I'm not an expert on the iCub, but I recall noticing that some kind of CAN protocol was running under the hood. We speak to TEO's motor drivers through a CANopen-compliant channel. More precisely, we have implemented yarp::dev::IPositionControl as a C++/CANopen interface according to profile position mode as defined in norm CiA 402-2 (IEC 61800-7-201). Which brings us to...

Would be fine if IPositionControl could cope with the present joint speeds to correct/adapt the already started trajectory !

In CiA 402-2/position profile mode, this is actually possible if the device is configured to operate in "single set-point" submode. That is, new setpoints (trajectory goals) can be loaded into the drive and processed immediately, in other words, the current trajectory is interrupted and a new one is started with no halt in between.

Of course, this is a no-go in case we need online-generated fine-grained sequences of intermediate setpoints, as you pointed out. In such cases, the IPositionDirect mode steps in. We also faced the cable-breakage problem and somewhat patched it with speed limitations (actually, we limit the length of the next step given the expected time between successive setpoints).

[jgvictores]

Thanks for your insights, both!

Small comment: I believe @EliseiFrederic's comment refers to the iCub's tendons, which make it specially sensible to peaks of jerk. In TEO we do not have this issue; in fact the comment by @PeterBowman refers to power/signal cables and connectors which can also be delicate.

[PeterBowman]

In our case, bit 5 of controlword ("change set immediately") is set, which means that as soon as a new target is provided, the drive recalculates and executes the new trajectory. This is the "single set-point" case according to IEC 61800-7-201 (CiA 402-2), chapter 10.2.2. When not set, this bit enables the "set of set-points" mode which enforces that a point must be fully processed in order to start moving to the next one.

Command flow in a single set-point scenario:

• Enter position control mode. Halt bit (8) of controlword is reset.
• Send the next target position (a.k.a. set-point), initiate handshake with the drive (bit 4 of controlword set): https://github.com/roboticslab-uc3m/yarp-devices/blob/e935e364751c86112b5dead877e3ee2a62df6999/libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IPositionControlRawImpl.cpp#L22-L26
• The drive acknowledges the reception of said set-point via bit 12 of statusword and motion starts. Upon receiving the acknowledge signal, we reset bit 4 of controlword to finalize the handshake and allow new set-points to be registered: https://github.com/roboticslab-uc3m/yarp-devices/blob/e935e364751c86112b5dead877e3ee2a62df6999/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.cpp#L236-L241
• When motion ends, bit 10 of statusword ("target reached") is signalized. Halt bit is reset by us: https://github.com/roboticslab-uc3m/yarp-devices/blob/e935e364751c86112b5dead877e3ee2a62df6999/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.cpp#L223-L229

The reference also states that whenever the halt bit is set in order to interrupt the movement, it is resumed as soon as said bit is reset. It doesn't look like the exact same behavior on our drives (?).

[PeterBowman]

Not doing, please use IPositionDirect instead.