Static constructor for CompositeTrajectory that automatically aligns segment timings.

[cohnt]

I have a scenario where I'm generating multiple trajectories (by solving KinematicTrajectoryOptimization problems), and then want to stitch them together into a single long trajectory. However, the start and end times might not line up, so the CompositeTrajectory constructor throws. This provides a convenient class to retime the start and end of each segment, so they align properly.

Not sure who makes sense for this PR -- feel free to assign anyone.

---

This change is 

[sherm1]

+@RussTedrake for feature review or disposition, please

Reviewable status: LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers

[RussTedrake]

Reviewed 2 of 6 files at r1, all commit messages.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers

---

common/trajectories/composite_trajectory.h line 56 at r1 (raw file):

  /** Constructs a composite trajectory from a list of trajectories whose start
  and ent times may not coincide, by translating their start and end times.

nit: "ent" => "end"

---

common/trajectories/composite_trajectory.cc line 3 at r1 (raw file):

#include "drake/common/trajectories/composite_trajectory.h"

#include <iostream>

remove this

---

common/trajectories/composite_trajectory.cc line 118 at r1 (raw file):

  for (int i = 1; i < ssize(segments); ++i) {
    DRAKE_DEMAND(segments[i]->rows() == segments[0]->rows());
    DRAKE_DEMAND(segments[i]->cols() == segments[0]->cols());

those could all be DRAKE_THROW_UNLESS, too.

---

common/trajectories/composite_trajectory.cc line 132 at r1 (raw file):

        PiecewisePolynomial<T>::FirstOrderHold(breaks, samples);
    std::cout << time_traj.value(new_start).value() << " "
              << time_traj.value(new_start + duration).value() << std::endl;

remove this.

---

common/trajectories/composite_trajectory.cc line 134 at r1 (raw file):

              << time_traj.value(new_start + duration).value() << std::endl;
    aligned_segments.emplace_back(copyable_unique_ptr(
        PathParameterizedTrajectory(*segments[i], time_traj)));

wait... this is not doing what the docstring advertised. you are potentially changing the representation of the trajectory... turning some other trajectory into a first order hold trajectory. I don't think we should do that.

I think probably the entry point needs to be more specific than taking any Trajectory. Or potentially we could teach CompositeTrajectory to remember a time offset for each of the segment trajectories.

[cohnt]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers

---

common/trajectories/composite_trajectory.h line 56 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

nit: "ent" => "end"

Done.

---

common/trajectories/composite_trajectory.cc line 3 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

remove this

Done.

---

common/trajectories/composite_trajectory.cc line 118 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

those could all be DRAKE_THROW_UNLESS, too.

Done.

---

common/trajectories/composite_trajectory.cc line 132 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

remove this.

Done.

---

common/trajectories/composite_trajectory.cc line 134 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

wait... this is not doing what the docstring advertised. you are potentially changing the representation of the trajectory... turning some other trajectory into a first order hold trajectory. I don't think we should do that.

I think probably the entry point needs to be more specific than taking any Trajectory. Or potentially we could teach CompositeTrajectory to remember a time offset for each of the segment trajectories.

Say one trajectory is $f:[t_0,t_1]\to\mathbb R^n$. If we want to set its start time to $t_2$, we can construct a linear time trajectory where $t_2'mapsto t_0$ and $t_2+\Delta t\mapsto t_1$, where $\Delta t=t_1-t_0$. All involved velocities (and higher-order derivatives) will remain unchanged. To effect this for any Trajectory, we can make a PathParametrizedTrajectory with the appropriate time trajectory. I would think this is a pretty low-overhead operation, although derivatives might get costly due to the implementation of PathParametrizedTrajectory?

Some ideas on how to better clarify this for the user:

• Note that the underlying trajectory representation changes to PathParametrizedTrajectory.
• Add more comments into the code explaining how the time trajectory is made.

Changing CompositeTrajectory (really, it would have to be PiecewiseTrajectory, right?) to support non-aligned breaks is an option, but requiring time breaks line up seems like a perfectly fair requirement. Perhaps it would be better to add a method to Trajectory that enables translating the start or end time to a given point, and we could use that?

[RussTedrake]

Reviewed 3 of 6 files at r1, 2 of 2 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers

[RussTedrake]

+@xuchenhan-tri for platform review, please.

Reviewable status: LGTM missing from assignee xuchenhan-tri(platform)

---

common/trajectories/composite_trajectory.cc line 134 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

Say one trajectory is $f:[t_0,t_1]\to\mathbb R^n$. If we want to set its start time to $t_2$, we can construct a linear time trajectory where $t_2'mapsto t_0$ and $t_2+\Delta t\mapsto t_1$, where $\Delta t=t_1-t_0$. All involved velocities (and higher-order derivatives) will remain unchanged. To effect this for any Trajectory, we can make a PathParametrizedTrajectory with the appropriate time trajectory. I would think this is a pretty low-overhead operation, although derivatives might get costly due to the implementation of PathParametrizedTrajectory?

Some ideas on how to better clarify this for the user:

• Note that the underlying trajectory representation changes to PathParametrizedTrajectory.
• Add more comments into the code explaining how the time trajectory is made.

Changing CompositeTrajectory (really, it would have to be PiecewiseTrajectory, right?) to support non-aligned breaks is an option, but requiring time breaks line up seems like a perfectly fair requirement. Perhaps it would be better to add a method to Trajectory that enables translating the start or end time to a given point, and we could use that?

Per f2f, I'm now convinced we should go ahead with this.

[xuchenhan-tri]

Platform review done with a few comments, PTAL.

Reviewed 4 of 6 files at r1, 2 of 2 files at r2, all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee xuchenhan-tri(platform)

---

common/trajectories/composite_trajectory.h line 60 at r2 (raw file):

  segments[0].cols()`. */
  static CompositeTrajectory<T> AlignAndConcatenate(
      std::vector<copyable_unique_ptr<Trajectory<T>>> segments);

