Add Expression Costs and Formula Constraints to Subgraphs in GcsTrajectoryOptimization

geometry/optimization/graph_of_convex_sets.cc

@@ -992,12 +992,14 @@ void GraphOfConvexSets::AddPerspectiveConstraint(
           a[0] = -lc->upper_bound()[i];
           a.tail(A.cols()) = A.row(i);
           prog->AddLinearConstraint(a, -inf, 0, vars);
-        } else if (lc->lower_bound()[i] > 0) {
+        } else if (lc->upper_bound()[i] < 0) {
           // If the upper bound is -inf, we cannot take the perspective of such
           // a constraint, so we throw an error.
           throw std::runtime_error(
               "Cannot take the perspective of a trivially-infeasible linear "
               "constraint of the form x <= -inf.");
+        } else {
+          // Do nothing for the constraint x <= inf.
         }
       }
     }
@@ -1029,6 +1031,8 @@ void GraphOfConvexSets::AddPerspectiveConstraint(
           throw std::runtime_error(
               "Cannot take the perspective of a trivially-infeasible linear "
               "constraint of the form x >= +inf.");
+        } else {
+          // Do nothing for the constraint x >= -inf.
         }
       }
     }

geometry/optimization/test/graph_of_convex_sets_test.cc

@@ -1735,24 +1735,82 @@ TEST_F(ThreeBoxes, LinearConstraint3) {
 TEST_F(ThreeBoxes, InvalidLinearConstraintUpper) {
   const Matrix2d A = Matrix2d::Identity();
   const Vector2d b{.5, .3};
+  const Vector2d c_good{1.0, kInf};
+  const Vector2d c_bad{1.0, -kInf};
+
+  e_on_->AddConstraint(CreateBinding(
+      std::make_shared<LinearConstraint>(A, b, c_good), e_on_->xv()));
+  // b ≤ e_on_->xv() ≤ c_good. No error on the upper bound -- the infinity
+  // component is trivially feasible.
+  DRAKE_EXPECT_NO_THROW(g_.SolveShortestPath(*source_, *target_, options_));
+
+  e_on_->AddConstraint(CreateBinding(
+      std::make_shared<LinearConstraint>(A, b, c_bad), e_on_->xv()));
+  // b ≤ e_on_->xv() ≤ c_bad. We can't take the perspective of the
+  // trivially-infeasible constraint, so solving should throw an error.
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      g_.SolveShortestPath(*source_, *target_, options_), ".*inf.*");
+}
+
+// Test the code path where the upper bounds are not all infinite or finite.
+TEST_F(ThreeBoxes, InvalidLinearConstraintUpper2) {
+  const Matrix2d A = Matrix2d::Identity();
+  const Vector2d b{.5, .3};
+  const Vector2d c_good{1.0, kInf};
+  const Vector2d c_bad{1.0, -kInf};
+
+  e_on_->AddConstraint(CreateBinding(
+      std::make_shared<LinearConstraint>(A, b, c_good), e_on_->xv()));
+  // b ≤ e_on_->xv() ≤ c_good. No error on the upper bound -- the infinity
+  // component is trivially feasible.
+  DRAKE_EXPECT_NO_THROW(g_.SolveShortestPath(*source_, *target_, options_));
+
   e_on_->AddConstraint(CreateBinding(
-      std::make_shared<LinearConstraint>(A, b, Vector2d::Constant(-kInf)),
-      e_on_->xv()));
-  // b ≤ e_on_->xv() ≤ -∞. We can't take the perspective of such a constraint,
-  // so solving should throw an error.
+      std::make_shared<LinearConstraint>(A, b, c_bad), e_on_->xv()));
+  // b ≤ e_on_->xv() ≤ c_bad. We can't take the perspective of the
+  // trivially-infeasible constraint, so solving should throw an error.
   DRAKE_EXPECT_THROWS_MESSAGE(
       g_.SolveShortestPath(*source_, *target_, options_), ".*inf.*");
 }
 
 // Test linear constraints with a lower bound of +inf.
 TEST_F(ThreeBoxes, InvalidLinearConstraintLower) {
   const Matrix2d A = Matrix2d::Identity();
-  const Vector2d b{.5, .3};
+  const Vector2d b_good{-1.0, -kInf};
+  const Vector2d b_bad{-1.0, kInf};
+  const Vector2d c{.5, .3};
+
+  e_on_->AddConstraint(CreateBinding(
+      std::make_shared<LinearConstraint>(A, b_good, c), e_on_->xv()));
+  // b_good ≤ e_on_->xv() ≤ c. No error on the upper bound -- the infinity
+  // component is trivially feasible.
+  DRAKE_EXPECT_NO_THROW(g_.SolveShortestPath(*source_, *target_, options_));
+
+  e_on_->AddConstraint(CreateBinding(
+      std::make_shared<LinearConstraint>(A, b_bad, c), e_on_->xv()));
+  // b_bad ≤ e_on_->xv() ≤ c. We can't take the perspective of the
+  // trivially-infeasible constraint, so solving should throw an error.
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      g_.SolveShortestPath(*source_, *target_, options_), ".*inf.*");
+}
+
+// Test the code path where the lower bounds are not all infinite or finite.
+TEST_F(ThreeBoxes, InvalidLinearConstraintLower2) {
+  const Matrix2d A = Matrix2d::Identity();
+  const Vector2d b_good{-1.0, -kInf};
+  const Vector2d b_bad{-1.0, kInf};
+  const Vector2d c{.5, .3};
+
+  e_on_->AddConstraint(CreateBinding(
+      std::make_shared<LinearConstraint>(A, b_good, c), e_on_->xv()));
+  // b_good ≤ e_on_->xv() ≤ c. No error on the upper bound -- the infinity
+  // component is trivially feasible.
+  DRAKE_EXPECT_NO_THROW(g_.SolveShortestPath(*source_, *target_, options_));
+
   e_on_->AddConstraint(CreateBinding(
-      std::make_shared<LinearConstraint>(A, Vector2d::Constant(kInf), b),
-      e_on_->xv()));
-  // ∞ ≤ e_on_->xv() ≤ b. We can't take the perspective of such a constraint, so
-  // solving should throw an error.
+      std::make_shared<LinearConstraint>(A, b_bad, c), e_on_->xv()));
+  // b_bad ≤ e_on_->xv() ≤ c. We can't take the perspective of the
+  // trivially-infeasible constraint, so solving should throw an error.
   DRAKE_EXPECT_THROWS_MESSAGE(
       g_.SolveShortestPath(*source_, *target_, options_), ".*inf.*");
 }

