traj: driver should skip pt if pt == curpos

[gavanderhoorn]

The first point in a ROS trajectory tends to be whatever the current state of the robot is (except in situations where trajectory replacement is active). In those cases it would be advantagous if ROS_TRAJ could detect that and immediately skip the point and request / execute the next one.

Could use FK and then the >=< operator, but not sure whether that works for groups that don't have kinematics enabled.

[gavanderhoorn]

Also not entirely sure whether we don't get this behaviour 'for free': the controller / motion engine could be smart enough to return early for J TP statements that ask it to move to its current position.

With J [..] ms that doesn't work, but for J [..] % it could.

[simonschmeisser]

It should probably also error out if pt != curpos within some margin? We got two crashes and desync of some sort due to manual mode might be an explanation

[gavanderhoorn]

Could you provide some more information?

All joint poses are absolute, so there is no possibility of desyncing (like motoman_driver can do).

---

Edit: ah, I believe I know what you're saying. The driver unconditionally skips the first point, which, with specific trajectories that you streamed, caused the robot to execute unexpected motions as you didn't have your trajectories start at the current state.

If that is the case, then:

1. bad trajectories ! ☹️
2. we need to add a check for distance from current state and abort if > some threshold