search

roboticslab-uc3m / yarp-devices

A place for YARP devices
https://robots.uc3m.es/yarp-devices/
9 stars 7 forks source link

Leverage CANopen & general improvements #232

Closed PeterBowman closed 4 years ago

PeterBowman commented 4 years ago

Project [CAN-TEO] gathers several tickets which aim to take full advantage of CANopen protocols with a brand new application layer (https://github.com/roboticslab-uc3m/yarp-devices/issues/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 commented 4 years ago

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 https://github.com/roboticslab-uc3m/yarp-devices/issues/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 https://github.com/roboticslab-uc3m/yarp-devices/issues/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 commented 4 years ago

CanBusControlboard has always inherited from a yarp::os::Thread class to implement a CAN read thread. In https://github.com/roboticslab-uc3m/yarp-devices/issues/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 https://github.com/roboticslab-uc3m/yarp-devices/issues/226 will allow multiple read/write threads, one per CAN bus.

PeterBowman commented 4 years ago

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

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 commented 4 years ago

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 https://github.com/roboticslab-uc3m/yarp-devices/issues/215#issuecomment-537137651.

PeterBowman commented 4 years ago

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

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:

PeterBowman commented 4 years ago

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

PeterBowman commented 4 years ago

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):

https://github.com/roboticslab-uc3m/yarp-devices/blob/f71f070ffd1da66e76393f2faeecbe12188aee6f/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp#L19-L66

jgvictores commented 4 years ago

Makes sense!

PeterBowman commented 4 years ago

Proposed transmit PDO mappings:

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 https://github.com/roboticslab-uc3m/yarp-devices/issues/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 commented 4 years ago

Ready at https://github.com/roboticslab-uc3m/yarp-devices/commit/b31442bfe11877c32cad700ef628aa87a7a4d7a5, I'm going to revisit this issue after SYNC is implemented and applied to TPDO3 (https://github.com/roboticslab-uc3m/yarp-devices/issues/223).

PeterBowman commented 4 years ago

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): (...)

(https://github.com/roboticslab-uc3m/yarp-devices/issues/232#issuecomment-532871260) I should mark IMotorEncoders::getMotorEncoderSpeeds with bold type due to https://github.com/roboticslab-uc3m/yarp-devices/issues/189#issuecomment-568929803. Thus, room would be made for additional 4 bytes of Object 6069h, "Velocity sensor actual value" or Object 606Ch, "Velocity actual value" (https://github.com/roboticslab-uc3m/yarp-devices/issues/232#issuecomment-538049683).

Edit: considering https://github.com/roboticslab-uc3m/yarp-devices/issues/189#issuecomment-568931172 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 commented 4 years ago

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 commented 4 years ago

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 https://github.com/roboticslab-uc3m/yarp-devices/commit/5a55c83ed1b3fbb986a717195f623a21f9d7059d.

Edit2: see https://github.com/roboticslab-uc3m/yarp-devices/commit/a460407cd7de60c64f62bacd62134e68895031e2 (attending to motion completion after halt command is issued).

PeterBowman commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

TPDO3 configured as cyclic synchronous (every 1 SYNC). The new syncPeriod option to CanBusControlboard enables the SYNC timer if present: https://github.com/roboticslab-uc3m/yarp-devices/commit/f7e0cc36ddd44902ddb240eb112793d3b9106a1d + https://github.com/roboticslab-uc3m/teo-configuration-files/commit/c4d354816156778892e8a8737f1b6440c77a67ce + https://github.com/roboticslab-uc3m/asibot-configuration-files/commit/4be82062bbf431778524c1d04e9e69d769909e78. 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 commented 4 years ago

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 commented 4 years ago

Velocity, torque and current commands are now forwarded to the drive via synchronous RPDOs: https://github.com/roboticslab-uc3m/yarp-devices/commit/f4970a267ba8970f54cdd5ef8cfe2607f369c60c.

PeterBowman commented 4 years ago

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 https://github.com/roboticslab-uc3m/yarp-devices/commit/79a475ebfa0e46a5424a33a1076586fc1fff5bc0 and https://github.com/roboticslab-uc3m/teo-configuration-files/commit/249edbe02f9747fec05611e18c96876aa5d4a1ca.

PeterBowman commented 4 years ago

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.

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 https://github.com/roboticslab-uc3m/yarp-devices/commit/d59b6af62a168ae06cb788acf09348edb46cbbd0; an upstream PR needs to fix yarpmotorgui first.

PeterBowman commented 4 years ago

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)).