Implement Technosoft::stopRaw

[PeterBowman]

Not implemented yet (click), thus we cannot issue a set sto / set stos RPC command (which is relevant for safety reasons).

Remember to remove this at kin-dyn when done (BasicCartesianControl::stopControl).

[PeterBowman]

This is a complex task. Extensive knowledge about the state model of the iPOS is required in order to transition to the fault state (which, if I remember correctly, commands motor stop) and back. See Section 5 in the iPOS user manual.

[jgvictores]

Shouln't be too complex... The basic commands of the state machines of Section 4 and 5 are currently already implemented in TechnosoftIpos/ICanBusSharerImpl.cpp. Note that some transitions are performed via the same command, namely readyToSwitchOn == shutdown as far as I recall and documented.

I would, however, block this issue by two tasks (please ref to other issues if they are already posted):

• Chop ICanBusSharerImpl.cpp and something like a new ICanOpenImpl.cpp and ICanBusSharerImpl.cpp (the latter could actually be killed via put() of a pointer, possibly said at Replace current ICanBusHico interface with yarp::dev::ICanBus #122 or some other issue).
• We also have some issue about replacing the current hard-coded delays with actual checks of the state machine.

As a side note, I've posted #171 which, related to #162 could change many things in the long-run.

Would have no problem in diving deeper into this after IROS deadline. :-)

[PeterBowman]

I mean, the TechnosoftIpos devices must be aware at any time of their internal driver state, thus some FSM solution looks mandatory. We do traverse the states ATM, but only upon initialization and shutdown, which is fairly straightforward.

[jgvictores]

Oh, ok. Understood.

[PeterBowman]

From 5.1 CiA402 State machine and command coding (P091.063.iPOS.STO.UM.0117, 2017):

Transition	Event	Action
11	Bit 2 is cancelled in Controlword (Quick Stop command)	The drive will execute the instructions from Object 605Ah: Quick stop option code.
12	Quick Stop is completed or bit 1 is cancelled in Controlword (Disable Voltage command)	Output stage is disabled. Motor has no torque.
16	Bit 2 is set in Controlword (Enable Operation command). This transition is possible if Quick-Stop-Option-Code is 5, 6, 7 or 8	Drive exits from Quick Stop state. Drive function is enabled.

Quick Stop active state description:

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.

5.3.5 Object 605Ah: Quick stop option code:

This object determines what action should be taken if the quick stop function is executed. The slow down ramp is a deceleration value set by the Profile acceleration object, index 6083h. The quick stop ramp is a deceleration value set by the Quick stop deceleration object, index 6085h.

Value	Description
0	Disable drive function
1	Slow down on slow down ramp and transit into Switch On Disabled
2	Slow down on quick stop ramp and transit into Switch On Disabled
5	Slow down on slow down ramp and stay in Quick Stop Active
6	Slow down on quick stop ramp and stay in Quick Stop Active

Default is 2. So, we need to configure object 605Ah to value 6 on initialization, and implement stopRaw so that Control Word is set to XXXX.XXXX.0XXX.X01X. Then, transition back to Operation Enabled (watch bit 5 of Status Word: Quick Stop. When this bit is zero, the drive is performing a quick stop).

[PeterBowman]

watch bit 5 of Status Word: Quick Stop. When this bit is zero, the drive is performing a quick stop

The Operation enabled to Quick stop transition seems instantaneous. I could either add a tiny delay or watch Status Word for state transitions, but it seems to work nice regardless of that. Adding a 1 ms delay ASWJ.

Tested on both position and velocity control with the real robot. Have to update BasicCartesianControl at kinematics-dynamics; we'll no longer perform a control mode change, which was a workaround for the missing stop implementation.

[PeterBowman]

Added ICanBusSharer::initialize to perform SDO-only requests on the pre-operational NMT state. This method is default-defined to return true and overriden by TechnosoftIpos to set the quick stop option code as described above.

Done at 4e3ca34.

[PeterBowman]

The "halt" command in controlword seems to achieve the same result without the need for switching drive states back and forth. According to the user manual, this bit is usually related to the "position reached" bit in statusword, i.e. an active halt command signalizes that the drive has ended decelerating when said bit is set. In other words, checkMotionDone may be used right after the halt command to detect stopping motion end; next, the halt bit should be reset.

[PeterBowman]

For now, I'm giving up on the halt command. Even though it seems to fit better our purposes (a single write action that does not involve a state change), the need to reset this controlword is a major drawback. We could do this in two situations:

