Implement cyclic synchronous position (CSP) mode

[PeterBowman]

In #198, an attempt was made to try the CSP mode of operation (#198 (comment)):

The Technosoft iPOS CANopen Programming User Manual (ref. P091.063.iPOS.STO.UM.0117, 2017) adds a Cyclic Synchronous Position mode (CSP) in section 10:

With this mode, the trajectory generator is located in the control device, not in the drive device. In cyclic synchronous manner, it provides a target position to the drive device, which performs position control, velocity control and torque control. Measured by sensors, the drive provides actual values for position, velocity and torque to the control device.

Section 9.2.1, Object 60C0h: Interpolation sub mode select (in Interpolated Position Mode), states that PT and PVT submodes should be regarded as legacy stuff. The preferred submode is introduced as "linear interpolation":

Linear interpolation as described in the CiA 402 standard (when object 208Eh bit8=1); This mode is almost identical with Cyclic Synchronous Position mode, only that it receives its position data into 60C1h sub-index 01 instead of object 607Ah. No interpolation point buffer will be used.

From About This Manual:

The iPOS drives are conforming to CiA 301 v4.2 application layer and communication profile, CiA WD 305 v.2.2.131 Layer Setting Services and to CiA (DSP) 402 v4.0 device profile for drives and motion control, now included in IEC 61800-7-1 Annex A, IEC 61800-7-201 and IEC 61800-7-301 standards.

P091.063.iPOS.UM.0615 (2015) states that:

The iPOS drives are confirming to CiA 301 v4.2 application layer and communication profile, CiA WD 305 v.2.2.131 Layer Setting Services and to CiA DSP 402 v3.0 device profile for drives and motion control (...)

So, no CSP mode in CiA DSP 402 v3.0. We'd probably need to perform a firmware update on our drives in order to use this mode.

Interestingly, from section 10.2.1, Object 60C2h: Interpolation time period:

The Interpolation time period indicates the configured interpolation cycle time. Its value must be set with the time value of the CANopen master communication cycle time and sync time in order for the Cyclic Synchronous Position mode to work properly.

Remark: due to the limitations of the CAN network, it is recommended that the interpolation time period should not be set lower than 4 ms.

Besides, in 10.1.1, Controlword in Cyclic Synchronous Position mode (CSP):

In Relative position mode, the drive will add to its current position the value received in object 607Ah. By sending this value periodically and setting the correct interpolation period time in object 60C2h, it will be like working in Cyclic Synchronous Velocity mode (CSV).

Tested again via object 6502h, Supported drive modes: #198 (comment).

Also learned that... (#198 (comment))

Regarding CSP mode... Just discovered object 208Eh, Auxiliary Settings Register:

• Bit 8 = 0: Set interpolation mode compatible with PT and PVT (legacy)
• Bit 8 = 1: Set interpolation mode (when 6060=7) as described in the CiA402 standard

Default is 1, but this object does not exist in earlier versions of the iPOS user manual.

And, more recently... (ref)

In case you are in (...) synchronous position mode, you either must set a maximum velocity in object 6081h or disable the "Limit maximum speed at" from Drive Setup (see picture below).

If either of these settings are not done, the motor will not move in Cyclic Synchronous Position mode. Letting the setup setting ON, a maximum velocity can be imposed using object 6081h Profile Velocity between position points received in object 607Ah Target Position.

[PeterBowman]

I can confirm object 6061h ("Modes of Operation Display") reports CSP mode is entered successfully with #229 and firmware F508M/F509M (tested on CAN ids 19 and 20), despite object 6502h ("Supported drive modes") hinting it was not possible. I did not change any EasySetup configuration.

For the record, iPOS firmware upgrade was tracked at https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/42.

[PeterBowman]

CSP mode successfully tested. I was going to retain old (legacy) interpolation submodes. However, the online position-direct implementation is more than enough (and probably more robust) via CSP, hence I'll probably desist. Offline trajectory execution through legacy modes will be targeted in a new issue.

[PeterBowman]

While providing backward support to pt/pvt modes, I redesigned the way parameters are passed to subdevices via IRemoteVariables(Raw) iface. From the POV of CanBusControlboard, i.e. this is exposed via YARP ports:

• getRemoteVariablesList: lists all CAN node names, e.g. (id15-ipos id16-ipos ...)
  • RPC sample usage: [get] [ivar] [lvar]
• getRemoteVariable: retrieves all available key-value pairs for this node; calls getRemoteVariablesListRaw to get keys and then iterates on getRemoteVariableRaw
  • RPC sample usage: [get] [ivar] [mvar] id15-ipos
• setRemoteVariable: requires a key-value two-element bottle, value is a nested list
  • RPC sample usage: [set] [ivar] [mvar] id15-ipos (linInterp ((mode pt) (bufferSize 1)))

[PeterBowman]

Ready at 3a9fe3e. Old PT/PVT can be configured as described above, but it is currently disabled. Any attempts to switching into position-direct mode with these legacy submodes enabled will lead to a failure. Also, I noticed a persisting integrity counter error and abnormal (= fierce) joint motion; ignoring this as explained in #222 (comment).

Offline usage of PT/PVT will be targeted at #245.

[jgvictores]

While providing backward support to pt/pvt modes, I redesigned the way parameters are passed to subdevices via IRemoteVariables(Raw) iface. From the POV of CanBusControlboard, i.e. this is exposed via YARP ports:

I added this at bd1a72b (the page has a disclaimer that the protocols may be subject to change anyway).

[PeterBowman]

I just implemented joint velocity checks in CSP mode at 9fb6962. Robot arms behave horribly when controlled via spnav mouse. I presume this is caused by the cartesian controller sending commands that assume the robot has achieved the desired target, thus losing the actual reference (see roboticslab-uc3m/kinematics-dynamics#173 (comment)). As a result of this, the arm motion is erratic and goes totally bananas (techie term) when it is told to move faster. Note maxVel is configured at a notably high 50 degrees/second threshold, even though it seems not enough at all.

Issue #188 derived from a long discussionmonologue at roboticslab-uc3m/kinematics-dynamics#173. Velocity checks were added at the joint command level (namely, in this repository). Now I think only callers should be responsible for performing those checks, i.e. the cartesian controller for the case being.

I did a quick search across the robotology org. IControlLimitsRaw::setVelLimitsRaw is implemented in iCubSimulationControl, CanBusMotionControl and GazeboYarpControlBoardDriver. Their robot board does not support joint velocity limit commands, see ref ("internally cached, not sent to the fw yet"). Ours doesn't, too.

cc @jgvictores for science. ATOW, we use maxVel in velocity (ref) and position-direct/CSP commands (ref), and incidentally in IPositionControl::setRefSpeed (ref) as well.

[PeterBowman]

I just implemented joint velocity checks in CSP mode at 9fb6962. Robot arms behave horribly when controlled via spnav mouse.

Guess what, it was my fault and commit ef6e264 easily fixed this. Of course, high-level apps would still suffer from this very same problem in case high-velocity targets are commanded to the robot boards. So, @jgvictores and I concluded such high-level apps (e.g. the cartesian motion controllers) should always perform position/velocity checks on resulting joint commands prior to sending them down to the joint controllers. Thus, low-level checks are per-joint mandatory lifeboats, but when coordinated motion is required, high-level should also clip those values (and probably perform FK/IK in cartesian space) in order to achieve correct behavior (e.g. linear movement).