Driver skips points in between trajectory

[gonzalocasas]

The motion control task of the driver skips trajectory points when they match the current configuration.

Isn't there a missing check to ensure this only happens for the first point of a trajectory?
At least, that seems to be the condition implemented on the ABB driver (which is conceptually the same but also checks current_index == 1)

Edit:
The main reason why this is incorrect imo is that motion point queueing is not the same as motion execution; the driver queues up the move commands onto the robot's internal motion buffer and then continues execution. But if the motion is stalled, herej() will always return the configuration at the time the point is queued, not at execution. The simplest case that leads to incorrect execution is starting with the robot on the zero-config, with the robot on but the motion button not active (paused motion). Then send a trajectory with 3 points: [0, 0, 90, 0, 0 ,0], [0, 0, 0, 0, 0 ,0], [0, 0, 90, 0, 0 ,0]. Upon receiving the points, the robot will skip the second point because it matches the configuration and the move that was supposed to involve 3 total moves, will actually do just one move and stop there.

[gavanderhoorn]

That looks like an incorrect implementation indeed: there's no way to guarantee that herej() will be called at the right time -- most likely it will be called at the wrong time, as you note.

[nilsmelchert]

Could this problem cause the jerky trajectories that were mentioned in #25 ?

[gonzalocasas]

It does cause trajectories to execute weirdly (depending on several factors, of course). We tested generating trajectories in MoveIt and then creating val3 code out of them and that works perfectly, so I would rule out MoveIt/OMPL as the source of jerkiness.

[gavanderhoorn]

@gonzalocasas: did you already check whether removing that line improves things?

Should be relatively easy to check, just comment the line.

[gonzalocasas]

@gavanderhoorn yes, I confirm. Removing the line indeed fixes the issue.

[gonzalocasas]

I can send a PR for it as is (just removing the line), although, I wanted to check first whether it was still meaningful to do this check but move it to the execution loop, so that it actually does what it was intended to do. But I can also send the removal-line PR for now and then send the other if I see it makes sense... What would be preferable from your perspective?

[gavanderhoorn]

Let's just remove the line.

I'm not sure it makes sense to add it back somewhere else, as here() will always have a delay / be desynced with the current state of the robot.

It could be that pruning points might make sense / improve performance when very dense trajectories are streamed in. In that case here() + a margin and filtering for that could be something to consider.

I don't have any way to test that, but it could be something to keep in mind for the future.