1. Within stopRaw(): perform a status motion checks that awaits motion completion, then reset this bit. I'd need to put a while loop somehow and use a hardcoded mini-delay. stopRaw() should not block!

2. In every motion command: prior to sending the new target, reset this bit in controlword. In the worst case, I'd need to query the controlword first, then adjust bits accordingly.

The current solution looks like this:

yarp-devices/libraries/YarpPlugins/TechnosoftIpos/IPositionControlRawImpl.cpp

Lines 253 to 254 in 9e65b22

	return can->driveStatus()->requestTransition(DriveTransition::QUICK_STOP)
	&& can->driveStatus()->requestTransition(DriveTransition::ENABLE_OPERATION);

Despite that double state change, the controller stops motion and is ready for processing the next target right after it leaves stopRaw(). Plus, I managed to remove any hardcoded delays (cf. develop branch). What could go wrong: the second transition fails and the controller is stuck in "Quick stop active" state.

I wonder if the quick stop ramp is too aggressive (i.e. the deceleration rate is quite high).

[PeterBowman]

Idea: leave DriveStatusMachine::requestTransition(DriveTransition::QUICK_STOP) in stopRaw(), then go back to "Operation enabled" in TechnosoftIpos::interpretStatusword when the transition completes. Pros: immediate action in stopRaw(), no delay, automatic comeback to enabled state. Cons: if any client is bursting invalid targets, it won't just quit doing so when we hit the stop button as new targets will be sent again. Guess that's why we have an idle mode?

[PeterBowman]

Cons: if any client is bursting invalid targets, it won't just quit doing so when we hit the stop button as new targets will be sent again. Guess that's why we have an idle mode?

Yes, the stopRaw() method only makes sense when the client itself is the main caller. Even if I'd monitor an external application via yarpmotorgui, I can't guarantee that the client will quit doing silly stuff after I press stop. In that case, the only way to make sure it really halts is changing drive state via idle control mode or pressing the red (emergency) button.

[PeterBowman]

Ready at ad3855e:

yarp-devices/libraries/YarpPlugins/TechnosoftIpos/IPositionControlRawImpl.cpp

Lines 250 to 255 in ad3855e

	bool TechnosoftIpos::stopRaw(int j)
	{
	CD_DEBUG("(%d)\n", j);
	CHECK_JOINT(j);
	return can->driveStatus()->controlword(can->driveStatus()->controlword().set(8));
	}

This changeset also spans to the API of DriveStatusMachine and restores previously deleted stuff. To sum up:

• The halt bit in controlword really seems the best fit for an instantaneous stop action.
• The quick stop transition looks more to me as an emergency stop button.
• The default behavior for the halt command is: "Slow down on quick stop ramp and stay in Operation Enabled" (Object 605Dh, Halt option code).
• On the other hand, the quick stop command will "Slow down on quick stop ramp and transit into Switch On Disabled" as defined in Object 605Ah, Quick stop option code.
• I'll reserve quick stop command for emergency actions, therefore I'd rather pick the defaults described above (the previous implementation of stopRaw assumed "Slow down on quick stop ramp and stay in Quick Stop Active" as the preferred quick stop behavior).
• By the way, quick stop ramp seems too aggressive and may even cause the drives to reset.
• The stopRaw() function must not block. In any case (either halt bit or quick stop transition), an external observer should keep track of drive's internals and restore motion after the stop action completes.
• That's why DriveStatusMachine stores the last requested controlword for future inspection, and an additional check was added in positionMove and others to check the halt bit and reset it if necessary prior to sending the proper motion command.
• Users may tweak specific bits of the controlword while keeping state of the remaining bits, see stopRaw implementation above.
• Within controlword, some bits refer to the state machine and some others point at miscellaneous behavior (e.g. the halt command). DriveStatusMachine will update these bits along with statusword and adjust them accordingly so that individually tweaked bits (see previous point) would not cause a state transition.
• Conversely, requesting a state transition will not alter non-state-related controlword bits.

[PeterBowman]

I decided to restrict stop(Raw) to position and velocity profile modes, given the availability of said method in YARP position and velocity control interfaces: 79a475e. There is no support drive-side for halt bit in CSP mode. Also, while external reference torque mode does support it, there is no such method in ITorqueControl. Also, setting the halt bit means applying zero torque anyway.

[PeterBowman]

CiA 402-2 in Table 26 – Transition events and actions states that "it is not recommended to support transition 16", i.e. from "Quick stop active" to "Operation enabled".