planning/trajectory_optimization/gcs_trajectory_optimization.cc

@@ -30,6 +30,7 @@ using Subgraph = GcsTrajectoryOptimization::Subgraph;
 using EdgesBetweenSubgraphs = GcsTrajectoryOptimization::EdgesBetweenSubgraphs;
 
 using drake::solvers::MathematicalProgram;
+using drake::solvers::MatrixXDecisionVariable;
 using drake::solvers::Solve;
 using drake::solvers::VectorXDecisionVariable;
 using Eigen::MatrixXd;
@@ -68,6 +69,7 @@ using solvers::LinearEqualityConstraint;
 using solvers::QuadraticCost;
 using symbolic::DecomposeLinearExpressions;
 using symbolic::Expression;
+using symbolic::Formula;
 using symbolic::MakeMatrixContinuousVariable;
 using symbolic::MakeVectorContinuousVariable;
 using trajectories::BezierCurve;
@@ -316,6 +318,16 @@ Subgraph::Subgraph(
         path_continuity_constraint,
         {GetControlPoints(*u).col(order), GetControlPoints(*v).col(0)}));
   }
+
+  // Construct placeholder variables.
+  placeholder_vertex_duration_var_ = symbolic::Variable("t");
+  placeholder_vertex_control_points_var_ =
+      MakeMatrixContinuousVariable(num_positions(), order + 1, "x");
+  placeholder_edge_durations_var_ =
+      std::make_pair(symbolic::Variable("tu"), symbolic::Variable("tv"));
+  placeholder_edge_control_points_var_ = std::make_pair(
+      MakeMatrixContinuousVariable(num_positions(), order + 1, "xu"),
+      MakeMatrixContinuousVariable(num_positions(), order + 1, "xv"));
 }
 
 Subgraph::~Subgraph() = default;
