Implement PVT mode through IRemoteVariables

[PeterBowman]

Stemming from https://github.com/roboticslab-uc3m/yarp-devices/issues/198, this issue aims to "restore" and enhance support for iPOS-specific interpolation modes. Restore: PT mode used to be accessed via yarp::dev::IPositionDirect, the external reference position mode has taken its place. Enhance: PVT looks better than PT, so let's go for it.

Ideas and roadmap (from https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-487386797):

• Enable PVT interpolation via IRemoteVariables:
  • On the YARP side, use VOCAB_CM_MIXED a.k.a. mixed mode (iCub reference).
  • Send a list (bottle) of PVT targets via IRemoteVariables.
  • Send a start signal along with this bottle. Mind synchronization across joints (first fill the buffer, then start motion).
  • Progressively fill the internal iPOS buffer using an intermediate buffer and a periodic thread.
  • Idea: increase the PVT and ICanBus buffer sizes.
  • If only PT information is available, estimate instantaneous speed between consecutive targets.
  • Set zero speed on the last target, but allow more points to be appended via IRemoteVariables::setRemoteVariable.
  • Enable IPositionControl::checkMotionDone and ::stop (https://github.com/roboticslab-uc3m/yarp-devices/issues/120) in this mode.

Pointers given by Technosoft support: https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-487279910.

Mind this issue (from https://github.com/roboticslab-uc3m/yarp-devices/issues/198):

It takes ages to switch to position direct mode ("seems like a must"). Certain applications start sending targets asynchronously right away after the mode change (e.g. keyboardController), thus the first actually commanded position happens after said delay and makes the robot jump too far away.

[PeterBowman]

This might be the right time to increase the size of TX/RX CAN buffers, see https://github.com/roboticslab-uc3m/yarp-devices/issues/159.

[PeterBowman]

Note: check velocities against the minVel/maxVel configuration parameters (https://github.com/roboticslab-uc3m/yarp-devices/issues/188).

[PeterBowman]

Proposed PVT message protocol (single waypoint): (time) ((pos_0) (vel_0)) ((pos_1) (vel_1))....

Example: ( ( t 50 ) ( ( p 2.0 ) ( v 3.5 ) ) ( ( p 3.0 ) ( v 1.5 ) ) ) ( ...next waypoint... )

Implications:

• same time interval across all joints
• tagged properties (p, v), velocity is optional
• easier to construct on client-side (all joints per waypoint), harder to parse on server-side (split list of waypoint per joint, pass on to each motor device)
• not sure whether the time property (expressed in milliseconds) should be of integer type
• considered a joint number property (e.g. ((j 0) (p ...) (v...))), probably redundant
• drop outer parentheses (and p tag?) in case no velocity property is present?

Alternatively: (t <time>) (p <pos0> <pos1>...) (v <vel0> <vel1>...) where v is optional.

[PeterBowman]

Mostly implemented:

• CanBusControlboard: (t <time>) (p (<pos0> <pos1>...)) (v (<vel0> <vel1>...)) where v is optional
• TechnosoftIpos: (<time> <pos> <vel>) where <vel> is optional

[PeterBowman]

not sure whether the time property (expressed in milliseconds) should be of integer type

Per 9.2.7, "Object 207Ah: Interpolated position 1st order time":

It is given in IU which is by default 0.8ms for steppers and 1ms for the other configurations.

Data type is UNSIGNED16. This means that we (for most cases) won't achieve a better resolution than 1 millisecond (shall I take into account a stepper configuration?), therefore the time property as described above, as well as all derived C/C++ variables used to store this representation in memory, can fit into an integer-type object which could be narrowed down to uint16_t, even.

[jgvictores]

shall I take into account a stepper configuration?

I do not think this is necessary.

[PeterBowman]

Closing as WONTFIX. PVT mode and custom buffer sizes are now supported thanks to https://github.com/roboticslab-uc3m/yarp-devices/pull/220. Users are encouraged to loop over recorded targets and send them individually, either in a fixed or arbitrary period, via VOCAB_CM_POSITION_DIRECT mode.

Additional notes:

• yarp::dev::IRemoteVariables is bugged: https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-495322968.
• In spite of Technosoft support's advices, it looks like the buffer size limits are not so high as expected: https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-495938645. Also, keep in mind that x points sent in one burst per joint means x*6 messages quickly transmitted via CAN network, thus easily leading to saturation.