From 5a820c129d77aedee46c270b2f05ca19a7131b10 Mon Sep 17 00:00:00 2001 From: Thomas Cohn Date: Wed, 6 Nov 2024 10:19:12 -0500 Subject: [PATCH] Fix a Bug in GCS When Passing in a Trivially-Infeasible Upper Bound (#22090) --- geometry/optimization/graph_of_convex_sets.cc | 6 +- .../test/graph_of_convex_sets_test.cc | 88 ++++++++++++++++--- 2 files changed, 82 insertions(+), 12 deletions(-) diff --git a/geometry/optimization/graph_of_convex_sets.cc b/geometry/optimization/graph_of_convex_sets.cc index e06fcde44e1f..e07e9c725edb 100644 --- a/geometry/optimization/graph_of_convex_sets.cc +++ b/geometry/optimization/graph_of_convex_sets.cc @@ -1021,12 +1021,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. } } } @@ -1058,6 +1060,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. } } } diff --git a/geometry/optimization/test/graph_of_convex_sets_test.cc b/geometry/optimization/test/graph_of_convex_sets_test.cc index 7900aac9768e..6a693739cf40 100644 --- a/geometry/optimization/test/graph_of_convex_sets_test.cc +++ b/geometry/optimization/test/graph_of_convex_sets_test.cc @@ -1743,26 +1743,92 @@ 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(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(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_), + ".*trivially-infeasible.*x\\s<=\\s-inf.*"); + // The latter portion of the regex is trying to match x <= -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(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(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(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.*"); + g_.SolveShortestPath(*source_, *target_, options_), + ".*trivially-infeasible.*x\\s<=\\s-inf.*"); + // The latter portion of the regex is trying to match x <= -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(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(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_), + ".*trivially-infeasible.*x\\s>=\\s\\+inf.*"); + // The latter portion of the regex is trying to match x >= +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(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(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(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.*"); + g_.SolveShortestPath(*source_, *target_, options_), + ".*trivially-infeasible.*x\\s>=\\s\\+inf.*"); + // The latter portion of the regex is trying to match x >= +inf. } TEST_F(ThreeBoxes, LorentzConeConstraint) {