Why do we need to pass by value here?

---

common/trajectories/composite_trajectory.cc line 116 at r2 (raw file):

  DRAKE_THROW_UNLESS(segments.size() > 0);
  for (int i = 1; i < ssize(segments); ++i) {
    DRAKE_THROW_UNLESS(segments[i]->rows() == segments[0]->rows());

This seems dangerous to me: segment[i] is dereferenced without checking that it's non-null.

In general, what's the expected behavior here if any segment passed in here is null?

---

common/trajectories/composite_trajectory.cc line 122 at r2 (raw file):

  aligned_segments.emplace_back(segments[0]);
  for (int i = 1; i < ssize(segments); ++i) {
    T new_start = aligned_segments.back()->end_time();

BTW, these temp variables can all be const.

---

bindings/pydrake/trajectories_py.cc line 593 at r2 (raw file):

                for (const Trajectory<T>* py_segment : py_segments) {
                  segments.emplace_back(
                      py_segment ? py_segment->Clone() : nullptr);

BTW, related to the thread above -- it seems like passing in a null segment is a real possibility. There should be a (c++) unit test covering this.

[cohnt]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee xuchenhan-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

common/trajectories/composite_trajectory.h line 60 at r2 (raw file):

Previously, xuchenhan-tri wrote…

Why do we need to pass by value here?

I was matching the signature of the constructor. Presumably, it was needed their since it's using std::move, which is not being used here?

Are you suggesting I make the argument type std::vector<copyable_unique_ptr<Trajectory<T>>>&?

---

common/trajectories/composite_trajectory.cc line 116 at r2 (raw file):

Previously, xuchenhan-tri wrote…

This seems dangerous to me: segment[i] is dereferenced without checking that it's non-null.

In general, what's the expected behavior here if any segment passed in here is null?

This was a segmentation fault waiting to happen in Drake master, as well as this PR. I've added a check for nullity to both this function, and the original constructor, as well as a test case exposing the danger.

---

common/trajectories/composite_trajectory.cc line 122 at r2 (raw file):

Previously, xuchenhan-tri wrote…

BTW, these temp variables can all be const.

Done.

---

bindings/pydrake/trajectories_py.cc line 593 at r2 (raw file):

Previously, xuchenhan-tri wrote…

BTW, related to the thread above -- it seems like passing in a null segment is a real possibility. There should be a (c++) unit test covering this.

Done.

[xuchenhan-tri]

Reviewed 3 of 3 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee xuchenhan-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

common/trajectories/composite_trajectory.h line 60 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

I was matching the signature of the constructor. Presumably, it was needed their since it's using std::move, which is not being used here?

Are you suggesting I make the argument type std::vector<copyable_unique_ptr<Trajectory<T>>>&?

By passing by value, this is creating an unnecessary copy of all the trajectories. We could either pass by const reference here or move from this argument in the .cc file when you create the aligned result.

But from a cursory look, The constructor ofPathParameterizedTrajectory doesn't support moving in an existing trajectory, so we should pass by reference here.

[cohnt]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee xuchenhan-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

common/trajectories/composite_trajectory.h line 60 at r2 (raw file):

Previously, xuchenhan-tri wrote…

By passing by value, this is creating an unnecessary copy of all the trajectories. We could either pass by const reference here or move from this argument in the .cc file when you create the aligned result.

But from a cursory look, The constructor ofPathParameterizedTrajectory doesn't support moving in an existing trajectory, so we should pass by reference here.

Done.

[xuchenhan-tri]

Reviewed 2 of 2 files at r4, all commit messages.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[jwnimmer-tri]

BTW Probably the best spelling for pydrake would be (const std::vector<const Trajectory<T>*>& segments). Once you have copyable_unique_ptr in play, I think it's always going to eat a spurious copy across the call?

[cohnt]

Seems like the memory management would be a hassle if the function takes in a vector of raw pointers?

Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[xuchenhan-tri]

We'd be cloning the segments (when constructing PathParametrizedTrajectory) and discarding the raw pointers if we go that route. I think it depends on what you have at call sites. If you already have a vector of copyable_unique_ptrs sitting around, you can just pass that in. Otherwise, you can do what Jeremy proposed above and save a copy.

[cohnt]

Is there a standard practice for how to handle vectors of trajectories? So far, I've only seen std::vector<copyable_unique_ptr<Trajectory<T>>> (in the constructor, for example).

[xuchenhan-tri]

I'm not familiar with trajectories usages to comment on the standard practice, but I would recommend do whatever that makes sense. If there's already a std::vector<copyable_unique_ptr<Trajectory<T>>> at a typical call site, then pass it in as a const reference; if you have to assemble that vector, passing in a vector of raw pointers is enough.

Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[cohnt]

In that case, let's stick with const std::vector<copyable_unique_ptr<Trajectory<T>>>&, since that matches the original constructor (so they can be used interchangeably).

Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[xuchenhan-tri]

Reviewed all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees RussTedrake(platform),xuchenhan-tri(platform)