Add Expression Costs and Formula Constraints to EdgesBetweenSubgraphs…

… in GcsTrajectoryOptimization. (#22155)

planning/trajectory_optimization/gcs_trajectory_optimization.cc

@@ -1087,6 +1087,15 @@ EdgesBetweenSubgraphs::EdgesBetweenSubgraphs(
       }
     }
   }
+
+  // Construct placeholder variables.
+  placeholder_edge_durations_var_ =
+      std::make_pair(symbolic::Variable("tu"), symbolic::Variable("tv"));
+  placeholder_edge_control_points_var_ =
+      std::make_pair(MakeMatrixContinuousVariable(
+                         num_positions(), from_subgraph_.order() + 1, "xu"),
+                     MakeMatrixContinuousVariable(
+                         num_positions(), to_subgraph_.order() + 1, "xv"));
 }
 
 EdgesBetweenSubgraphs::~EdgesBetweenSubgraphs() = default;
@@ -1576,6 +1585,37 @@ std::vector<const GraphOfConvexSets::Edge*> EdgesBetweenSubgraphs::Edges()
   return edges;
 }
 
+template <typename T>
+T EdgesBetweenSubgraphs::SubstituteEdgePlaceholderVariables(
+    T e, const Edge& edge) const {
+  return SubstituteAllVariables(
+      e,
+      {placeholder_edge_durations_var_.first,
+       placeholder_edge_durations_var_.second},
+      {GetTimeScalingU(edge), GetTimeScalingV(edge)}, {}, {},
+      {placeholder_edge_control_points_var_.first,
+       placeholder_edge_control_points_var_.second},
+      {GetControlPointsU(edge), GetControlPointsV(edge)});
+}
+
+void EdgesBetweenSubgraphs::AddEdgeCost(
+    const Expression& e,
+    const std::unordered_set<Transcription>& used_in_transcription) {
+  for (Edge*& edge : edges_) {
+    Expression post_substitution = SubstituteEdgePlaceholderVariables(e, *edge);
+    edge->AddCost(post_substitution, used_in_transcription);
+  }
+}
+
+void EdgesBetweenSubgraphs::AddEdgeConstraint(
+    const symbolic::Formula& e,
+    const std::unordered_set<Transcription>& used_in_transcription) {
+  for (Edge*& edge : edges_) {
+    Formula post_substitution = SubstituteEdgePlaceholderVariables(e, *edge);
+    edge->AddConstraint(post_substitution, used_in_transcription);
+  }
+}
+
 Eigen::Map<const MatrixX<symbolic::Variable>>
 EdgesBetweenSubgraphs::GetControlPointsU(
     const geometry::optimization::GraphOfConvexSets::Edge& e) const {

planning/trajectory_optimization/gcs_trajectory_optimization.h

@@ -299,7 +299,7 @@ class GcsTrajectoryOptimization final {
     /** Returns a pair of placeholder decision variables (not actually declared
     as decision variables in the MathematicalProgram) associated with the time
     scaling of the trajectory in two sets within this subgraph that are
-    connected by an internal edge. This variable will be substituted for real
+    connected by an internal edge. These variables will be substituted for real
     decision variables in methods like AddEdgeCost and AddEdgeConstraint.
     Passing this variable directly into objectives/constraints will result in an
     error. */
@@ -313,7 +313,7 @@ class GcsTrajectoryOptimization final {
     control points of the trajectory in two sets within this subgraph that are
     connected by an internal edge. Each variable will be of shape
     (num_positions(), order+1), where the ith column is the ith control point.
-    This variable will be substituted for real decision variables in methods
+    These variables will be substituted for real decision variables in methods
     like AddEdgeCost and AddEdgeConstraint. Passing this variable directly into
     objectives/constraints will result in an error. */
     const std::pair<solvers::MatrixXDecisionVariable,
@@ -615,6 +615,89 @@ class GcsTrajectoryOptimization final {
     std::vector<const geometry::optimization::GraphOfConvexSets::Edge*> Edges()
         const;
 
+    /** Returns a pair of placeholder decision variables (not actually declared
+    as decision variables in the MathematicalProgram) associated with the time
+    scaling of the trajectory in two sets that are
+    connected by an edge from this EdgesBetweenSubgraphs. These variables will
+    be substituted for real decision variables in methods like AddEdgeCost and
+    AddEdgeConstraint. Passing this variable directly into
+    objectives/constraints will result in an error. */
+    const std::pair<symbolic::Variable, symbolic::Variable>&
+    edge_constituent_vertex_durations() const {
+      return placeholder_edge_durations_var_;
+    }
+
+    /** Returns a pair of placeholder decision variables (not actually declared
+    as decision variables in the MathematicalProgram) associated with the
+    control points of the trajectory in two sets that are
+    connected by an edge from this EdgesBetweenSubgraphs. Each variable will be
+    of shape (num_positions(), order+1), where the ith column is the ith control
+    point. (Note that the first and second variable will have different shapes
+    if the order of the two subgraphs is different.) These variables will be
+    substituted for real decision variables in methods like AddEdgeCost and
+    AddEdgeConstraint. Passing this variable directly into
+    objectives/constraints will result in an error. */
+    const std::pair<solvers::MatrixXDecisionVariable,
+                    solvers::MatrixXDecisionVariable>&
+    edge_constituent_vertex_control_points() const {
+      return placeholder_edge_control_points_var_;
+    }
+
+    /** Adds an arbitrary user-defined cost to every edge within the
+    EdgesBetweenSubgraphs. The cost should be defined using the placeholder
+    control point variables (obtained from
+    edge_constituent_vertex_control_points()) and the placeholder time scaling
+    variables (obtained from edge_constituent_vertex_durations()). This enables
+    greater modeling freedom, but we cannot guarantee a feasible solution for
+    all possible costs.
+
+    @throws std::exception if any variables besides those from @ref
+    edge_constituent_vertex_durations and @ref
+    edge_constituent_vertex_control_points are used.
+
+    Costs which do not support the perspective operation cannot be used with
+    Transcription::kMIP or Transcription::kRelaxation. Consider providing an
+    appropriate "convex surrogate" that is supported within GraphOfConvexSets,
+    or exclusively using the SolveConvexRestriction method. */
+    void AddEdgeCost(
+        const symbolic::Expression& e,
+        const std::unordered_set<
+            geometry::optimization::GraphOfConvexSets::Transcription>&
+            used_in_transcription = {
+                geometry::optimization::GraphOfConvexSets::Transcription::kMIP,
+                geometry::optimization::GraphOfConvexSets::Transcription::
+                    kRelaxation,
+                geometry::optimization::GraphOfConvexSets::Transcription::
+                    kRestriction});
+
+    /** Adds an arbitrary user-defined constraint to every edge within the
+    EdgesBetweenSubgraphs. The constraint should be defined using the
+    placeholder control point variables (obtained from
+    edge_constituent_vertex_control_points()) and the placeholder time scaling
+    variables (obtained from edge_constituent_vertex_durations()). This enables
+    greater modeling freedom, but we cannot guarantee a feasible solution for
+    all possible constraints.
+
+    @throws std::exception if any variables besides those from @ref
+    edge_constituent_vertex_durations and @ref
+    edge_constituent_vertex_control_points are used.
+
+    Constraints which do not support the perspective operation cannot be used
+    with Transcription::kMIP or Transcription::kRelaxation. Consider providing
+    an appropriate "convex surrogate" that is supported within
+    GraphOfConvexSets, or exclusively using the SolveConvexRestriction method.
+    */
+    void AddEdgeConstraint(
+        const symbolic::Formula& e,
+        const std::unordered_set<
+            geometry::optimization::GraphOfConvexSets::Transcription>&
+            used_in_transcription = {
+                geometry::optimization::GraphOfConvexSets::Transcription::kMIP,
+                geometry::optimization::GraphOfConvexSets::Transcription::
+                    kRelaxation,
+                geometry::optimization::GraphOfConvexSets::Transcription::
+                    kRestriction});
+
    private:
     EdgesBetweenSubgraphs(
         const Subgraph& from_subgraph, const Subgraph& to_subgraph,
@@ -654,6 +737,10 @@ class GcsTrajectoryOptimization final {
     symbolic::Variable GetTimeScalingV(
         const geometry::optimization::GraphOfConvexSets::Edge& e) const;
 
+    template <typename T>
+    T SubstituteEdgePlaceholderVariables(
+        T e, const geometry::optimization::GraphOfConvexSets::Edge& edge) const;
+
     GcsTrajectoryOptimization& traj_opt_;
     const Subgraph& from_subgraph_;
     const Subgraph& to_subgraph_;
@@ -663,6 +750,12 @@ class GcsTrajectoryOptimization final {
 
     std::vector<geometry::optimization::GraphOfConvexSets::Edge*> edges_;
 
+    std::pair<symbolic::Variable, symbolic::Variable>
+        placeholder_edge_durations_var_;
+    std::pair<solvers::MatrixXDecisionVariable,
+              solvers::MatrixXDecisionVariable>
+        placeholder_edge_control_points_var_;
+
     friend class GcsTrajectoryOptimization;
   };
 

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc

@@ -2754,9 +2754,9 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, GenericSubgraphEdgeCostConstraint) {
   // Check the cost.
   double cost = 0.0;
   cost += 0.2;  // We traverse one edge in the middle subgraph. The time on the
-                // first edge must be at least 0.2, the two times must be equal,
-                // and the time of the second edge has a cost applied, so it
-                // should be 0.2.
+                // first vertex must be at least 0.2, the two times must be
+                // equal, and the time of the second vertex has a cost applied,
+                // so it should be 0.2.
   cost +=
       -0.85;  // We add a cost equal to the -1 times the value of the first
               // control point of the second set in an edge. The second control
@@ -2805,6 +2805,106 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, GenericSubgraphEdgeCostConstraint) {
                               ".*IsSubsetOf\\(allowed_vars_\\).*");
 }
 
+GTEST_TEST(GcsTrajectoryOptimizationTest,
+           GenericEdgesBetweenSubgraphsCostConstraint) {
+  const double kTol = 1e-9;
+
+  GcsTrajectoryOptimization gcs(1);
+  const double kMinimumDuration = 0.1;
+  auto& start = gcs.AddRegions(MakeConvexSets(Point(Vector1d(0))), 0, 0);
+  auto& middle1 =
+      gcs.AddRegions(MakeConvexSets(Hyperrectangle(Vector1d(0), Vector1d(0.9))),
+                     1, kMinimumDuration);
+  auto& middle2 =
+      gcs.AddRegions(MakeConvexSets(Hyperrectangle(Vector1d(0.8), Vector1d(2))),
+                     2, kMinimumDuration);
+  auto& goal = gcs.AddRegions(MakeConvexSets(Point(Vector1d(2))), 0, 0);
+  gcs.AddEdges(start, middle1);
+  auto& edges = gcs.AddEdges(middle1, middle2);
+  gcs.AddEdges(middle2, goal);
+
+  auto edge_control_points = edges.edge_constituent_vertex_control_points();
+  ASSERT_EQ(edge_control_points.first.rows(), 1);
+  ASSERT_EQ(edge_control_points.second.rows(), 1);
+  ASSERT_EQ(edge_control_points.first.cols(), 2);
+  ASSERT_EQ(edge_control_points.second.cols(), 3);
+  auto edge_durations = edges.edge_constituent_vertex_durations();
+
+  // Requires that the time of the first set of an edge be lower bounded by 0.2.
+  Formula outgoing_time_minimum =
+      Expression(edge_durations.first) >= Expression(2 * kMinimumDuration);
+  edges.AddEdgeConstraint(outgoing_time_minimum);
+
+  // Require that the time of the first and second sets of an edge be equal.
+  Formula equal_time_constraint =
+      Expression(edge_durations.first) == Expression(edge_durations.second);
+  edges.AddEdgeConstraint(equal_time_constraint);
+
+  // Add a cost to the time of the second set of an edge.
+  Expression incoming_time_cost = Expression(edge_durations.second);
+  edges.AddEdgeCost(incoming_time_cost);
+
+  // Require that the second control point of the first set of an edge be at
+  // most 0.85.
+  Formula control_point_limit =
+      Expression(edge_control_points.first(0, 1)) <= Expression(0.85);
+  edges.AddEdgeConstraint(control_point_limit);
+
+  // Add a cost to maximize the first control point of the second set of an
+  // edge.
+  Expression control_point_cost =
+      -1 * Expression(edge_control_points.second(0, 0));
+  edges.AddEdgeCost(control_point_cost);
+
+  // Also add the summed costs and logical conjunction of the constraints, but
+  // just to the restriction (due to parsing limitations).
+  edges.AddEdgeCost(incoming_time_cost + control_point_cost,
+                    {GraphOfConvexSets::Transcription::kRestriction});
+  edges.AddEdgeConstraint(outgoing_time_minimum && equal_time_constraint &&
+                          control_point_limit);
+
+  // All costs and constraints are convex, so the relaxation should be solvable.
+  GraphOfConvexSetsOptions options;
+  options.convex_relaxation = true;
+  auto [traj, result] = gcs.SolvePath(start, goal, options);
+  ASSERT_TRUE(result.is_success());
+
+  // Check the cost.
+  double cost = 0.0;
+  cost += 0.2;  // We traverse one edge between the two middle subgraphs. The
+                // time on the first vertex must be at least 0.2, the two times
+                // must be equal, and the time of the second vertex has a cost
+                // applied, so it should be 0.2.
+  cost +=
+      -0.85;  // We add a cost equal to the -1 times the value of the first
+              // control point of the second set in an edge. The second control
+              // point of the first set in the edge (which is equal by
+              // continuity constraints) has been limited to be at most 0.85.
+  cost *= 2;  // We double the cost due to the additional constraint on the
+              // restriction.
+  EXPECT_NEAR(result.get_optimal_cost(), cost, kTol);
+
+  // Check the constraints.
+  ASSERT_EQ(traj.get_number_of_segments(), 2);
+
+  double dt0 = traj.segment(0).end_time() - traj.segment(0).start_time();
+  double dt1 = traj.segment(1).end_time() - traj.segment(1).start_time();
+  EXPECT_GE(dt0, 2 * kMinimumDuration);  // Duration lower bound constraint.
+  EXPECT_NEAR(dt0, dt1, kTol);           // Same duration constraint.
+
+  Vector1d key_point = traj.value(traj.segment(0).end_time());
+  EXPECT_LE(key_point[0], 0.85);  // Control point constraint.
+
+  // Check that passing in a expression using a non-placeholder variable thows.
+  symbolic::Variable extra_var;
+  Expression bad_expression = Expression(extra_var);
+  Formula bad_formula = bad_expression == Expression(0.0);
+  DRAKE_EXPECT_THROWS_MESSAGE(edges.AddEdgeCost(bad_expression),
+                              ".*IsSubsetOf\\(allowed_vars_\\).*");
+  DRAKE_EXPECT_THROWS_MESSAGE(edges.AddEdgeConstraint(bad_formula),
+                              ".*IsSubsetOf\\(allowed_vars_\\).*");
+}
+
 }  // namespace
 }  // namespace trajectory_optimization
 }  // namespace planning