Leverage CANopen & general improvements

[PeterBowman]

Project [CAN-TEO] gathers several tickets which aim to take full advantage of CANopen protocols with a brand new application layer (#223). Apart from the usual refactorization and reduction of code bloat, it is expected to achieve new functionalities and improve existing ones. This ticket tracks said improvements.

[PeterBowman]

The SDO protocol is a confirmed service, that is, any message we send to a CAN node generates a response. CANopen docs use the terms request/response for CAN reads, and indication/confirm for CAN writes.

In the develop branch, CAN reads (from the CAN master POV) employ a tiny hardcoded delay hoping to receive the expected response before it elapses. I noticed that this delay is too small for certain operations, perhaps always. On the other hand, CAN writes do never expect a confirm message.

Issue #223 adds the new SdoClient class (name is not final). It encapsulates reads and writes in a user-friendly manner so that there is no need to worry about command specifiers, data lengths and such. Issue Issue #159 implements a semaphore-based wait-with-timeout mechanism that turns the hardcoded delays obsolete. With these improvements, all SDO transfers are checked for the incoming response/indication message. In case a timeout occurs, or a SDO abort transfer message arrives (previously not implemented at all), relevant methods return a false boolean.

Similarly, a similar wait-with-timeout mechanism has been applied on CiA 402 state machine transitions (switch on, enable, shutdown...) and made several 1-2 second hardcoded delays vanish.

[PeterBowman]

CanBusControlboard has always inherited from a yarp::os::Thread class to implement a CAN read thread. In #209, this has been moved to a separate class and replicated so that CAN writes are orquestated from their own thread, too. This aimed to move all read/write operations to the CanBusControlboard wrapper, and to make the raw subdevices focus on interpreting incoming messages (read thread) and registering the outgoing ones (write thread). Both cases involve the usage of CAN RX/TX buffers for the first time (PeakCAN-software side). The latter case exploits this fact extensively: subdevices place message data in available consecutive slots within said buffer; then, the write thread checks periodically for awaiting messages and send them in one batch.

Issue #226 will allow multiple read/write threads, one per CAN bus.

[PeterBowman]

The ControlBoardWrapper class queries the following data on each RateThread::run iteration (ref1, ref2):

• IEncodersTimed::getEncodersTimed -> p->getEncodersTimed(double *, double *)
• IEncoders::getEncoderSpeeds -> p->getEncoderSpeeds(double *)
• IEncoders::getEncoderAccelerations -> p->getEncoderAccelerations(double *)
• IMotorEncoders::getMotorEncoders (Implement additional yarp::dev motor interfaces #157) -> p->getMotorEncoders(double *)
• IMotorEncoders::getMotorEncoderSpeeds -> p->getMotorEncoderSpeeds(double *)
• IMotorEncoders::getMotorEncoderAccelerations -> p->getMotorEncoderAccelerations(double *)
• IPWMControl::getDutyCycles -> p->getDutyCycles(double *)
• ICurrentControl::getCurrents -> p->getCurrents(double *)
• ITorqueControl::getTorques -> p->getTorques(double *)
• IControlMode::getControlModes -> p->getControlModes(int *)
• IInteractionMode::getInteractionModes -> p->getInteractionModes(int *)

All of these map to array-like methods (as opposed to single-joint and joint-group signatures).

The only ones that entail a CAN network transfer and do not derive from the others are bolded. Both motor encoder reads and actual currents can fit a single TPDO frame. I'm yet to decide on whether control modes should either be handled via local variable or await a TPDO event.

[PeterBowman]

Control modes YARP queries are no longer forwarded to the CAN network, that is, no CAN request is generated upon getControlMode(). Now, control mode changes are listened upon and stored in an internal variable via TPDO event callback. See #215 (comment).

[PeterBowman]

According to iPOS CAN user manual (2019), these are the relevant dictionary objects we (may) want to listen to via TPDO:

• 6041h, Statusword: UNSIGNED16. Defines current drive state (among other relevant bits), already handled.
• 6061h, Modes of Operation Display: INTEGER8. Already handled.
• 1002h: Manufacturer Status Register: UNSIGNED32 (comprises object 6041h, Statusword). Most importantly, reports I2t warning status.
• 2000h, Motion Error Register (MER): UNSIGNED16. Defines most contingencies leading to a fault state.
• 2002h, Detailed Error Register (DER): UNSIGNED16. Complements MER with additional error info.
• 2009h, Detailed Error Register 2 (DER2): UNSIGNED16. Displays detailed information linked to the command feedback error bit in MER. Allegedly available in F514x, only.
• 2003h: Communication Error Register (CER): UNSIGNED16. Expands on CAN/SPI/SCI communication errors.
• From section 5.5, Digital I/O control and status objects: 60FDh, 208F, 60FEh, 2090h, 2045h, 2102h, 2046h, 2047h and 2055h. These objects relate to digital I/O, analogue inputs and brake status. If possible, it would be nice to leave some PDO room even if we are not going to use them.
• From that same section, see object 2058h, Drive Temperature: UNSIGNED16. All I want for Christmas is a temperature sensor.
• 6064h, Position actual value: INTEGER32. Provides position measurements in user-defined units, which boils down to encoder counts. I'm considering to use 6063h, Position actual internal value instead:

  This object represents the actual value of the position measurement device in increments.

• 207Eh, Current actual value: INTEGER16. We want this badly. User manual says only available with firmwares F508I/F509I and above; seemingly, it works on our hardware, though. Object 6077h, Torque actual value reflects the same value as confirmed by Technosoft support.

Certain error registers are forwarded from the drives within an EMCY message in case a fault condition arises, see section 4.1.4.1, Emergency message structures:

• 2003h, Communication Error Register (CER): on 0x7500, "Communication error"
• 2009h, Detailed Error Register 2 (DER2): on 0x7300, "Sensor error"
• 2072h, Interpolated position status: on 0xFF01, "Generic interpolated position mode error (PVT / PT error)"

[PeterBowman]

Technosoft support has confirmed that object 1002h, Manufacturer Status Register can be indeed mapped to a PDO.

[PeterBowman]

I'm going to remove the iPOS-product-code-to-drive-peak-current mappings so that this value is parsed from .ini file. For future reference (using amperes):

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

Lines 19 to 66 in f71f070

	switch (productCode)
	{
	case 24300101: // iPOS2401 MX-CAN
	case 24200121: // iPOS2401 MX-CAT
	*peakCurrent = 0.9;
	break;
	case 28001001: // iPOS3602 VX-CAN
	case 28001021: // iPOS3602 VX-CAT
	case 28001101: // iPOS3602 MX-CAN
	case 28001201: // iPOS3602 BX-CAN
	case 28001501: // iPOS3602 HX-CAN
	*peakCurrent = 3.2;
	break;
	case 28002001: // iPOS3604 VX-CAN
	case 28002021: // iPOS3604 VX-CAT
	case 28002101: // iPOS3604 MX-CAN
	case 28002201: // iPOS3604 BX-CAN
	case 28002501: // iPOS3604 HX-CAN
	*peakCurrent = 10.0;
	break;
	case 27014001: // iPOS4808 VX-CAN
	case 27014101: // iPOS4808 MX-CAN
	case 27014121: // iPOS4808 MX-CAT
	case 27414101: // iPOS4808 MY-CAN (standard)
	case 27424101: // iPOS4808 MY-CAN (extended)
	case 27314111: // iPOS4808 MY-CAN-STO (standard)
	case 27324111: // iPOS4808 MY-CAN-STO (extended)
	case 27314121: // iPOS4808 MY-CAT-STO (standard)
	case 27324121: // iPOS4808 MY-CAT-STO (extended)
	case 27014201: // iPOS4808 BX-CAN
	case 27214201: // iPOS4808 BX-CAN (standard)
	case 27214701: // iPOS4808 BX-CAN (hall)
	case 27214221: // iPOS4808 BX-CAT (standard)
	case 27214721: // iPOS4808 BX-CAT (hall)
	case 27314221: // iPOS4808 BX-CAT-STO (standard)
	case 27314721: // iPOS4808 BX-CAT-STO (hall)
	case 29025201: // iPOS8010 BX-CAN
	case 29025221: // iPOS8010 BX-CAT
	case 29025202: // iPOS8010 BA-CAN
	case 29025222: // iPOS8010 BA-CAT
	*peakCurrent = 20.0;
	break;
	case 29026201: // iPOS8020 BX-CAN
	case 29026221: // iPOS8020 BX-CAT
	case 29026202: // iPOS8020 BA-CAN
	case 29026222: // iPOS8020 BA-CAT
	*peakCurrent = 40.0;
	break;

[jgvictores]

Makes sense!

[PeterBowman]

Proposed transmit PDO mappings:

• TPDO1: 1002h, Manufacturer Status Register, MSR (4 bytes; includes 6041h, Statusword in the LSB half) + 6061h, Modes of Operation Display (1 byte) = 5 bytes

• TPDO2: 2000h, Motion Error Register, MER (2 bytes) + 2002h, Detailed Error Register, DER (2 bytes) = 4 bytes

• TPDO3: 6064h/6063h, Position actual (internal) value (4 bytes) + 6077h/207Eh, Torque/Current actual value (2 bytes) = 6 bytes

TPDO3 is meant for time critical data streams. For now, I will set a tiny event timer value in async mode and no inhibit time in order to simulate the final desired behavior in synchronous operation (via SYNC signal). Issue #223 will expand on that matter. There is room for two additional bytes which could fit the output of a temperature sensor (wink wink @smcdiaz) stream via 2058h.

TPDO1 and TPDO2 are both event driven PDOs. I feel that grouping status-related stuff together is probably more efficient given that such events are prone to fire at any time, as opposed to casual emergency messages linked to fault states that hopefully don't arise that often. Hence, TPDO1 will host statusword and the MSB half of MSR - these are also state-related bits - as well as current mode of operation. On TPDO2, MER/DER should fire on special events and both provide related information.

We are probably fine in reusing the default TPDO configuration, i.e. no event timer and a 30 ms inhibit time. That is, if and only if a bit changes in either PDO, it will produce a message at least 30 milliseconds away from the previous one. In case I decide to set an event timer of e.g. 1 second, those PDOs will send data with such a period, and more often if inhibit time is set as well.

Finally, TPDO4 is vacant and might host IO events in the future.

[PeterBowman]

Ready at b31442b, I'm going to revisit this issue after SYNC is implemented and applied to TPDO3 (#223).

[PeterBowman]

From that same section, see object 2058h, Drive Temperature: UNSIGNED16. All I want for Christmas is a temperature sensor.

Oh.

The ControlBoardWrapper class queries the following data on each RateThread::run iteration (ref1, ref2): (...)

(#232 (comment)) I should mark IMotorEncoders::getMotorEncoderSpeeds with bold type due to #189 (comment). Thus, room would be made for additional 4 bytes of Object 6069h, "Velocity sensor actual value" or Object 606Ch, "Velocity actual value" (#232 (comment)).

Edit: considering #189 (comment) and given that there is no way to query instantaneous acceleration from the drive and we want this info, I'd stick to the current TPDO config.

[PeterBowman]

Also, it is interesting to note that iPOS firmware provides a low pass filter that can be applied on a 16bit variable value, see Object 2108h. By default, this is configured to filter motor current and can be mapped to a PDO.

[PeterBowman]

It is important to note that the default inhibit time (30 milliseconds) causes some crucial messages to be lost, e.g. this may occur when modes of operation change twice before this time elapses. It happened to me upon switching from idle mode to position control mode. I did process the statusword callback correctly, but another message was supposed to be generated due to the mode transition, which got lost in the meantime (I presume it was attempted to be broadcast before those 30 milliseconds elapsed).

Edit: this problem had nothing to do with the alleged cause and has been solved at 5a55c83.

Edit2: see a460407 (attending to motion completion after halt command is issued).

[PeterBowman]

Drives are currently configured to stream joint state (encoder reads & current measurements) via TPDO3. This PDO is event-driven, no event timer, default inhibit time (for now) of 30 milliseconds. It means that any new encoder read or current measurement will be broadcast from the drive no sooner than 30 milliseconds after the last message sent. This copes well with our buffering mechanisms, and it is convenient to remember that encoder reads are highly unstable, i.e. the drives are steadily reporting minor variations (hence, in the practice, TPDO3 will stream data at the inhibit time rate despite the motor not being commanded at all).

A question arises: is it convenient to SYNC this particular TPDO? If we take a look at how those reads and measurements are consumed, we notice that the only caller is controlboardwrapper2's periodic thread. Even if we achieve synchronous data streaming across all joints, those data will be consumed at a later time (milliseconds apart). Each drive will report a different acquisition time, so the question boils down to determining whether simultaneousness is a pressing matter here. Client code (YARP-side) will always get the precise instant of data arrival via IEncodersTimed. Should this be the same instant across all joints (even across all parts of the robot, in a whole-body launchCanBus configuration)?

A positive answer yields another important question: since all synchronous TPDOs obey the same SYNC signal, and we are sure we want to tell TPDO3 to stream at the SYNC rate, how can other PDOs (RPDO/TPDO) play with this config so that a different rate is used for their specific case, in case of need?

See: Super-Nyquist Theorem.

[PeterBowman]

A question arises: is it convenient to SYNC this particular TPDO?

Synchronization helps design a CAN bus that copes well with traffic. Event-driven PDOs make this highly unpredictable. Also, syncing is not as hard as I initially thought.

A positive answer yields another important question: since all synchronous TPDOs obey the same SYNC signal, and we are sure we want to tell TPDO3 to stream at the SYNC rate, how can other PDOs (RPDO/TPDO) play with this config so that a different rate is used for their specific case, in case of need?

PDOs can be configured in a cyclic synchronous manner, i.e. accept/stream data every 2/3/4... SYNC signals.

[PeterBowman]

Chapter 6, "Factor group" in iPOS CANopen user manual (2019) looks appealing:

The iPOS drives family offers the possibility to interchange physical dimensions and sizes into the device internal units.

This set of objects could replace several unit-conversion methods from the StateVariables class and move those calculations to the iPOS drives, perhaps with greater precision (see 6.1.10.1, "Setting the numerator and divisor in a factor group object. Example"). Also, object 607Eh, "Polarity" reverses the sign in several position and velocity control-related objects, which is currently performed by our CAN master code via multiplication by 1 or -1 (see StateVariables::reverse).

However, I'm not implementing this. There are several places across our codebase that would need a "manual" conversion anyway, that is, the iPOS drives do not apply such unit conversions and sign multiplications in all places we actually need them, hence the helper methods in StateVariables would never go away. Also:

Because the iPOS drives work with Fixed 32 bit numbers (not floating point), some calculation round off errors might occur when using objects 6093 h , 6094 h, 6097 h and 2071 h . If the CANopen master supports handling the scaling calculations on its side, it is recommended to use them instead of using the “Factor” scaling objects.

And:

In the CiA 402 standard, the dimension and notion index objects have been declared as obsolete.

[PeterBowman]

TPDO3 configured as cyclic synchronous (every 1 SYNC). The new syncPeriod option to CanBusControlboard enables the SYNC timer if present: f7e0cc3 + roboticslab-uc3m/teo-configuration-files@c4d3548 + roboticslab-uc3m/asibot-configuration-files@4be8206. The write message queues are processed as soon as a SYNC message has been registered.

I chose not to communicate CanBusControlboard with its wrapped CAN nodes (and vice versa), possibly via new methods in the ICanBusSharer interface, mind the KISS rule. Also, if users need to disable this SYNC timer (unused, e.g. all CAN nodes are fake nodes or not TechnosoftIpos devices), they can comment out the syncPeriod in the .ini file.

[PeterBowman]

Object 2103h, Number of encoder counts per revolution reports our encoderPulses (4096 in TEO). I'm not going to query this value because of:

Remark: this object will not indicate a correct number in case a Brushed DC motor is used.

Anyway, dropping the encoderPulses value from encoder .ini files is not a good idea.

[PeterBowman]

Velocity, torque and current commands are now forwarded to the drive via synchronous RPDOs: f4970a2.

[PeterBowman]

It is important to note that the default inhibit time (30 milliseconds) causes some crucial messages to be lost, e.g. this may occur when modes of operation change twice before this time elapses. It happened to me upon switching from idle mode to position control mode. I did process the statusword callback correctly, but another message was supposed to be generated due to the mode transition, which got lost in the meantime (I presume it was attempted to be broadcast before those 30 milliseconds elapsed).

Edit2: see a460407 (attending to motion completion after halt command is issued).

Event timer implemented and defaulted to 250 milliseconds at 79a475e and roboticslab-uc3m/teo-configuration-files@249edbe.

[PeterBowman]

Currently, a get encs RPC requests yield all-zeroes and a true result when drives are powered off, even when not at home position. This has been observed on launchCanBus --home.

• Commit 3dc7530 checks this and reports a failure unless drives have been initialized beforehand.
• Commit f32fbba prints a warning on failed initial (via --home) homing procedure instead of shutting down the process.

Sadly, yarpmanager doesn't like a getEncoders call yielding false and disables certain operations, e.g. it does not start the GUI and, when started, GUI indicators are no longer updated (drive state is not reflected, basically nothing works anymore). Therefore, I reverted the first commit at d59b6af; an upstream PR needs to fix yarpmotorgui first.

[PeterBowman]

Commit 3dc7530 checks this and reports a failure unless drives have been initialized beforehand.

ASWJ we'd better ask the drive for its current "control mode" and test against the VOCAB_CM_NOT_CONFIGURED value, which means drive off. Also, we found the yarp::dev::IEncoderArrays interface, part of the multiple analog sensors device group:

This interface is typically used for group of encoders that are not explicitly controlled by one of the interfaces typically used for motor control, such as IEncoders and IPositionControl, such as encoders measuring the complete state in an underactuated mechanism.

We are good to go with our current setup, that is, treat absolute encoders as tightly coupled with their TechnosoftIpos counterpart per joint (c.f. Dextra hands which actually could pose a nice use case for this MAS interface (if they actually had encoders per physical joint)).