[planning] Add velocity bounds to subgraphs

bindings/pydrake/planning/planning_py_trajectory_optimization.cc

@@ -386,7 +386,9 @@ void DefinePlanningTrajectoryOptimization(py::module m) {
         .def("AddPathLengthCost",
             py::overload_cast<double>(&Class::Subgraph::AddPathLengthCost),
             py::arg("weight") = 1.0,
-            subgraph_doc.AddPathLengthCost.doc_1args_weight);
+            subgraph_doc.AddPathLengthCost.doc_1args_weight)
+        .def("AddVelocityBounds", &Class::Subgraph::AddVelocityBounds,
+            py::arg("lb"), py::arg("ub"), subgraph_doc.AddVelocityBounds.doc);
 
     // EdgesBetweenSubgraphs
     const auto& subgraph_edges_doc =
@@ -428,6 +430,8 @@ void DefinePlanningTrajectoryOptimization(py::module m) {
         .def("AddPathLengthCost",
             py::overload_cast<double>(&Class::AddPathLengthCost),
             py::arg("weight") = 1.0, cls_doc.AddPathLengthCost.doc_1args_weight)
+        .def("AddVelocityBounds", &Class::AddVelocityBounds, py::arg("lb"),
+            py::arg("ub"), cls_doc.AddVelocityBounds.doc)
         .def("SolvePath", &Class::SolvePath, py::arg("source"),
             py::arg("target"),
             py::arg("options") =

bindings/pydrake/planning/test/trajectory_optimization_test.py

@@ -320,6 +320,8 @@ def test_gcs_trajectory_optimization_2d(self):
             np.array([[5.0, 5.0, 4.4, 4.4], [2.8, 5.0, 5.0, 2.8]])
         ]
 
+        max_vel = np.ones((2, 1))
+
         # We add a path length cost to the entire graph.
         # This can be called ahead of time or after adding the regions.
         gcs.AddPathLengthCost(weight=1.0)
@@ -334,6 +336,9 @@ def test_gcs_trajectory_optimization_2d(self):
         # but useful to check the binding with the default values.
         gcs.AddTimeCost()
 
+        # Add velocity bounds to the entire graph.
+        gcs.AddVelocityBounds(lb=-max_vel, ub=max_vel)
+
         # Add two subgraphs with different orders.
         main1 = gcs.AddRegions(
             regions=[HPolyhedron(VPolytope(v)) for v in vertices],
@@ -430,6 +435,9 @@ def test_gcs_trajectory_optimization_2d(self):
         # but useful to check the binding with the default values.
         main2.AddTimeCost()
 
+        # Add tighter velocity bounds to the main2 subgraph.
+        main2.AddVelocityBounds(lb=-0.5*max_vel, ub=0.5*max_vel)
+
         options = GraphOfConvexSetsOptions()
         options.convex_relaxation = True
         options.max_rounded_paths = 5

planning/trajectory_optimization/gcs_trajectory_optimization.cc

@@ -1,5 +1,6 @@
 #include "drake/planning/trajectory_optimization/gcs_trajectory_optimization.h"
 
+#include <limits>
 #include <unordered_map>
 #include <unordered_set>
 
@@ -32,6 +33,7 @@ using solvers::Binding;
 using solvers::Constraint;
 using solvers::Cost;
 using solvers::L2NormCost;
+using solvers::LinearConstraint;
 using solvers::LinearCost;
 using solvers::LinearEqualityConstraint;
 using symbolic::DecomposeLinearExpressions;
@@ -46,6 +48,8 @@ using Edge = GraphOfConvexSets::Edge;
 using VertexId = GraphOfConvexSets::VertexId;
 using EdgeId = GraphOfConvexSets::EdgeId;
 
+const double kInf = std::numeric_limits<double>::infinity();
+
 Subgraph::Subgraph(
     const ConvexSets& regions,
     const std::vector<std::pair<int, int>>& edges_between_regions, int order,
@@ -200,6 +204,52 @@ void Subgraph::AddPathLengthCost(double weight) {
   return Subgraph::AddPathLengthCost(weight_matrix);
 }
 
+void Subgraph::AddVelocityBounds(const Eigen::Ref<const VectorXd>& lb,
+                                 const Eigen::Ref<const VectorXd>& ub) {
+  DRAKE_THROW_UNLESS(lb.size() == num_positions());
+  DRAKE_THROW_UNLESS(ub.size() == num_positions());
+  if (order() == 0) {
+    throw std::runtime_error(
+        "Velocity Bounds are not defined for a set of order 0.");
+  }
+
+  // We have q̇(t) = drds * dsdt = ṙ(s) / h, and h >= 0, so we
+  // use h * lb <= ṙ(s) <= h * ub, formulated as:
+  // - inf <=   h * lb - ṙ(s) <= 0
+  // - inf <= - h * ub + ṙ(s) <= 0
+
+  // This also leverages the convex hull property of the B-splines: if all of
+  // the control points satisfy these convex constraints and the curve is
+  // inside the convex hull of these constraints, then the curve satisfies the
+  // constraints for all t.
+
+  const MatrixX<Expression> u_rdot_control =
+      dynamic_pointer_cast_or_throw<BezierCurve<Expression>>(
+          u_r_trajectory_.MakeDerivative())
+          ->control_points();
+
+  MatrixXd b(u_h_.rows(), u_vars_.size());
+  DecomposeLinearExpressions(u_h_.cast<Expression>(), u_vars_, &b);
+
+  for (int i = 0; i < u_rdot_control.cols(); ++i) {
+    MatrixXd M(num_positions(), u_vars_.size());
+    DecomposeLinearExpressions(u_rdot_control.col(i), u_vars_, &M);
+    // TODO(wrangelvid) The matrix M might be sparse, so we could drop columns
+    // for both M and b here.
+
+    MatrixXd H(2 * num_positions(), M.cols());
+    H << M - ub * b, -M + lb * b;
+
+    const auto velocity_constraint = std::make_shared<LinearConstraint>(
+        H, VectorXd::Constant(2 * num_positions(), -kInf),
+        VectorXd::Zero(2 * num_positions()));
+
+    for (Vertex* v : vertices_) {
+      v->AddConstraint(Binding<LinearConstraint>(velocity_constraint, v->x()));
+    }
+  }
+}
+
 EdgesBetweenSubgraphs::EdgesBetweenSubgraphs(
     const Subgraph& from_subgraph, const Subgraph& to_subgraph,
     const ConvexSet* subspace, GcsTrajectoryOptimization* traj_opt)
@@ -349,10 +399,13 @@ Subgraph& GcsTrajectoryOptimization::AddRegions(
   }
 
   if (order > 0) {
-    // These costs rely on the derivative of the trajectory.
+    // These costs and constraints rely on the derivative of the trajectory.
     for (const MatrixXd& weight_matrix : global_path_length_costs_) {
       subgraph->AddPathLengthCost(weight_matrix);
     }
+    for (const auto& [lb, ub] : global_velocity_bounds_) {
+      subgraph->AddVelocityBounds(lb, ub);
+    }
   }
   return *subgraphs_.emplace_back(subgraph);
 }
@@ -412,6 +465,20 @@ void GcsTrajectoryOptimization::AddPathLengthCost(double weight) {
   return GcsTrajectoryOptimization::AddPathLengthCost(weight_matrix);
 }
 
+void GcsTrajectoryOptimization::AddVelocityBounds(
+    const Eigen::Ref<const VectorXd>& lb,
+    const Eigen::Ref<const VectorXd>& ub) {
+  DRAKE_THROW_UNLESS(lb.size() == num_positions());
+  DRAKE_THROW_UNLESS(ub.size() == num_positions());
+  // Add path velocity bounds to each subgraph.
+  for (std::unique_ptr<Subgraph>& subgraph : subgraphs_) {
+    if (subgraph->order() > 0) {
+      subgraph->AddVelocityBounds(lb, ub);
+    }
+  }
+  global_velocity_bounds_.push_back({lb, ub});
+}
+
 std::pair<CompositeTrajectory<double>, solvers::MathematicalProgramResult>
 GcsTrajectoryOptimization::SolvePath(const Subgraph& source,
                                      const Subgraph& target,

planning/trajectory_optimization/gcs_trajectory_optimization.h

@@ -111,6 +111,18 @@ class GcsTrajectoryOptimization final {
     */
     void AddPathLengthCost(double weight = 1.0);
 
+    /** Adds a linear velocity constraint to the subgraph `lb` ≤ q̇(t) ≤
+    `ub`.
+    @param lb is the lower bound of the velocity.
+    @param ub is the upper bound of the velocity.
+
+    @throws std::exception if subgraph order is zero, since the velocity is
+    defined as the derivative of the Bézier curve.
+    @throws std::exception if lb or ub are not of size num_positions().
+    */
+    void AddVelocityBounds(const Eigen::Ref<const Eigen::VectorXd>& lb,
+                           const Eigen::Ref<const Eigen::VectorXd>& ub);
+
    private:
     /* Constructs a new subgraph and copies the regions. */
     Subgraph(const geometry::optimization::ConvexSets& regions,
@@ -314,6 +326,21 @@ class GcsTrajectoryOptimization final {
   */
   void AddPathLengthCost(double weight = 1.0);
 
+  /** Adds a linear velocity constraint to the entire graph `lb` ≤ q̇(t) ≤
+  `ub`.
+  @param lb is the lower bound of the velocity.
+  @param ub is the upper bound of the velocity.
+
+  This constraint will be added to the entire graph. Since the velocity requires
+  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.
+
+  @throws std::exception if lb or ub are not of size num_positions().
+  */
+  void AddVelocityBounds(const Eigen::Ref<const Eigen::VectorXd>& lb,
+                         const Eigen::Ref<const Eigen::VectorXd>& ub);
+
   /** Formulates and solves the mixed-integer convex formulation of the
   shortest path problem on the whole graph. @see
   `geometry::optimization::GraphOfConvexSets::SolveShortestPath()` for further
@@ -355,6 +382,8 @@ class GcsTrajectoryOptimization final {
   std::vector<std::unique_ptr<EdgesBetweenSubgraphs>> subgraph_edges_;
   std::vector<double> global_time_costs_;
   std::vector<Eigen::MatrixXd> global_path_length_costs_;
+  std::vector<std::pair<Eigen::VectorXd, Eigen::VectorXd>>
+      global_velocity_bounds_{};
 };
 
 }  // namespace trajectory_optimization