Stitching together trajectories

Question

I'm attempting to construct a Drake Trajectory that is a concatenation of several other trajectories.

I want my robot to perform a sequence of actions, roughly equivalent to a pick and place. For each action, I use KinematicTrajectoryOptimization to generate the desired trajectory.

I want to join all these trajectories into one unified trajectory, so I can simulate the whole sequence in one pass. All the individual trajectories have start_time of zero. Is there a way of achieving this?

I attempted to use CompositeTrajectory but that requires the start_time of each input trajectory to coincide with the end_time of the previous one.

Answer 1

I found one way of doing this - use PathParameterizedTrajectory to shift the start/end times of each trajectory so they form contiguous ranges, then pass the results into CompositeTrajectory.