@@ -638,6 +650,9 @@ void Subgraph::AddPathContinuityConstraints(int continuity_order) {
 }
 
 void Subgraph::AddContinuityConstraints(int continuity_order) {
+  // TODO(cohnt): Rewrite to use the generic AddCost and AddConstraint
+  // interfaces.
+
   if (continuity_order == 0) {
     throw std::runtime_error(
         "Path continuity is enforced by default. Choose a higher order.");
@@ -739,6 +754,177 @@ symbolic::Variable Subgraph::GetTimeScaling(
   return v.x()(v.x().size() - 1);
 }
 
+std::variant<Expression, Formula>
+Subgraph::SubstituteVertexPlaceholderVariables(
+    const std::variant<Expression, Formula>& e, const Vertex* vertex) const {
+  int num_rows = num_positions();
+  int num_cols = order_ + 1;
+  MatrixXDecisionVariable control_points_vars(GetControlPoints(*vertex));
+
+  // Note: the logic is identical for symbolic::Expression and
+  // symbolic::Formula, but since there is no inheritance structure, we have to
+  // split into cases based on which type is actually used.
+  if (std::holds_alternative<Expression>(e)) {
+    Expression e_out = std::get<Expression>(e);
+    symbolic::Variables e_vars = e_out.GetVariables();
+
+    // Substitute the control point variables.
+    for (int i = 0; i < num_rows; ++i) {
+      for (int j = 0; j < num_cols; ++j) {
+        if (e_vars.include(placeholder_vertex_control_points_var_(i, j))) {
+          e_out = e_out.Substitute(placeholder_vertex_control_points_var_(i, j),
+                                   control_points_vars(i, j));
+        }
+      }
+    }
+    // Substitute the time scaling variable.
+    if (e_vars.include(placeholder_vertex_duration_var_)) {
+      e_out = e_out.Substitute(placeholder_vertex_duration_var_,
+                               GetTimeScaling(*vertex));
+    }
+    return e_out;
+  } else {
+    Formula e_out = std::get<Formula>(e);
+    symbolic::Variables e_vars = e_out.GetFreeVariables();
+
+    for (int i = 0; i < num_rows; ++i) {
+      for (int j = 0; j < num_cols; ++j) {
+        if (e_vars.include(placeholder_vertex_control_points_var_(i, j))) {
+          e_out = e_out.Substitute(placeholder_vertex_control_points_var_(i, j),
+                                   control_points_vars(i, j));
+        }
+      }
+    }
+    if (e_vars.include(placeholder_vertex_duration_var_)) {
+      e_out = e_out.Substitute(placeholder_vertex_duration_var_,
+                               GetTimeScaling(*vertex));
+    }
+    return e_out;
+  }
+}
+
+std::variant<Expression, Formula> Subgraph::SubstituteEdgePlaceholderVariables(
+    const std::variant<Expression, Formula>& e, const Edge* edge) const {
+  const Vertex& v1 = edge->u();
+  const Vertex& v2 = edge->v();
+
+  int num_rows = num_positions();
+  int num_cols = order_ + 1;
+  MatrixXDecisionVariable control_points_vars_1(GetControlPoints(v1));
+  MatrixXDecisionVariable control_points_vars_2(GetControlPoints(v2));
+
+  // Note: the logic is identical for symbolic::Expression and
+  // symbolic::Formula, but since there is no inheritance structure, we have to
+  // split into cases based on which type is actually used.
+  if (std::holds_alternative<Expression>(e)) {
+    Expression e_out = std::get<Expression>(e);
+    symbolic::Variables e_vars = e_out.GetVariables();
+
+    // Substitute the control point variables for the first vertex.
+    for (int i = 0; i < num_rows; ++i) {
+      for (int j = 0; j < num_cols; ++j) {
+        if (e_vars.include(placeholder_edge_control_points_var_.first(i, j))) {
+          e_out =
+              e_out.Substitute(placeholder_edge_control_points_var_.first(i, j),
+                               control_points_vars_1(i, j));
+        }
+      }
+    }
+    // Substitute the control point variables for the second vertex.
+    for (int i = 0; i < num_rows; ++i) {
+      for (int j = 0; j < num_cols; ++j) {
+        if (e_vars.include(placeholder_edge_control_points_var_.second(i, j))) {
+          e_out = e_out.Substitute(
+              placeholder_edge_control_points_var_.second(i, j),
+              control_points_vars_2(i, j));
+        }
+      }
+    }
+    // Substitute the time scaling variable for the first vertex.
+    if (e_vars.include(placeholder_edge_durations_var_.first)) {
+      e_out = e_out.Substitute(placeholder_edge_durations_var_.first,
+                               GetTimeScaling(v1));
+    }
+    // Substitute the time scaling variable for the second vertex.
+    if (e_vars.include(placeholder_edge_durations_var_.second)) {
+      e_out = e_out.Substitute(placeholder_edge_durations_var_.second,
+                               GetTimeScaling(v2));
+    }
+    return e_out;
+  } else {
+    Formula e_out = std::get<Formula>(e);
+    symbolic::Variables e_vars = e_out.GetFreeVariables();
+
+    for (int i = 0; i < num_rows; ++i) {
+      for (int j = 0; j < num_cols; ++j) {
+        if (e_vars.include(placeholder_edge_control_points_var_.first(i, j))) {
+          e_out =
+              e_out.Substitute(placeholder_edge_control_points_var_.first(i, j),
+                               control_points_vars_1(i, j));
+        }
+      }
+    }
+    for (int i = 0; i < num_rows; ++i) {
+      for (int j = 0; j < num_cols; ++j) {
+        if (e_vars.include(placeholder_edge_control_points_var_.second(i, j))) {
+          e_out = e_out.Substitute(
+              placeholder_edge_control_points_var_.second(i, j),
+              control_points_vars_2(i, j));
+        }
+      }
+    }
+    if (e_vars.include(placeholder_edge_durations_var_.first)) {
+      e_out = e_out.Substitute(placeholder_edge_durations_var_.first,
+                               GetTimeScaling(v1));
+    }
+    if (e_vars.include(placeholder_edge_durations_var_.second)) {
+      e_out = e_out.Substitute(placeholder_edge_durations_var_.second,
+                               GetTimeScaling(v2));
+    }
+    return e_out;
+  }
+}
+
+void Subgraph::AddVertexCost(
+    const Expression& e,
+    const std::unordered_set<Transcription>& used_in_transcription) {
+  for (Vertex*& vertex : vertices_) {
+    Expression post_substitution =
+        std::get<Expression>(SubstituteVertexPlaceholderVariables(e, vertex));
+    vertex->AddCost(post_substitution, used_in_transcription);
+  }
+}
+
+void Subgraph::AddVertexConstraint(
+    const Formula& e,
+    const std::unordered_set<Transcription>& used_in_transcription) {
+  for (Vertex*& vertex : vertices_) {
+    Formula post_substitution =
+        std::get<Formula>(SubstituteVertexPlaceholderVariables(e, vertex));
+    vertex->AddConstraint(post_substitution, used_in_transcription);
+  }
+}
+
+void Subgraph::AddEdgeCost(
+    const Expression& e,
+    const std::unordered_set<Transcription>& used_in_transcription) {
+  for (Edge*& edge : edges_) {
+    Expression post_substitution =
+        std::get<Expression>(SubstituteEdgePlaceholderVariables(e, edge));
+    edge->AddCost(post_substitution, used_in_transcription);
+  }
+}
+
+void Subgraph::AddEdgeConstraint(
+    const symbolic::Formula& e,
+    const std::unordered_set<Transcription>& used_in_transcription) {
+  for (Edge*& edge : edges_) {
+    Formula post_substitution =
+        std::get<Formula>(SubstituteEdgePlaceholderVariables(e, edge));
+    edge->AddConstraint(post_substitution, used_in_transcription);
+  }
+}
+
 EdgesBetweenSubgraphs::EdgesBetweenSubgraphs(
     const Subgraph& from_subgraph, const Subgraph& to_subgraph,
     const ConvexSet* subspace, GcsTrajectoryOptimization* traj_opt,