From 88dae28f1c421ffe01f739e4b9d41d7a711f652d Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Wed, 26 Apr 2023 06:54:08 -0400 Subject: [PATCH] Adds Spectrahedron isa ConvexSet This adds the minimal functionality to be useful. I will follow-up with more PRs to fill in the "not implemented yet" blocks as needed. --- .../geometry/geometry_py_optimization.cc | 11 + .../geometry/test/optimization_test.py | 9 + geometry/optimization/BUILD.bazel | 12 ++ geometry/optimization/spectrahedron.cc | 190 ++++++++++++++++++ geometry/optimization/spectrahedron.h | 80 ++++++++ .../optimization/test/spectrahedron_test.cc | 100 +++++++++ 6 files changed, 402 insertions(+) create mode 100644 geometry/optimization/spectrahedron.cc create mode 100644 geometry/optimization/spectrahedron.h create mode 100644 geometry/optimization/test/spectrahedron_test.cc diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc index f701237dd3cd..140d713b597f 100644 --- a/bindings/pydrake/geometry/geometry_py_optimization.cc +++ b/bindings/pydrake/geometry/geometry_py_optimization.cc @@ -17,6 +17,7 @@ #include "drake/geometry/optimization/iris.h" #include "drake/geometry/optimization/minkowski_sum.h" #include "drake/geometry/optimization/point.h" +#include "drake/geometry/optimization/spectrahedron.h" #include "drake/geometry/optimization/vpolytope.h" namespace drake { @@ -257,6 +258,16 @@ void DefineGeometryOptimization(py::module m) { py::implicitly_convertible>(); } + // Spectrahedron + { + const auto& cls_doc = doc.Spectrahedron; + py::class_(m, "Spectrahedron", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc_0args) + .def(py::init(), py::arg("prog"), + cls_doc.ctor.doc_1args); + py::implicitly_convertible>(); + } + // VPolytope { const auto& cls_doc = doc.VPolytope; diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py index 45236552a170..61f7932affe7 100644 --- a/bindings/pydrake/geometry/test/optimization_test.py +++ b/bindings/pydrake/geometry/test/optimization_test.py @@ -206,6 +206,15 @@ def test_minkowski_sum(self): self.assertEqual(sum2.num_terms(), 2) self.assertIsInstance(sum2.term(0), mut.Point) + def test_spectrahedron(self): + s = mut.Spectrahedron() + prog = MathematicalProgram() + X = prog.NewSymmetricContinuousVariables(3) + prog.AddPositiveSemidefiniteConstraint(X) + prog.AddLinearEqualityConstraint(X[0, 0] + X[1, 1] + X[2, 2], 1) + s = mut.Spectrahedron(prog=prog) + self.assertEqual(s.ambient_dimension(), 6) + def test_v_polytope(self): vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]]) vpoly = mut.VPolytope(vertices=vertices) diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 3b49df26bc0c..34a6b3d4e583 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -35,6 +35,7 @@ drake_cc_library( "intersection.cc", "minkowski_sum.cc", "point.cc", + "spectrahedron.cc", "vpolytope.cc", ], hdrs = [ @@ -45,6 +46,7 @@ drake_cc_library( "intersection.h", "minkowski_sum.h", "point.h", + "spectrahedron.h", "vpolytope.h", ], interface_deps = [ @@ -55,6 +57,7 @@ drake_cc_library( deps = [ "//geometry:read_obj", "//solvers:choose_best_solver", + "//solvers:get_program_type", "//solvers:gurobi_solver", "//solvers:ipopt_solver", "//solvers:mosek_solver", @@ -332,6 +335,15 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "spectrahedron_test", + deps = [ + ":convex_set", + "//common/test_utilities:expect_throws_message", + "//solvers:solve", + ], +) + drake_cc_googletest( name = "vpolytope_test", data = ["//geometry:test_obj_files"], diff --git a/geometry/optimization/spectrahedron.cc b/geometry/optimization/spectrahedron.cc new file mode 100644 index 000000000000..0d71f4dced49 --- /dev/null +++ b/geometry/optimization/spectrahedron.cc @@ -0,0 +1,190 @@ +#include "drake/geometry/optimization/spectrahedron.h" + +#include +#include + +#include + +#include "drake/solvers/get_program_type.h" + +namespace drake { +namespace geometry { +namespace optimization { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using solvers::Binding; +using solvers::Constraint; +using solvers::MathematicalProgram; +using solvers::ProgramAttribute; +using solvers::ProgramAttributes; +using solvers::VectorXDecisionVariable; +using symbolic::Variable; + +namespace { + +// TODO(russt): This can be replaced by vars(indices) once we have Eigen 3.4. +VectorXDecisionVariable GetVariablesByIndex( + const Eigen::Ref& vars, + std::vector indices) { + VectorXDecisionVariable new_vars(indices.size()); + for (int i = 0; i < static_cast(indices.size()); ++i) { + new_vars[i] = vars[indices[i]]; + } + return new_vars; +} + +} // namespace + +const solvers::ProgramAttributes Spectrahedron::supported_attributes_ = { + ProgramAttribute::kLinearCost, ProgramAttribute::kLinearConstraint, + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kPositiveSemidefiniteConstraint}; + +Spectrahedron::Spectrahedron() + : ConvexSet(&ConvexSetCloner, 0) {} + +Spectrahedron::Spectrahedron(const MathematicalProgram& prog) + : ConvexSet(&ConvexSetCloner, prog.num_vars()) { + for (const auto& attr : prog.required_capabilities()) { + if (supported_attributes().count(attr) < 1) { + throw std::runtime_error( + fmt::format("Spectrahedron does not support MathematicalPrograms " + "that require ProgramAttribute {}. If that attribute is " + "convex, it might be possible to add that support.", + attr)); + } + } + sdp_ = prog.Clone(); + // Remove any objective functions. + for (const auto& binding : sdp_->GetAllCosts()) { + sdp_->RemoveCost(binding); + } +} + +Spectrahedron::~Spectrahedron() = default; + +bool Spectrahedron::DoIsBounded() const { + throw std::runtime_error( + "Spectrahedron::IsBounded() is not implemented yet."); +} + +bool Spectrahedron::DoPointInSet(const Eigen::Ref& x, + double tol) const { + return sdp_->CheckSatisfied(sdp_->GetAllConstraints(), x, tol); +} + +void Spectrahedron::DoAddPointInSetConstraints( + MathematicalProgram* prog, + const Eigen::Ref& x) const { + DRAKE_DEMAND(x.size() == sdp_->num_vars()); + for (const auto& binding : sdp_->GetAllConstraints()) { + prog->AddConstraint( + binding.evaluator(), + GetVariablesByIndex( + x, sdp_->FindDecisionVariableIndices(binding.variables()))); + } +} + +std::vector> +Spectrahedron::DoAddPointInNonnegativeScalingConstraints( + MathematicalProgram* prog, + const Eigen::Ref& x, + const Variable& t) const { + DRAKE_DEMAND(x.size() == sdp_->num_vars()); + std::vector> constraints; + const double kInf = std::numeric_limits::infinity(); + + // TODO(russt): Support SparseMatrix constraints. + for (const auto& binding : sdp_->bounding_box_constraints()) { + // t*lb ≤ x ≤ t*ub, implemented as + // [I,-lb]*[x;t] ≥ 0, [I,-ub]*[x;t] ≤ 0. + VectorXDecisionVariable vars(binding.evaluator()->num_vars()+1); + vars << GetVariablesByIndex( + x, sdp_->FindDecisionVariableIndices(binding.variables())), + t; + MatrixXd Ab = MatrixXd::Identity(binding.evaluator()->num_constraints(), + binding.evaluator()->num_vars() + 1); + // TODO(russt): Handle individual elements that are infinite. + if (binding.evaluator()->lower_bound().array().isFinite().any()) { + Ab.rightCols<1>() = -binding.evaluator()->lower_bound(); + prog->AddLinearConstraint(Ab, 0, kInf, vars); + } + if (binding.evaluator()->upper_bound().array().isFinite().any()) { + Ab.rightCols<1>() = -binding.evaluator()->upper_bound(); + prog->AddLinearConstraint(Ab, -kInf, 0, vars); + } + } + for (const auto& binding : sdp_->linear_equality_constraints()) { + // Ax = t*b, implemented as + // [A,-lb]*[x;t] == 0. + VectorXDecisionVariable vars(binding.evaluator()->num_vars()+1); + vars << GetVariablesByIndex( + x, sdp_->FindDecisionVariableIndices(binding.variables())), + t; + MatrixXd Ab(binding.evaluator()->num_constraints(), + binding.evaluator()->num_vars() + 1); + Ab.leftCols(binding.evaluator()->num_vars()) = + binding.evaluator()->GetDenseA(); + Ab.rightCols<1>() = -binding.evaluator()->lower_bound(); + prog->AddLinearConstraint(Ab, 0, 0, vars); + } + for (const auto& binding : sdp_->linear_constraints()) { + // t*lb <= Ax = t*ub, implemented as + // [A,-lb]*[x;t] ≥ 0, [A,-ub]*[x;t] ≤ 0. + VectorXDecisionVariable vars(binding.evaluator()->num_vars()+1); + vars << GetVariablesByIndex( + x, sdp_->FindDecisionVariableIndices(binding.variables())), + t; + MatrixXd Ab(binding.evaluator()->num_constraints(), + binding.evaluator()->num_vars() + 1); + Ab.leftCols(binding.evaluator()->num_vars()) = + binding.evaluator()->GetDenseA(); + if (binding.evaluator()->lower_bound().array().isFinite().any()) { + Ab.rightCols<1>() = -binding.evaluator()->lower_bound(); + prog->AddLinearConstraint(Ab, 0, kInf, vars); + } + if (binding.evaluator()->upper_bound().array().isFinite().any()) { + Ab.rightCols<1>() = -binding.evaluator()->upper_bound(); + prog->AddLinearConstraint(Ab, -kInf, 0, vars); + } + } + for (const auto& binding : sdp_->positive_semidefinite_constraints()) { + // These constraints get added without modification -- a non-negative + // scaling of the PSD cone is just the PSD cone. + constraints.emplace_back(prog->AddConstraint( + binding.evaluator(), + Eigen::Map>( + GetVariablesByIndex( + x, sdp_->FindDecisionVariableIndices(binding.variables())) + .data(), + binding.evaluator()->matrix_rows(), + binding.evaluator()->matrix_rows()))); + } + return constraints; +} + +std::vector> +Spectrahedron::DoAddPointInNonnegativeScalingConstraints( + MathematicalProgram* prog, const Eigen::Ref& A, + const Eigen::Ref& b, const Eigen::Ref& c, + double d, const Eigen::Ref& x, + const Eigen::Ref& t) const { + unused(prog, A, b, c, d, x, t); + throw std::runtime_error( + "Spectrahedron::PointInNonnegativeScalingConstraints() is not " + "implemented yet for the case where A, b, c, and d are passed in as " + "arguments."); +} + +std::pair, math::RigidTransformd> +Spectrahedron::DoToShapeWithPose() const { + // I could potentially visualize the 2x2 case in three dimensions (as a mesh + // if nothing else). + throw std::runtime_error( + "ToShapeWithPose is not supported by Spectrahedron."); +} + +} // namespace optimization +} // namespace geometry +} // namespace drake diff --git a/geometry/optimization/spectrahedron.h b/geometry/optimization/spectrahedron.h new file mode 100644 index 000000000000..99888fe3aabe --- /dev/null +++ b/geometry/optimization/spectrahedron.h @@ -0,0 +1,80 @@ +#pragma once + +#include +#include +#include +#include + +#include "drake/geometry/optimization/convex_set.h" +#include "drake/solvers/mathematical_program.h" + +namespace drake { +namespace geometry { +namespace optimization { + +/** Implements a spectrahedron (the feasible set of a semidefinite program). +The ambient dimension of the set is N(N+1)/2; the number of variables required +to describe the N-by-N semidefinite matrix. + +@ingroup geometry_optimization +*/ +class Spectrahedron final : public ConvexSet { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Spectrahedron) + + /** Default constructor (which constructs the empty set). */ + Spectrahedron(); + + /** Constructs the spectrahedron from a MathematicalProgram. + @throws std::exception if @p prog.required_capabilities() is not a subset of + supported_attributes(). */ + explicit Spectrahedron(const solvers::MathematicalProgram& prog); + + ~Spectrahedron() final; + + /** Returns the list of solvers::ProgramAttributes supported by this class. */ + static const solvers::ProgramAttributes& supported_attributes() { + return supported_attributes_; + } + + // TODO(russt): Add PointInSet(MatrixXd X, double tol) overload, which will + // only work in the case where the ambient_dimension is ONLY symmetric + // matrices. + + private: + bool DoIsBounded() const final; + + bool DoPointInSet(const Eigen::Ref& x, + double tol) const final; + + void DoAddPointInSetConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& vars) + const final; + + std::vector> + DoAddPointInNonnegativeScalingConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& x, + const symbolic::Variable& t) const final; + + std::vector> + DoAddPointInNonnegativeScalingConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& A, + const Eigen::Ref& b, + const Eigen::Ref& c, double d, + const Eigen::Ref& x, + const Eigen::Ref& t) const final; + + std::pair, math::RigidTransformd> DoToShapeWithPose() + const final; + + copyable_unique_ptr sdp_{}; + + static const solvers::ProgramAttributes supported_attributes_; +}; + +} // namespace optimization +} // namespace geometry +} // namespace drake diff --git a/geometry/optimization/test/spectrahedron_test.cc b/geometry/optimization/test/spectrahedron_test.cc new file mode 100644 index 000000000000..26e86e23acde --- /dev/null +++ b/geometry/optimization/test/spectrahedron_test.cc @@ -0,0 +1,100 @@ +#include "drake/geometry/optimization/spectrahedron.h" + +#include + +#include + +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/solvers/solve.h" + +namespace drake { +namespace geometry { +namespace optimization { +namespace { + +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; +using solvers::MathematicalProgram; +using solvers::Solve; + +const double kInf = std::numeric_limits::infinity(); + +GTEST_TEST(SpectrahedronTest, Empty) { + Spectrahedron spect; + EXPECT_EQ(spect.ambient_dimension(), 0); +} + +GTEST_TEST(SpectrahedronTest, Attributes) { + EXPECT_GT(Spectrahedron::supported_attributes().count( + solvers::ProgramAttribute::kPositiveSemidefiniteConstraint), + 0); +} + +GTEST_TEST(SpectrahedronTest, UnsupportedProgram) { + MathematicalProgram prog; + symbolic::Variable x = prog.NewContinuousVariables<1>()[0]; + prog.AddConstraint(x * x * x + x >= 0); // Add a generic constraint. + DRAKE_EXPECT_THROWS_MESSAGE(Spectrahedron(prog), ".*does not support.*"); +} + +/** + * A trivial SDP + * max X1(0, 1) + X1(1, 2), + * s.t X1 ∈ ℝ³ˣ³ is psd, + * X1(0, 0) + X1(1, 1) + X1(2, 2) = 1, + * X1(0, 1) + X1(1, 2) - 2 * X1(0, 2) ≤ 0, + * X1(2, 2) ∈ [-1, 1]. + */ +GTEST_TEST(SpectrahedronTest, TrivialSdp1) { + MathematicalProgram prog; + auto X1 = prog.NewSymmetricContinuousVariables<3>(); + prog.AddLinearCost(-(X1(0, 1) + X1(1, 2))); + prog.AddPositiveSemidefiniteConstraint(X1); + prog.AddLinearEqualityConstraint(X1(0, 0) + X1(1, 1) + X1(2, 2), 1); + prog.AddLinearConstraint(X1(0, 1) + X1(1, 2) - 2 * X1(0, 2), -kInf, 0); + prog.AddBoundingBoxConstraint(-1, 1, X1(2, 2)); + auto result = Solve(prog); + ASSERT_TRUE(result.is_success()); + const Vector6d x_star = result.GetSolution(prog.decision_variables()); + Vector6d x_bad; + x_bad << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + + Spectrahedron spect(prog); + EXPECT_EQ(spect.ambient_dimension(), 3 * (3 + 1) / 2); + DRAKE_EXPECT_THROWS_MESSAGE(spect.IsBounded(), ".*not implemented yet.*"); + + const double kTol{1e-6}; + EXPECT_TRUE(spect.PointInSet(x_star, kTol)); + EXPECT_FALSE(spect.PointInSet(x_bad, kTol)); + + MathematicalProgram prog2; + auto x2 = prog2.NewContinuousVariables<6>("x"); + spect.AddPointInSetConstraints(&prog2, x2); + EXPECT_EQ(prog2.positive_semidefinite_constraints().size(), 1); + EXPECT_TRUE(prog2.CheckSatisfied(prog2.GetAllConstraints(), x_star)); + EXPECT_FALSE(prog2.CheckSatisfied(prog2.GetAllConstraints(), x_bad)); + + MathematicalProgram prog3; + auto x3 = prog3.NewContinuousVariables<6>("x"); + auto t3 = prog3.NewContinuousVariables<1>("t"); + spect.AddPointInNonnegativeScalingConstraints(&prog3, x3, t3[0]); + Vector xt_test; + xt_test << x_star, 1; + EXPECT_TRUE(prog3.CheckSatisfied(prog3.GetAllConstraints(), xt_test)); + EXPECT_TRUE( + prog3.CheckSatisfied(prog3.GetAllConstraints(), VectorXd::Zero(7))); + xt_test << x_bad, 1; + EXPECT_FALSE(prog3.CheckSatisfied(prog3.GetAllConstraints(), xt_test)); + + DRAKE_EXPECT_THROWS_MESSAGE( + spect.AddPointInNonnegativeScalingConstraints( + &prog3, MatrixXd::Identity(6, 6), Vector6d::Zero(), Vector1d::Zero(), + 0, x3, t3), + ".*not implemented yet.*"); +} + +} // namespace +} // namespace optimization +} // namespace geometry +} // namespace drake