From 192cc3083dcfa9f0a90edc58a6c12a235b00602d Mon Sep 17 00:00:00 2001 From: Lujie Yang <47341031+lujieyang@users.noreply.github.com> Date: Sun, 21 Apr 2024 05:35:07 -0400 Subject: [PATCH] Add perspective of positive semidefinite constraint (#21296) Add perspective of positive semidefinite constraint --- geometry/optimization/graph_of_convex_sets.cc | 7 +++ .../test/graph_of_convex_sets_test.cc | 52 +++++++++++++++++++ 2 files changed, 59 insertions(+) diff --git a/geometry/optimization/graph_of_convex_sets.cc b/geometry/optimization/graph_of_convex_sets.cc index fd057fb09e5d..0f8806d97747 100644 --- a/geometry/optimization/graph_of_convex_sets.cc +++ b/geometry/optimization/graph_of_convex_sets.cc @@ -49,6 +49,7 @@ using solvers::MathematicalProgram; using solvers::MathematicalProgramResult; using solvers::MatrixXDecisionVariable; using solvers::PerspectiveQuadraticCost; +using solvers::PositiveSemidefiniteConstraint; using solvers::ProgramType; using solvers::QuadraticCost; using solvers::RotatedLorentzConeConstraint; @@ -778,6 +779,12 @@ void GraphOfConvexSets::AddPerspectiveConstraint( A_cone.block(0, 1, rc->A().rows(), rc->A().cols()) = rc->A_dense(); prog->AddRotatedLorentzConeConstraint(A_cone, VectorXd::Zero(rc->A().rows()), vars); + } else if (dynamic_cast(constraint) != + nullptr) { + // Since we have ϕ ≥ 0, we have S ≽ 0 ⇔ ϕS ≽ 0. + // It is sufficient to add the original constraint to the program (with the + // new variables). + prog->AddConstraint(binding.evaluator(), vars.tail(vars.size() - 1)); } else { throw std::runtime_error( fmt::format("ShortestPathProblem::Edge does not support this " diff --git a/geometry/optimization/test/graph_of_convex_sets_test.cc b/geometry/optimization/test/graph_of_convex_sets_test.cc index 66e65d578884..d67242a412a2 100644 --- a/geometry/optimization/test/graph_of_convex_sets_test.cc +++ b/geometry/optimization/test/graph_of_convex_sets_test.cc @@ -1170,6 +1170,58 @@ TEST_F(ThreeBoxes, LorentzConeConstraint) { EXPECT_GE(z[0], 0); } +TEST_F(ThreeBoxes, PositiveSemidefiniteConstraint1) { + auto constraint = + std::make_shared(2); + solvers::VectorXDecisionVariable psd_x_on(4); + // [ xᵤ[0], xᵤ[1]; xᵤ[1], xᵥ[0]] ≽ 0. + psd_x_on << e_on_->xu()[0], e_on_->xu()[1], e_on_->xu()[1], e_on_->xv()[0]; + e_on_->AddConstraint(solvers::Binding(constraint, psd_x_on)); + // Prevent the matrix from collapsing to zero. + e_on_->AddConstraint(psd_x_on[0] + psd_x_on[3] == 1); + + // Make sure the PSD constraint is inactive for the off edge. + solvers::VectorXDecisionVariable psd_x_off(4); + psd_x_off << e_off_->xu()[0], e_off_->xu()[1], e_off_->xu()[1], + e_off_->xv()[0]; + e_off_->AddConstraint(solvers::Binding(constraint, psd_x_off)); + e_off_->AddConstraint(psd_x_off[0] + psd_x_off[3] == 1); + + auto result = g_.SolveShortestPath(*source_, *target_, options_); + ASSERT_TRUE(result.is_success()); + + Eigen::Matrix2d mat; + mat << source_->GetSolution(result)[0], source_->GetSolution(result)[1], + source_->GetSolution(result)[1], target_->GetSolution(result)[0]; + Eigen::SelfAdjointEigenSolver solver(mat); + EXPECT_GE(solver.eigenvalues()[0], 0); + EXPECT_GE(solver.eigenvalues()[1], 0); + EXPECT_TRUE(sink_->GetSolution(result).hasNaN()); +} + +// This test has linear constraints to prevent the matrix from being positive +// semidefinite. The solver should fail. +TEST_F(ThreeBoxes, PositiveSemidefiniteConstraint2) { + auto constraint = + std::make_shared(2); + solvers::VectorXDecisionVariable psd_x_on(4); + // [ xᵤ[0], xᵤ[1]; xᵤ[1], xᵥ[0]] ≽ 0. + psd_x_on << e_on_->xu()[0], e_on_->xu()[1], e_on_->xu()[1], e_on_->xv()[0]; + e_on_->AddConstraint(solvers::Binding(constraint, psd_x_on)); + e_on_->AddConstraint(psd_x_on[0] + psd_x_on[3] == -1); + + solvers::VectorXDecisionVariable psd_x_off(4); + psd_x_off << e_off_->xu()[0], e_off_->xu()[1], e_off_->xu()[1], + e_off_->xv()[0]; + e_off_->AddConstraint(solvers::Binding(constraint, psd_x_off)); + e_off_->AddConstraint(psd_x_off[0] + psd_x_off[3] == -1); + + auto result = g_.SolveShortestPath(*source_, *target_, options_); + ASSERT_FALSE(result.is_success()); + + EXPECT_TRUE(sink_->GetSolution(result).hasNaN()); +} + TEST_F(ThreeBoxes, RotatedLorentzConeConstraint) { Eigen::MatrixXd A(5, 4); // clang-format off