Add perspective of positive semidefinite constraint (RobotLocomotion#…

…21296)

Add perspective of positive semidefinite constraint

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<PositiveSemidefiniteConstraint*>(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 "

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<solvers::PositiveSemidefiniteConstraint>(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<Eigen::Matrix2d> 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<solvers::PositiveSemidefiniteConstraint>(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