Understanding trajectory replacement

As already mentioned in the joint trajectory controller description, the arrival of a new trajectory command does not necessarily mean that the controller will completely discard the currently running trajectory and substitute it with the new one. Rather, the controller will take the useful parts of both and combine them appropriately, yielding a smarter trajectory replacement strategy.

The steps followed by the controller for trajectory replacement are as follows:

• Get useful parts of the new trajectory: Preserve all waypoints whose time to be reached is in the future, and discard those with times in the past. If there are no useful parts (ie. all waypoints are in the past) the new trajectory is rejected and the current one continues execution without changes.

• Get useful parts of the current trajectory: Preserve the current trajectory up to the start time of the new trajectory, discard the later parts.

• Combine the useful parts of the current and new trajectories.

The following examples describe this behavior in detail.

The basics

This example shows a joint which is in hold position mode (flat grey line labeled pos hold in the figure below). A new trajectory (shown in red) arrives at the current time (now), which contains three waypoints and a start time in the future (traj start). The time at which waypoints should be reached (time_from_start member of trajectory_msgs/JointTrajectoryPoint) is relative to the trajectory start time.

The controller splices the current hold trajectory at time traj start and appends the three waypoints. Notice that between now and traj start the previous position hold is still maintained, as the new trajectory is not supposed to start yet. After the last waypoint is reached, its position is held until new commands arrive.

The controller guarantees that the transition between the current and new trajectories will be smooth. Longer times to reach the first waypoint mean slower transitions.

Effects of the trajectory start time

This example describes the effect of sending the same trajectory to the controller with different start times. The scenario is that of a controller executing the trajectory from the previous example (shown in red), and receiving a new command (shown in green) with a trajectory start time set to either zero (start now), a future time, or a time in the past.

Of special interest is the last figure, where the new trajectory start time and first waypoint are in the past (before now). In this case, the first waypoint is discarded and only the second one is realized.

Zero trajectory start time (start now)

Trajectory start time in the future

Trajectory start time in the past