[planning] Add velocity bounds to subgraphs #19334

wrangelvid · 2023-05-02T22:08:52Z

This adds velocity bounds to the subgraphs, which enables useful minimum time planning.

This change is

wrangelvid

+@RussTedrake for feature review.

Reviewable status: LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @RussTedrake and @wrangelvid)

RussTedrake

awesome! i like the test/example. with a few small questions/corrections.

Reviewed 5 of 5 files at r1, all commit messages.
Reviewable status: 8 unresolved discussions, needs at least two assigned reviewers, missing label for release notes (waiting on @wrangelvid)

planning/trajectory_optimization/gcs_trajectory_optimization.h line 119 at r1 (raw file):

    @param ub is the upper bound of the velocity.

    The subgraph order must be greater than zero, since the velocity is defined

You should write e.g.

throws std::exception if the subgraph order is zero...

you also throw if the sizes of lb/ub are wrong.

planning/trajectory_optimization/gcs_trajectory_optimization.h line 336 at r1 (raw file):

  forming the derivative of the Bézier curve, this constraint will only added to
  all subgraphs with order greater than zero. Note that this constraint will be
  applied even to subgraphs added in the future.

report that you throw if lb and ub are the wrong size.

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 51 at r1 (raw file):

using EdgeId = GraphOfConvexSets::EdgeId;

const double inf = std::numeric_limits<double>::infinity();

inf => kInf.

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 217 at r1 (raw file):

  // We have q̇(t) = drds * dsdt = ṙ(s) / h, and h >= 0, so we
  // use duration * lb <= ṙ(s) <= duration * ub. But will be reformulated as:

duration => h (twice)

nit: "But will be reformulated as" . maybe ", formulated as:"

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 118 at r1 (raw file):

  // Add shortest path objective.
  gcs.AddPathLengthCost();

nit: seems inconsistent with the problem setup? add a line saying that you

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 158 at r1 (raw file):

  // Now we want to find the fastest path from S to G.
  // Add minimum time objective with a very high weight to overpower the
  // shortest path objective.

nit: would be slightly cleaner to not have both in the same problem, but i realize that removing the costs would be nontrivial, and making a new gcs would be a bunch of lines...

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 171 at r1 (raw file):

  // We expect the fastest path to take a detour and avoid the slow gravel,
  // going through the gravel. Thus we expect the number of segments in the

nit: redundant. "slow gravel, going through the gravel"

bindings/pydrake/planning/test/trajectory_optimization_test.py line 340 at r1 (raw file):

        # Add velocity bounds to the entire graph.
        gcs.AddVelocityBounds(-max_vel, max_vel)

lb=, ub=

wrangelvid

Reviewable status: 5 unresolved discussions, needs at least two assigned reviewers, missing label for release notes (waiting on @wrangelvid)

planning/trajectory_optimization/gcs_trajectory_optimization.h line 119 at r1 (raw file):