Cannot change control mode some/many times

[jgvictores]

Similar to https://github.com/roboticslab-uc3m/yarp-devices/issues/127, thing seem pretty bad. Changing from initial position mode to torque mode, the mode is not enabled even after several tries. This is, either it works the first time, or never works at all.

Additionally, as described at https://github.com/roboticslab-uc3m/teo-main/issues/35, the pos -> torq -> pos -> torq sequence is not possible either.

[PeterBowman]

Same thing happened today:

• start BasicCartesianControl, connect to a running launchManipulation instance
• check driver state (get icmd cmds), should default to pos
• issue the gcmp command on the BCC's RPC server, drivers should switch to torq mode
• issue the stop command on the BCC's RPC server, drivers should switch back to pos mode
• issue gcmp again, check that drivers are in torq mode
• everything should look fine, but all motors are controlling position and the arms can't move

[PeterBowman]

I can confirm this is still an issue in current https://github.com/roboticslab-uc3m/yarp-devices/pull/229 code, although this time I did not test it beyond the pos->torq->pos transition sequence. Torque mode is successfully enabled, but motors do not control position after the last transition. I noticed the behavior of this mode either differs from the indications in the user manual, or simply I'm missing something and the solution is quite obvious. I contacted support in that regard.

[PeterBowman]

I noticed the behavior of this mode either differs from the indications in the user manual, or simply I'm missing something and the solution is quite obvious. I contacted support in that regard.

Answered:

Our implementation of the CANopen protocol respects the standard (CiA402). (...) So the "2F" command is mandatory if you want to change immediately target position. The "2F" command it will tell the drive that it will accept a new target position ("a single set-point is expected by the drive" - CiA402). This will put the "Set-point acknowledge" on 0. After "3F" command the "Set-point acknowledge" bit is 1, the drive will not accept a new target position. The acknowledge bit is reset only after "2F" command.

However, CiA DSP 402 (v2.0) states in 12.4:

Two different ways to apply target positions to a drive, are supported by this device profile.

Set of set-points: After reaching the target position the drive unit immediately processes the next target position which results in a move where the velocity of the drive normally is not reduced to zero after achieving a set-point.

Single set-point: After reaching the target position the drive unit signals this status to a host computer and then receives a new set-point. After reaching a target position the velocity normally is reduced to zero before starting a move to the next set-point.

Support may have misunderstood me. I want a "set of set-points", in the above comment I'm told how to configure a "single set-point".

[PeterBowman]

Support may have misunderstood me. I want a "set of set-points", in the above comment I'm told how to configure a "single set-point".

Nope. Finally, thanks to @jgvictores, we found a recent version of the relevant CiA standard (402-2), see https://github.com/roboticslab-uc3m/yarp-devices/issues/223#issue-475315660.

A set of set-points, if supported by the drive, implies an internal buffer of at least one additional set-point. The next set-point is not sent until the previous one has been processed, executed and reached.

The hand-shake is intended for the single set-point set-up as we have had it implemented. The procedure should look as follows:

• Send target set-point.
• Set set-point bit in controlword.
• The drive responds with set-point acknowledge in statusword and begins movement.
• Apparently, we need to reset the set-point bit in controlword upon receiving the set-point acknowledge so that more set-points can be sent.

Edit: done at https://github.com/roboticslab-uc3m/yarp-devices/commit/0d104f68cedff3eb2cb5095098af882d757cc5b0.

[PeterBowman]

Made the following experiment via YARP2CAN ports (https://github.com/roboticslab-uc3m/yarp-devices/issues/160):

• Begin in position profile mode, move some joint to a random position.
• Switch to external reference torque mode.
• Don't do anything.
• Reset bit 4 of controlword (external reference torque mode inactive).
• Switch back to position profile mode (6060h).
• Set new set-point bit in controlword (bit 4), this should start the movement.

Two things can happen now:

• The motor keeps its current position.
• The motor goes full bananas and the drive dies.

I noticed both behaviors are reproducible. The latter situation is easier to replicate if the target position (object 607Ah, used as reference in position profile mode) differs from the actual encoder read, probably because you spinned the motor manually when in torque mode.

I managed to avoid this. Before transitioning from torque to position mode, request the SWITCHED_ON drive state and then go back to OPERATION_ENABLED.

[PeterBowman]

Mostly ready and tested at https://github.com/roboticslab-uc3m/yarp-devices/commit/8872f4077fbf8be1b0b63ea775ad02cfe06d0fc0. This is hopefully robust enough (tested via RPC and yarpmotorgui), but I'm going to contact support anyway to learn how the drive's internals behave.

To sum up, we want to transition back and forth between four modes of operation (YARP control mode mappings aside):

from\to	pp	pv	csp	ert
pp	N/A	✔️	✔️	✔️
pv	✔️	N/A	❌	✔️
csp	✔️	✔️	N/A	✔️
ert	❌	✔️	❌	N/A

Where:

code	iPOS mode of operation	YARP control mode
pp	profile position	IPositionControl
pv	profile velocity	IVelocityControl
csp	cyclic synchronous position	IPositionDirect/IVelocityControl
ert	external reference torque	ITorqueControl/ICurrentControl

Green ticks (✔️) mean seamless transitions in "Operation enabled" state, whereas red crosses (❌) mean abnormal behavior, e.g. sudden motor spin and transition to fault state. Implemented solutions:

• pv to csp: request position profile mode first, then proceed as usual
• ert to pp/csp: request transition to "Switched on" state, then proceed as usual

Also, I expanded the latter to the ert-to-pv case for completeness, I just don't fully trust the iPOS drives.

Important remark: motors are free to rotate (i.e. limbs may fall due to gravity) when switching from ert to any other mode due to the "Switched on" state transition.

[PeterBowman]

red crosses (x) mean abnormal behavior, e.g. sudden motor spin and transition to fault state

This has been reported to Technosoft support and confirmed by them, we have found a firmware bug. The suggested and working solution is to request homing mode upon leaving external reference torque, then switch to the desired mode of operation. This is highly convenient since spares us from transitioning to "Switched on" state, which results in loss of motor control for a brief while.

I have implemented this in https://github.com/roboticslab-uc3m/yarp-devices/commit/ece1971ee4f75c57b082a3d338424de925ed8031, and also used this same solution in the pv-to-csp transition.