Offline position-direct implementation via PT/PVT

Stemming from https://github.com/roboticslab-uc3m/yarp-devices/issues/222, which drops legacy PT/PVT interpolation modes in favor of CSP for online commands (e.g. joystick teleoperation) since the single-element buffer solution was never a good choice anyway, I want to focus on offline usage to actually take advantage of said buffer.

Issues:

Setpoints need to be stored in memory prior to populating the buffer(s): keep in mind it will be pretty easy to overflow the 500-element PCan TX buffer despite having a lower PT/PVT buffer limit (285/222 setpoints, respectively). Those limits do not scale well since there are usually several nodes on the same bus (285 * 6 = ?).
In the same manner, either the CanBusPeak implementation or CAN RW threads of CanBusControlboard should internally manage a queue of messages that exceeds the hardware-limit (500 CAN frames). Currently, it is not allowed (via software) to register more messages into a full queue.

Possible usage:

Make sure position-direct mode is not active.
Configure PT/PVT mode via IRemoteVariables (https://github.com/roboticslab-uc3m/yarp-devices/issues/222#issuecomment-575095243).
Call IPositionDirect::setPositions successively in order to save desired setpoints into memory.
Load some or all of those points into the drive internal buffer (keep in mind bus load and buffer limits)
Start the motion: how?
Watch for buffer-low or buffer-empty states and replenish the PT/PVT drive buffer.

See https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-487279910 regarding PT/PVT buffer size and how to increase it. Default is 9 PT or 7 PVT points.

Start the motion: how?

Tiny grain of salt, regarding options we've contemplated in the past (I like the second one slightly better):

Fix a threshold to always send motion start command automatically upon Nth (e.g. 4th) element sent to the buffer.
Count on user starting to fill the buffer in other modes (e.g. position or something like idle), then send motion start command immediately upon switching to position direct mode.

PS: There was a third option, via IRemoteVariables, which itself can ramify into many options. I'm still more inclined to the second of above due to all the issues this would involve.

Current implementation looks promising, has been successfully tested via examplePositionDirect app.

Config:

Assumes max PT buffer size of 9 setpoints.
Assumes max PVT buffer size of 7 setpoints.
Buffer-low threshold set at 4 setpoints.

Workflow:

Whilst not in position-direct mode, users configure the interpolation mode via IRemoteVariables interface. Supported options are mode (either pt or pvt) and periodMs (interpolation time, in milliseconds):
- example via RPC (ref): set ivar mvar all (linInterp (enable 1) (mode pt) (periodMs 50))
- example via C++ API: https://github.com/roboticslab-uc3m/yarp-devices/blob/131168dde2a449dcde474e949ccc30528693eebc/examples/cpp/examplePositionDirect/examplePositionDirect.cpp#L136-L144
Upon switching to position-direct mode, the drive is configured to forward commands to an abstraction of the interpolation buffer (LinearInterpolationBuffer class) instead of using CSP setpoints.
Every call to IPositionDirect::setPosition pushes the arriving setpoint into an internal FIFO queue. Clients may want to either burst all offline-generated trajectories into this queue in a single shot or fill it gradually.
The first time this queue reaches the expected maximum buffer size (9 points for PT mode, 7 for PVT), as many points as can fit in the drive's buffer are sent to it. More points may keep arriving, but those will always land on the queue as an intermediate software buffer.
Object 2072h ("Interpolated position mode status") is monitored:
- on buffer full condition, if and only if it's the first time we send stuff into the buffer, a motion start command is sent via controlword to actually start motion; the drive starts processing points from the buffer
- on buffer low condition, several points are popped from the start of the internal queue and burst into the drive's buffer; this cycle is repeated until there are no more points in the queue
- on buffer empty condition, if and only if the queue has depleted (ideally, we shouldn't reach this state otherwise), interpolation mode is disabled via controlword command; the drive stays dormant until new points arrive via IPositionDirect::setPosition, then the cycle starts anew

Tested with both PT and PVT submodes, interpolation time period ranging from 5 to 50 milliseconds, on a single node. I wonder whether bursting setpoints to six nodes at once would saturate the CAN bus.

See:

As mentioned above, the linear interpolation submodes are interfaced via IPositionDirect. The following options have been considered and discarded, for future reference:

Fully interact via IRemoteVariables, i.e. load points into the internal queue and send the start command without having to resort to IPositionDirect. This solution has several drawbacks due its poor/flawed support via ControlBoardWrapper, see https://github.com/roboticslab-uc3m/yarp-devices/issues/208. It could pose an advantage due to the fact that it allows sending velocity data and even different periods per setpoint along with position data. However, there is hardly a compelling reason to vary periods between acquisitions, and velocity is usually obtained via differentiation of consecutive positions anyway. This effort can be easily transferred from the caller application to the LinearInterpolationBuffer class itself.
Use mixed mode (https://github.com/roboticslab-uc3m/yarp-devices/issues/215). Per http://wiki.icub.org/wiki/Control_Modes: "the mixed control (VOCAB_CM_MIXED) mode allows the user to send position and velocity commands on the same joint (to perform saccadic movement, for example)". See previous point regarding velocity data. Also, position and velocity commands would be sent separately, more checks would be involved (position data arrived, but velocity didn't) and so on.

Velocity setpoints in the PVT submode are currently obtained via arithmetic mean of the mean velocities between three consecutive points. That is, the mean velocity is obtained between the current and the previous setpoints in the queue, same for the next and the current setpoints, and then both results are weighed (same period, so this is as simple (A+B)/2).

(...) velocity is usually obtained via differentiation of consecutive positions anyway

I wonder if this solution could yield better results in a cubic interpolation (regarding position data). Surely it is less convolute, must check its "jerkiness".

I wonder if this solution could yield better results in a cubic interpolation (regarding position data). Surely it is less convolute, must check its "jerkiness".

This MATLAB script performs cubic interpolation on a randomly generated curve: splinetest.zip. It also plots y (position) against x (time), therefore slopes are instantaneous velocities.

Three different approaches regarding slope values are depicted in the following figure. Cyan means zero slope on boundaries and blue points, red line represents the differential approach (mean slope between current and previous point), and black line corresponds to the currently implemented arithmetic mean approach (weighed result of previous and next slopes):

Out of sheer curiosity, MATLAB's spline function applied over the full x range yields the following result (red line) versus the arithmetic mean approach (black line) obtained by applying the spline algorithm on each interval:

I definitely like the implemented approach.

Fully interact via IRemoteVariables, i.e. load points into the internal queue and send the start command without having to resort to IPositionDirect. This solution has several drawbacks due its poor/flawed support via ControlBoardWrapper, see #208. It could pose an advantage due to the fact that it allows sending velocity data and even different periods per setpoint along with position data. However, there is hardly a compelling reason to vary periods between acquisitions, and velocity is usually obtained via differentiation of consecutive positions anyway. This effort can be easily transferred from the caller application to the LinearInterpolationBuffer class itself.

I'm considering this option (again), mainly because of:

Why should I send X YARP commands if those X targets can be grouped in one single call?
Yes, sometimes we may want to pass a custom time parameter (e.g. yarpdatadumper acquisitions).
In whole-body control, only IRemoteVariables can guarantee that we issue the robot-wide motion start command simultaneously.
The IRemoteVariables interface calls on a multiple-wrapper configuration (e.g. several network wrappers talking with a single CanBusControlboard) is misleading, indeed: I could interface through any port (/teo/leftArm, /teo/rightArm) and yet target any started node on the robot. However, this cannot be avoided and I'm already doing this in order to select the interpolation submode and such.

Considering that from path-planning now we can have a nice big Bottle that encapsulates a Trajectory:

Short term: IRemoteVariables
Long term: PR on YARP controlboard interfaces and network wrappers, perhaps involving these classes I just discovered: https://www.yarp.it/git-master/namespaceyarp_1_1rosmsg_1_1trajectory__msgs.html

Note: problems reported by ControlboardWrapper such as Error while trying to command a streaming position direct message on all joints are caused by hardware faults and elevated through the current control mode check. See CHECK_MODE in:

https://github.com/roboticslab-uc3m/yarp-devices/blob/5a2abd14e71e64d6cef7260f56bfcfecb8ddafaf/libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp#L11-L18

Long term: PR on YARP controlboard interfaces and network wrappers, perhaps involving these classes I just discovered: https://www.yarp.it/namespaceyarp_1_1rosmsg_1_1trajectory__msgs.html

Note that this is in fact a rosmsg.

After a long chat with @jgvictores in the aftermath of https://github.com/roboticslab-uc3m/yarp-devices/issues/254#issuecomment-776256880, we've summarized the main use cases for the variety of IPositionDirect implementations. First, a few words on the principles behind CSP and legacy linear interpolation (a.k.a. PT/PVT):

CSP stands for cyclic synchronous position. A periodic, strictly timed SYNC signal is sent from our YARP-CAN utilities through all managed CAN buses. This tells the drives to: 1. publish encoder/current measurements, and 2. process incoming CSP commands on each SYNC signal reception. Note that control applications (e.g. an external Python script to read a trajectory from file and send setPositions to the robot) must be tightly coupled period-wise wrt. to the internal synchronization period. As observed and reported in https://github.com/roboticslab-uc3m/yarp-devices/issues/254#issuecomment-776256880, even a small drift in thread synchronization leads to degraded motion behavior. It is important to note CSP seems to be implemented with an internal, 9 or 10-element buffer, so there is always a delay between setPositions and resulting joint motion.
Legacy linear interpolation was implemented in our devices in a similar, i.e. synchronous manner. Not much to say that it's pretty similar to CSP but harder to use, care must be taken to monitorize buffer state.

I am going to propose an asynchronous PT/PVT implementation in clear contrast to CSP, which is probably going to stay the way it is now. Going async means that the drive will not expect new setpoints at a fixed, preconfigured rate (we used to work with a period of 50 milliseconds). Instead, the delay between successive points will be computed on-the-fly, i.e. as soon as TechnosoftIpos receives point n+1, it substracts current time from the time point n was received and sets the "T" component accordingly. In short: if n+1 arrived 47 ms later than n, instruct the drive to perform n+1 in 47 ms. In consequence, new setpoints may be sent in arbitrary time periods.

This solution aims to decouple external control applications from the internal point processing rates in synchronous drive modes. In conclusion, expected use cases are:

Online trajectories (anything generated on-the-fly, e.g. joystick controllers or trajectories with a known time-dependent expression) with strict synchronization across all robot joints: use CSP. External controllers must implement some sort of callback mechanism listening to the new /sync:o port (name not final) and produce new commands accordingly. Rate is fixed. Needs extra work to adapt the same code for simulation (no sync port). Might skip some points due to network issues.
Either online or offline trajectories, the external control app decides sampling and command rates: use async PT/PVT. Rate might be variable (e.g. offline setpoints acquired along with a timestamp). Code can be reused in simulation. Most versatile solution, but might be prone to desynchronization across joints due to message loss or significant latency.
Same as above (async PT/PVT), but especially suited for reliable execution of offline trajectories: launch the control application aboard the robot, instantiate CanBusControlboard instead of remote_controlboard. Probably needs extra code to account for robot initialization.

Once implemented and tested, these guidelines should land in teo-developer-manual.

As a side note, implementations could take advantage of some filtering. See the iCub's collection of utilities.

External controllers must implement some sort of callback mechanism listening to the new /sync:o port (name not final) and produce new commands accordingly. Rate is fixed. Needs extra work to adapt the same code for simulation (no sync port).

In fact, the sync port can be easily emulated with yarp sample --output /sync:o --period 0.05, though (or --rate 20).

Mostly implemented at https://github.com/roboticslab-uc3m/yarp-devices/commit/2308f03ced8aebe9f94c7c3e10967062f07048fc, needs testing. Main remarks regarding PT/PVT submodes:

PT: hardware buffer size is 9 points, minimum internal software queue size is 9 points as well (= there have to be 9 points in said queue in order to start a PT motion).
PVT: hardware buffer size is 7 points, internal queue must at least point larger on motion start to account for mean velocity computations (see https://github.com/roboticslab-uc3m/yarp-devices/issues/245#issuecomment-580527807). Last point to be sent to the drive always has zero velocity.

See https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-487279910 regarding buffer sizes. Default buffer low limit has been set to 4 points.

Workflow:

The user enables legacy interpolation mode and requests a mode change to IPositionDirect, initial drive configuration ensues.
The user sends new setpoints via setPosition(s), each point is stored in the internal queue along with the current timestamp.
As soon as the minimum queue size is reached (9 for PT, 7+1 for PVT), all points stored in the queue are sent to the drive and the queue is depleted.
A buffer-full signal is received from the iPOS drive. Motion start (ip mode active) is requested via controlword, the drive starts processing setpoints stored in its hardware buffer.
The user is free to keep sending new setpoints via setPosition at the desired command rate. New points are placed at the end of the internal (FIFO) queue.
A buffer-low signal (only 4 points in hardware buffer) is received from the iPOS drive. If the internal queue is not empty, a batch of setpoints is popped from the start (9-4 for PT, 7-4 for PVT) and sent to the drive.
- Special case: in PVT, an additional point is reserved in the internal queue so that mean velocity can be obtained using a window of three consecutive points. If there is exactly one point left in the queue upon receiving the buffer-low signal, this last point is sent assuming zero velocity. This helps end the motion gracefully since otherwise an error is reported and quick stop is demanded from the drive. However, it will cause degraded motion if new points arrive before buffer-empty is signaled.
Repeat steps 5-6 until there are no more points to send.
A buffer-empty signal is received from the iPOS drive. Motion end (ip mode inactive) is requested via controlword.
In case more points are sent via setPosition(s), go back to step 2 and repeat the whole sequence.

In order to implement asynchronous PT/PVT, the internal queue stores setpoint-timestamp pairs. The exact time at which each point is placed in the queue is captured in order to obtain the difference between successive points. For instance, if point n1 was sent by the user at timestamp t1 and n2 at t2, then the result of t2-t1 is used as the time component in the ip setpoint for n2. Note that there is no such reference to obtain the time component for the very first point in the queue (before motion start). It is assumed that t1 is equal to t2 and a dummy point is made up (but not placed in the drive's buffer) from the current joint position in order to obtain the velocity for n1 in PVT submode. In addition, I am storing an initial timestamp to compensate for rounding and accumulation errors in the conversion from seconds to period samples.

To be considered (TO-DO list):

Stop motion if an integrity counter error is reported? https://github.com/roboticslab-uc3m/yarp-devices/commit/99b7f4119ff91fddcb91b1bf6d0194cda0f037cd
~~Use integrity counter to determine next batch size instead of relying on the internal queue length?~~ https://github.com/roboticslab-uc3m/yarp-devices/issues/245#issuecomment-782648641
~~Following on the previous point, abandon batched point transmission and always send points individually (even before motion start) while attending to integrity counter updates?~~ https://github.com/roboticslab-uc3m/yarp-devices/issues/245#issuecomment-782652608
Ignore subsequent points registered via setPosition if the HW buffer has been already declared empty, in which case disable ip mode and keep loading new points in the internal queue until the full buffer size is reached, then replenish. Currently, because of the buffer-low check evaluating before buffer-empty, more points could be sent despite commanding the drive to disable ip (the buffer accepts points anyway), and upon activation the next batch (9 or 7 points) would be wrongly calculated. Done at https://github.com/roboticslab-uc3m/yarp-devices/commit/10b09bbc938b1f603a1891d3e473f0af384c6373 and https://github.com/roboticslab-uc3m/yarp-devices/commit/0e38541363efdf441412426b239914b86f666305.

Mostly implemented at 2308f03, needs testing.

~~Not working.~~ Rolled back to https://github.com/roboticslab-uc3m/yarp-devices/commit/0137786d8e985b15c535f3d76924c5e6b6315310 and tested examplePositionDirect successfully. The implementation is not final, though, since I want to consider the previous TO-DO list. I have learned that the drive reports not the current, but the next integrity counter expected, which makes impossible to know for sure the current number of points in the HW buffer and thus to implement this idea:

Use integrity counter to determine next batch size instead of relying on the internal queue length?

Logs:

[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 0
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 1
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 2
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 3
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 4
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 5
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 6
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 7
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 8
[warning] TechnosoftIpos.cpp:394 interpretPtStatus(): Got ic 9
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [pt] Buffer is full. (canId: 26)
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 26)
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Interpolated position mode active. (canId: 26)
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [MSR] Bit reset: Target command reached. (canId: 26)
[warning] TechnosoftIpos.cpp:394 interpretPtStatus(): Got ic 9
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [pt] Buffer is not full. (canId: 26)
[warning] TechnosoftIpos.cpp:394 interpretPtStatus(): Got ic 9
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [pt] Buffer is low. (canId: 26)
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 9
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 10
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 11
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 12
[warning] LinearInterpolationBuffer.cpp:86 operator()(): Prepared setpoint with ic 13
[warning] TechnosoftIpos.cpp:394 interpretPtStatus(): Got ic 14
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [pt] Buffer is full. (canId: 26)
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [pt] Buffer is not low. (canId: 26)
[warning] TechnosoftIpos.cpp:394 interpretPtStatus(): Got ic 14
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [pt] Buffer is not full. (canId: 26)
[warning] TechnosoftIpos.cpp:394 interpretPtStatus(): Got ic 14
[info] TechnosoftIpos.cpp:58 reportBitToggle(): [pt] Buffer is low. (canId: 26)

Sequence:

Prepare first nine points with integrity counter ranging from 0 to 9, send to drive and start motion.
Drive reports ic equal to 9 and buffer full.
Life goes on, buffer gets slowly depleted until buffer low condition, which triggers another batch of five points (9-13).
Drive reports ic 14 and buffer full, cycle repeats.

I'm quite positive that the integrity counter feedback can be used to implement a simple yet effective handshake.

PS newly generated logs of the full trajectory (old implementation at https://github.com/roboticslab-uc3m/yarp-devices/commit/8dbd67f3777dc8c60b7776ce096b166fcdbfe3dd): old-ip-21022021.txt. PS2 forgot that examplePositionDirect was prepared for sending the whole trajectory in a single batch, I instructed it to send them in regular intervals instead and it indeed worked: new-ip-21022021.txt. PS3 also successfully tested in PVT submode, but last setpoint did not have zero velocity: new-ip-pvt-21022021.txt.

However, IP status is reported via EMCY, which we map to TPDO2 with default inhibit time of 30 ms and no event timer (ref). A too high inhibit time can be limiting with regard to the IP command rate, i.e. the drive may not be able to report back its buffer state until up to 30 ms after the last setpoint was sent.

For this very reason, I am not considering this option anymore:

Following on the previous point, abandon batched point transmission and always send points individually (even before motion start) while attending to integrity counter updates?

PS my bad, TPDO2 is mapped to MER/DER:

https://github.com/roboticslab-uc3m/yarp-devices/blob/8a2910dcb6173bbaf584f1fa0091f2a208868332/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp#L129-L130

I don't think there is an inhibit time setting for EMCY messages, so they are supposed to be triggered as soon as an emergency condition arises. From CiA 301 v4.2.0, 7.2.7.1, Emergency object usage:

Emergency objects are triggered by the occurrence of a CANopen device internal error situation and are transmitted from an emergency producer on the CANopen device. Emergency objects are suitable for interrupt type error alerts. An emergency object is transmitted only once per 'error event'. No further emergency objects shall be transmitted as long as no new errors occur on a CANopen device.

Anyway, I'd rather keep sending points in batches (as opposed to mimicking a handshake via individual requests and integrity counter checks) to avoid the saturation of the CAN network and timing issues.

While synchronous IP (interpolated position) works fine, asynchronous needs still more work:

As soon as motion is started, one point is consumed from the internal buffer, therefore the driver signals it has 8 points stored (full = 9 points), while the software queue is empty (has just sent the ninth point). Solution: add an offset to async PT (similarly to sync/async PVT, which uses this extra point for velocity estimation), don't start motion unless the queue stores at least one point.
I have noticed the iPOS seems to consume points faster: async-pt-20210228.txt. Either TEO's PC timer lags behind the iPOS, or the internal iPOS clock is really a bit faster. In any case the internal buffer depletes sooner than it should, which defeats the purpose of registering new timestamps associated to points in the queue in a timely manner. Bad news if it's the iPOS' fault: bye bye, real time.

Quick test: I have sent 2000 synchronous PT points that theoretically should have been executed in 20 seconds (period equal to 10 ms). I measured the time elapsed between buffer full and buffer empty signals and got only 19 seconds. There is a difference of 0.5 seconds if I supply half the number of points (period 20 ms), and 2 seconds if I double that amount (period 5 ms). It seems that the interpolation substracts like 0.5 ms from each setpoint.

ASWJ there is a bunch of use cases that involve us implementing yarp::dev::IPositionDirect in one way or another:

	Remote commands	Local commands
Offline trajectory	Synchronous PT/PVT	Synchronous PT/PVT
Offline trajectory (individual samples)	Asynchronous PT/PVT CSP (timer)	Asynchronous PT/PVT CSP (timer)
Online trajectory	CSP (sync port, timer)	CSP (shared state observer, timer)

Definitions:

Remote commands: our commanding application, i.e. the one that calls IPositionDirect::setPositions, is executed outside TEO's PC and communication is achieved with usual YARP mechanisms (network control board wrapper pair).
Local commands: conversely, the application runs inside TEO's PC and instantiates CanBusControlboard instead of remote_controlboard. Always prefer this way if "hard" real-time control is pursued (combine with CSP mode thanks to whole-body synchronization) or network latency is unbearable.
Offline trajectory: all setpoints are available beforehand, usually you'll store and read them from file. The individual samples row is a special use case added for convenience since it follows widespread practices, as explained later.
Online trajectory: any trajectory generation method that produces new setpoints on the fly, e.g. visual servoing, joystick control, a known mathematical expression expressed in terms of time...

Methods:

Synchronous PT/PVT: "synchronous" means that the iPOS would process new setpoints at equal, pre-configured time intervals. This is the "T" term in PT/PVT. Since there is no dependence on time involved in the process of sending and executing new setpoints, users are expected to transfer all points at once to an internal software queue (TEO PC side), which is then depleted gradually whenever the iPOS hardware buffer requests more points. In your application, you will call IPositionDirect::setPositions as many times as points are composing the trajectory, no delays in-between. Note that such application cannot be connected to our simulators since those expect new setpoints sent at arbitraty non-zero intervals.
Asynchronous PT/PVT: in order to overcome that last limitation of synchronous mode, this use case is devised to make our old applications work the way as they did before: either a loop or a periodic thread fetches new points of the offline trajectory, transfers them to the robot via setPositions, and applies a delay (the "T" term). This delay, if used within a for loop, may vary, hence the "asynchronous" qualifier. This is not suitable for online trajectories because a buffer is involved, but you could use this anyway if you know what you're doing. Main issue at time of writing: the iPOS internal clock is slightly faster as it should be (https://github.com/roboticslab-uc3m/yarp-devices/issues/245#issuecomment-787459721).
CSP: according to CiA, PT/PVT is devised as legacy and should be replaced today with cyclic synchronous position mode. There is no buffer involved, therefore this method can work both for online and online trajectories. Motion is performed in a synchronized manner across all TEO's joints followed by simultaneous encoder reads, hence if you really need (almost?) real-time control with proper feedback, prefer CSP. This mode is not devoid of quirks, though:
- You can use a timer (or a for loop, for that case) to send new setpoints regardless of the online/offline nature of your trajectory, and the same code can address both real and simulated scenarios. As explained in https://github.com/roboticslab-uc3m/yarp-devices/issues/254, issue https://github.com/robotology/yarp/issues/2488 must be resolved upstream in order to minimize harsh transitions between skipped points. Even if solved, if you are unlucky enough and the command loop overlaps with the internal synchronization loop, you'll observe this same unwanted behavior. Perhaps some workaround could be arranged, pending investigation.
- The synchronization issue can be amended in a twofold manner, depending on whether your app is executing remotely or locally (in terms of TEO's PC). The synchronization mechanism can be either a callback on sync bottles sent from an additional port (remote use case), or a state observer shared via Property (local use case). Main drawback: you can only emulate the remote scenario on simulation (use yarp repeat), not that bad though, but requires a bit more effort and lines of code.

I've been doing my first tests, launching the trajectory of the issue https://github.com/roboticslab-uc3m/yarp-devices/issues/254#issue-791519244 -> test_elbow.zip and modifying your example examplePositionDirect.cpp to execute it in Synchronous PT/PVT (default in pt), reading the csv file.

My program would be the following:

```cxx #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define DEFAULT_ROBOT "/teo" #define DEFAULT_JOINT_ID 3 // elbow #define DEFAULT_POSD_PERIOD_MS 50 #define DEFAULT_IP_MODE "pt" int main(int argc, char *argv[]) { yarp::os::ResourceFinder rf; rf.configure(argc, argv); std::string robot = rf.check("robot",yarp::os::Value(DEFAULT_ROBOT),"name of robot to be used").asString(); int jointId = rf.check("joint", yarp::os::Value(DEFAULT_JOINT_ID), "joint number to test").asInt32(); int period = rf.check("period", yarp::os::Value(DEFAULT_POSD_PERIOD_MS), "posd command period [ms]").asInt32(); bool batch = rf.check("batch", "stream interpolation data in batches"); std::string ipMode = rf.check("ipMode", yarp::os::Value(DEFAULT_IP_MODE), "linear interpolation mode [pt|pvt]").asString(); if (period <= 0) { CD_ERROR("Illegal period: %d.\n", period); return false; } if (ipMode != "pt" && ipMode != "pvt") { CD_ERROR("Illegal ipMode: %s.\n", ipMode.c_str()); return false; } yarp::os::Network yarp; if (!yarp::os::Network::checkNetwork()) { CD_ERROR("Please start a yarp name server first.\n"); return 1; } yarp::dev::IControlMode * ramode; yarp::dev::IEncoders * raencs; yarp::dev::IPositionControl * rapos; yarp::dev::IPositionDirect * raposd; yarp::dev::IRemoteVariables * ravar; yarp::os::Property raoptions; raoptions.put("device","remote_controlboard"); raoptions.put("remote",robot+"/rightArm"); raoptions.put("local","/test"+robot+"/rightArm"); yarp::dev::PolyDriver radev(raoptions); radev.open(raoptions); if(!radev.isValid()) { CD_ERROR("robot rightArm device not available.\n"); radev.close(); yarp::os::Network::fini(); return false; } bool ok = true; ok &= radev.view(ramode); ok &= radev.view(raencs); ok &= radev.view(rapos); ok &= radev.view(raposd); ok &= !batch || radev.view(ravar); if (!radev.isValid()) { CD_ERROR("Remote device not available.\n"); return 1; } if (!ok) { CD_ERROR("Problems acquiring robot interfaces.\n"); return 1; } if (batch) { yarp::os::Property dict {{"enable", yarp::os::Value(true)}, {"mode", yarp::os::Value(ipMode)}, {"periodMs", yarp::os::Value(period)}}; yarp::os::Value v; v.asList()->addString("linInterp"); v.asList()->addList().fromString(dict.toString()); if (!ravar->setRemoteVariable("all", {v})) { CD_ERROR("Unable to set linear interpolation mode.\n"); return 1; } } if (!ramode->setControlMode(jointId, VOCAB_CM_POSITION_DIRECT)) { CD_ERROR("Problems setting position control: POSITION_DIRECT.\n"); return 1; } std::ifstream lectura; std::vector pose; double value; lectura.open("../test_elbow.csv",std::ios::in); for (std::string linea; std::getline(lectura, linea); ) { std::stringstream registro(linea); for (std::string dato; std::getline(registro, dato, ','); ) { pose.push_back( std::stod(dato)); } CD_INFO_NO_HEADER("sending position: %f to joint: %d period: %d ms\n", pose[3], jointId, period); raposd->setPosition(jointId, pose[3]); if (!batch) { yarp::os::Time::delay(period * 0.001); } pose.clear(); } CD_INFO_NO_HEADER("end\n"); return 0; } ```

Terminal 1: ./examplePositionDirect --robot /teo --batch

Terminal 2 of launchCanBus (from the connection between yarp ports of the application with the launchCanBus):

``` yarp: Receiving input from /test/teo/rightArm/rpc:o to /teo/rightArm/rpc:i using tcp yarp: Receiving input from /test/teo/rightArm/command:o to /teo/rightArm/command:i using udp yarp: Sending output from /teo/rightArm/stateExt:o to /test/teo/rightArm/stateExt:i using udp [debug] IRemoteVariablesImpl.cpp:131 setRemoteVariable(): all: linInterp ((enable 1) (mode pt) (periodMs 50)) [debug] IRemoteVariablesRawImpl.cpp:53 setRemoteVariableRaw(): linInterp [success] IRemoteVariablesRawImpl.cpp:101 setRemoteVariableRaw(): Created pt buffer with 9 points and period 50 ms (canId: 15). [debug] IRemoteVariablesRawImpl.cpp:53 setRemoteVariableRaw(): linInterp [success] IRemoteVariablesRawImpl.cpp:101 setRemoteVariableRaw(): Created pt buffer with 9 points and period 50 ms (canId: 16). [debug] IRemoteVariablesRawImpl.cpp:53 setRemoteVariableRaw(): linInterp [success] IRemoteVariablesRawImpl.cpp:101 setRemoteVariableRaw(): Created pt buffer with 9 points and period 50 ms (canId: 17). [debug] IRemoteVariablesRawImpl.cpp:53 setRemoteVariableRaw(): linInterp [success] IRemoteVariablesRawImpl.cpp:101 setRemoteVariableRaw(): Created pt buffer with 9 points and period 50 ms (canId: 18). [debug] IRemoteVariablesRawImpl.cpp:53 setRemoteVariableRaw(): linInterp [success] IRemoteVariablesRawImpl.cpp:101 setRemoteVariableRaw(): Created pt buffer with 9 points and period 50 ms (canId: 19). [debug] IRemoteVariablesRawImpl.cpp:53 setRemoteVariableRaw(): linInterp [success] IRemoteVariablesRawImpl.cpp:101 setRemoteVariableRaw(): Created pt buffer with 9 points and period 50 ms (canId: 20). [debug] IControlModeImpl.cpp:40 setControlMode(): (3, posd) [debug] IControlModeRawImpl.cpp:43 setControlModeRaw(): (0, posd) [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("COB-ID RPDO3"). 40 02 14 01 00 00 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("COB-ID RPDO3"). 43 02 14 01 12 04 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("COB-ID RPDO3"). 23 02 14 01 12 04 00 80. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("COB-ID RPDO3"). 60 02 14 01 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("RPDO3 mapping parameters"). 2f 02 16 00 00 00 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("RPDO3 mapping parameters"). 60 02 16 00 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("RPDO3: mapped object 1"). 23 02 16 01 20 01 c1 60. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("RPDO3: mapped object 1"). 60 02 16 01 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("RPDO3: mapped object 2"). 23 02 16 02 20 02 c1 60. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("RPDO3: mapped object 2"). 60 02 16 02 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("RPDO3 mapping parameters"). 2f 02 16 00 02 00 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("RPDO3 mapping parameters"). 60 02 16 00 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("COB-ID RPDO3"). 23 02 14 01 12 04 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("COB-ID RPDO3"). 60 02 14 01 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("Auxiliary Settings Register"). 2b 8e 20 00 00 00 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("Auxiliary Settings Register"). 60 8e 20 00 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("Interpolation sub mode select"). 2b c0 60 00 00 00 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("Interpolation sub mode select"). 60 c0 60 00 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("Interpolated position buffer length"). 2b 73 20 00 09 00 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("Interpolated position buffer length"). 60 73 20 00 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("Interpolated position buffer configuration"). 2b 74 20 00 80 b4 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("Interpolated position buffer configuration"). 60 74 20 00 00 00 00 00. canId(18) via(0x580). [info] SdoClient.cpp:336 performTransfer(): SDO client request/indication ("Modes of Operation"). 2f 60 60 00 07 00 00 00. canId(18) via(0x600). [success] SdoClient.cpp:358 performTransfer(): SDO server response/confirm ("Modes of Operation"). 60 60 60 00 00 00 00 00. canId(18) via(0x580). [info] TechnosoftIpos.cpp:347 interpretModesOfOperation(): Interpolated Position Mode. canId: 18. [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 18) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [MSR] Bit reset: Target command reached. (canId: 18) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Interpolated position mode active. (canId: 18) yarp: Removing input from /test/teo/rightArm/rpc:o to /teo/rightArm/rpc:i yarp: Removing input from /test/teo/rightArm/command:o to /teo/rightArm/command:i yarp: Removing output from /teo/rightArm/stateExt:o to /test/teo/rightArm/stateExt:i [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 20) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 17) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 20) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 17) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 20) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 17) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 20) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 17) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 20) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 20) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 17) [warning] TechnosoftIpos.cpp:54 reportBitToggle(): [MER] I2t protection. Set when protection is triggered. (canId: 18) [warning] TechnosoftIpos.cpp:487 handleEmcy(): I2t protection triggered (canId 18). [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 17) [warning] TechnosoftIpos.cpp:54 reportBitToggle(): [MSR] Motor I2t protection warning level reached. (canId: 18) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 19) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 16) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Bit reset: Target reached. (canId: 15) [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Target reached. (canId: 15) [warning] TechnosoftIpos.cpp:54 reportBitToggle(): [MER] Short-circuit. Set when protection is triggered. (canId: 16) [warning] TechnosoftIpos.cpp:487 handleEmcy(): Short-circuit (canId 16). [info] TechnosoftIpos.cpp:58 reportBitToggle(): [status] Motor supply voltage is absent (canId: 18) [warning] TechnosoftIpos.cpp:487 handleEmcy(): DC-link under-voltage (canId 18). [warning] TechnosoftIpos.cpp:54 reportBitToggle(): [MER] Under-voltage. Set when protection is triggered. (canId: 18) [warning] TechnosoftIpos.cpp:487 handleEmcy(): DC-link under-voltage (canId 16). [warning] TechnosoftIpos.cpp:487 handleEmcy(): DC-link under-voltage (canId 20). [warning] TechnosoftIpos.cpp:54 reportBitToggle(): [MER] Under-voltage. Set when protection is triggered. (canId: 17) [warning] TechnosoftIpos.cpp:487 handleEmcy(): DC-link under-voltage (canId 17). [warning] TechnosoftIpos.cpp:54 reportBitToggle(): [MER] Under-voltage. Set when protection is triggered. (canId: 20) [error] TechnosoftIpos.cpp:540 monitorWorker(): Last heartbeat response was 0.867129 seconds ago (canId 15). [error] TechnosoftIpos.cpp:540 monitorWorker(): Last heartbeat response was 0.814427 seconds ago (canId 16). [error] TechnosoftIpos.cpp:540 monitorWorker(): Last heartbeat response was 0.862165 seconds ago (canId 17). [error] TechnosoftIpos.cpp:540 monitorWorker(): Last heartbeat response was 0.777979 seconds ago (canId 18). [error] TechnosoftIpos.cpp:540 monitorWorker(): Last heartbeat response was 0.827197 seconds ago (canId 19). [error] TechnosoftIpos.cpp:540 monitorWorker(): Last heartbeat response was 0.788353 seconds ago (canId 20). ^C[INFO][try 1 of 3] Trying to shut down. ```

The result is that there is a loss of control when the arm returns. I've repeated the experiment executing it as before without --batch parameter, which would lead to the execution of the trajectory in CSP mode (by default). The elbow runs good through the trajectory, although I imagine dragging the problems already mentioned in the issue #254.

Video in PT mode: https://youtu.be/yR4z7nSV7TE

Run the experiment again, uncommenting this line. Result of launchCanBus: launchCanBus.zip Video: https://youtu.be/VTqFul9036U

exactly, the first jump occurs on line 1686:

[debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -83.383257)
[debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -61.902524)

I've also seen this jump (although it doesn't arrive here) line 1750:

[debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -45.270002)
[debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -12.867257)

Applying a delay in the sending of 25ms (half of the execution period of 50ms by default) between one point and the next, the result is that it performs the execution of the trajectory successfully without any failure.

Added a delay between sending setpoints half the execution period:

       if (!batch)
        {
            yarp::os::Time::delay(period * 0.001);
        }
        else
        {
            yarp::os::Time::delay(period * 0.0005); // -> period / 2
        }

I've been reducing the period little by little until reaching 5 ms. It looks nice ^^! : Video: https://youtu.be/y6XFXQuruHs

I'm going to comment on some of the experiments that I've carried out to test the PT mode, executing an offline trajectory obtained from a CSV file. In this case, it's a trajectory captured from the mocap system and later processed through the teo blender project. Then, I've created a program that allows reading the csv file and sending the commands in PT / PVT mode to the robot or CSP to the simulator, following the example of the tests carried out previously.

csv file: writing-teo.csv
cpp app: launchTrajectory.cpp

I've tried to run the same csv both in the simulator (in CSP mode by default without the --batch parameter), and in the real robot (in PT mode with the --batch parameter). In both cases, the trajectory is executed with a period of 30 ms. The result is that in the real robot, a certain desynchronization between the joints (arms and trunk) can be seen even in certain joints of the arm itself, giving rise to an incorrect result of the trajectory. I've recorded both modes and tried to synchronize them, without modifying their real speed. Observe trunk movement when the robot is writing "TEO" and especially when it's tries to erase its name, making circular movements with the left arm. It seems that the turns are made in the opposite directions, probably due to a desynchronization with the sagittal motor of the shoulder.

https://youtu.be/ww3wVhUJ8V0

@PeterBowman suggested to me to visualize the current value of yarp::os::time::now() and can->getId() after the lines https://github.com/roboticslab-uc3m/yarp-devices/blob/b5cc7e41e0ab54984e73d8acb520d0557cdad664/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp#L423 and https://github.com/roboticslab-uc3m/yarp-devices/blob/b5cc7e41e0ab54984e73d8acb520d0557cdad664/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp#L404 to appreciate the time it takes for each driver to execute the path.

The result of the values printed by terminal can be reviewed here. In conclusion, not all drivers execute the trajectory in unison or end at the same time.

Not accounting for trunk joints, time measurements for all upper body joints look as follows:

ID	start	end	diff	drift (overall)	drift (per point)
15	1615402270,575350	1615402392,332290	121,756940	1,243060	0,30318539
16	1615402270,575900	1615402390,460330	119,884430	3,115570	0,75989514
17	1615402270,577650	1615402389,416890	118,839240	4,160760	1,01481955
18	1615402270,579060	1615402393,494740	122,915680	0,084320	0,02056587
19	1615402270,579160	1615402393,491390	122,912230	0,087770	0,02140731
20	1615402270,581560	1615402390,127610	119,546050	3,453950	0,84242681
21	1615402270,575340	1615402391,761130	121,185790	1,814210	0,44249023
22	1615402270,575800	1615402390,606790	120,030990	2,969010	0,72414875
23	1615402270,577850	1615402390,770560	120,192710	2,807290	0,6847049
24	1615402270,579270	1615402389,414100	118,834830	4,165170	1,01589511
25	1615402270,580240	1615402391,979030	121,398790	1,601210	0,39053905
26	1615402270,581580	1615402390,187200	119,605620	3,394380	0,82789753

The theoretical duration of this trajectory is 123 seconds. However, motion was completed sooner that expected and also inconsistently across all joints, varying from 0.08 seconds (quite close) to 4.16 seconds (worse case).

I've replicated the drift bug with only ID26 up and running on the network, no SYNC (using default TPDO3 with 30 ms inhibit time), 2000 PT points with period 10 ms (from 50º to -50º at 5º/s): dump.txt. Still getting 19 seconds instead of 20.

Let's take a look at these two consecutive PT points (logged by dumpCanBus --with-ts):

[1616331605.115300]  26  RPDO3  08 00 00 00 0a 00 00 92
[1616331605.115300]  26  RPDO3  d0 ff ff ff 09 00 00 94

Bytes 4 and 5 should read 0x000a, i.e. 10 samples (time component). As soon as the target position goes negative, the two's complement system wreaks havoc on the most significant bytes coming next, i.e. the 0x000a is turned into 0x0009 (9 samples). That's why the trajectory ended prematurely.

Fixed at https://github.com/roboticslab-uc3m/yarp-devices/commit/a881f95078e45ea74a95875a33e166312f2df472.

Applying a delay in the sending of 25ms (half of the execution period of 50ms by default) between one point and the next, the result is that it performs the execution of the trajectory successfully without any failure.

Along the lines of https://github.com/roboticslab-uc3m/yarp-devices/issues/259, it should not be necessary to apply this hardcoded delay if the remote_controlboard device is configured with --writeStrict on (cc @rsantos88). Note that the command reader port on the ControlBoardWrapper side is already set as strict, i.e. it should never drop messages, although it will print a "Number of streaming input messages to be read is XXX and can overflow" warning if the buffer size grows rapidly. I believe that this buffer is not limited in size (ref) anyway.

Commit https://github.com/roboticslab-uc3m/yarp-devices/commit/e8d69d3b69e3aae65f7ce6f83933bb6e2f8c95b4 increases the threshold for motion start trigger on the internal queue size so that batches in async pt/pvt are as big as possible (not accounting for occasional timer drifts). Now, despite a max HW buffer size of 9 and 7 points for PT and PVT submodes, respectively, the internal queue needs to allocate 10 and 9 points prior to commanding motion start (one extra point in PVT so that the mean velocity can be extrapolated).

I have successfully tested sync/async pt/pvt with base command examplePositionDirect --batch --ipMode pt --speed 2 --posTarget -10 --posdTarget -20 --period 50: ip-20210327.zip. Also performing nice on longer distances (100º), higher speeds (10º/s) and tighter periods (10 ms).

PS sending a batch of 5000 PT sync points did not cause CBW to print a warning (using --writeStrict on and launching both client and server on the same machine).

Done and merged at https://github.com/roboticslab-uc3m/yarp-devices/commit/44f2fbc8ca7271cf4e6184ba0e06936f9663319c.

Commit https://github.com/roboticslab-uc3m/yarp-devices/commit/3493779135961acdfb987bbb9a4aa7e656a86659 adds support for async ip and unifies the three currently supported IPositionDirect implementations: CSP, sync PT/PVT, async PT/PVT. A few command-line parameters have been renamed (--pos, --posd, --ip).

Re-launched the same trajectory in PT mode with the new changes. Executed with a period of 20ms/setpoint. The trajectory now runs synchronously on all of its joints:

https://youtu.be/-NajttHvCzY

roboticslab-uc3m / yarp-devices

Offline position-direct implementation via PT/PVT #245