Adds Spectrahedron isa ConvexSet

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<Point, copyable_unique_ptr<ConvexSet>>();
   }
 
+  // Spectrahedron
+  {
+    const auto& cls_doc = doc.Spectrahedron;
+    py::class_<Spectrahedron, ConvexSet>(m, "Spectrahedron", cls_doc.doc)
+        .def(py::init<>(), cls_doc.ctor.doc_0args)
+        .def(py::init<const solvers::MathematicalProgram&>(), py::arg("prog"),
+            cls_doc.ctor.doc_1args);
+    py::implicitly_convertible<Spectrahedron, copyable_unique_ptr<ConvexSet>>();
+  }
+
   // VPolytope
   {
     const auto& cls_doc = doc.VPolytope;

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)

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,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "spectrahedron_test",
+    deps = [
+        ":convex_set",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+        "//solvers:solve",
+    ],
+)
+
 drake_cc_googletest(
     name = "vpolytope_test",
     data = ["//geometry:test_obj_files"],

geometry/optimization/spectrahedron.cc

@@ -0,0 +1,191 @@
+#include "drake/geometry/optimization/spectrahedron.h"
+
+#include <limits>
+#include <memory>
+
+#include <fmt/format.h>
+
+#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<const VectorXDecisionVariable>& vars,
+    std::vector<int> indices) {
+  VectorXDecisionVariable new_vars(indices.size());
+  for (int i = 0; i < ssize(indices); ++i) {
+    new_vars[i] = vars[indices[i]];
+  }
+  return new_vars;
+}
+
+}  // namespace
+
+Spectrahedron::Spectrahedron()
+    : ConvexSet(&ConvexSetCloner<Spectrahedron>, 0) {}
+
+Spectrahedron::Spectrahedron(const MathematicalProgram& prog)
+    : ConvexSet(&ConvexSetCloner<Spectrahedron>, prog.num_vars()) {
+  for (const ProgramAttribute& 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;
+
+const ProgramAttributes& Spectrahedron::supported_attributes() {
+  static const never_destroyed<ProgramAttributes> kSupportedAttributes{
+      ProgramAttributes{ProgramAttribute::kLinearCost,
+                        ProgramAttribute::kLinearConstraint,
+                        ProgramAttribute::kLinearEqualityConstraint,
+                        ProgramAttribute::kPositiveSemidefiniteConstraint}};
+  return kSupportedAttributes.access();
+}
+
+bool Spectrahedron::DoIsBounded() const {
+  throw std::runtime_error(
+      "Spectrahedron::IsBounded() is not implemented yet.");
+}
+
+bool Spectrahedron::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
+                                 double tol) const {
+  return sdp_->CheckSatisfied(sdp_->GetAllConstraints(), x, tol);
+}
+
+void Spectrahedron::DoAddPointInSetConstraints(
+    MathematicalProgram* prog,
+    const Eigen::Ref<const VectorXDecisionVariable>& 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<Binding<Constraint>>
+Spectrahedron::DoAddPointInNonnegativeScalingConstraints(
+    MathematicalProgram* prog,
+    const Eigen::Ref<const VectorXDecisionVariable>& x,
+    const Variable& t) const {
+  DRAKE_DEMAND(x.size() == sdp_->num_vars());
+  std::vector<Binding<Constraint>> constraints;
+  const double kInf = std::numeric_limits<double>::infinity();
+
+  // Helper function that given a binding.variables() returns the corresponding
+  // subset of variables from `x` with `t` tacked on the end.
+  auto stack_xt = [&x, &t, this](const VectorXDecisionVariable& bind_vars) {
+    VectorXDecisionVariable xt(bind_vars.size() + 1);
+    xt << GetVariablesByIndex(x, sdp_->FindDecisionVariableIndices(bind_vars)),
+        t;
+    return xt;
+  };
+
+  // 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 = stack_xt(binding.variables());
+    MatrixXd Ab =
+        MatrixXd::Identity(binding.evaluator()->num_constraints(), vars.size());
+    // 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 = stack_xt(binding.variables());
+    MatrixXd Ab(binding.evaluator()->num_constraints(), vars.size());
+    Ab.leftCols(binding.evaluator()->num_vars()) =
+        binding.evaluator()->GetDenseA();
+    Ab.rightCols<1>() = -binding.evaluator()->lower_bound();
+    prog->AddLinearEqualityConstraint(Ab, 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 = stack_xt(binding.variables());
+    MatrixXd Ab(binding.evaluator()->num_constraints(), vars.size());
+    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.
+    VectorXDecisionVariable vars = GetVariablesByIndex(
+        x, sdp_->FindDecisionVariableIndices(binding.variables()));
+    constraints.emplace_back(prog->AddConstraint(
+        binding.evaluator(),
+        Eigen::Map<MatrixX<Variable>>(vars.data(),
+                                      binding.evaluator()->matrix_rows(),
+                                      binding.evaluator()->matrix_rows())));
+  }
+  return constraints;
+}
+
+std::vector<Binding<Constraint>>
+Spectrahedron::DoAddPointInNonnegativeScalingConstraints(
+    MathematicalProgram* prog, const Eigen::Ref<const MatrixXd>& A,
+    const Eigen::Ref<const VectorXd>& b, const Eigen::Ref<const VectorXd>& c,
+    double d, const Eigen::Ref<const VectorXDecisionVariable>& x,
+    const Eigen::Ref<const VectorXDecisionVariable>& 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<std::unique_ptr<Shape>, 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

geometry/optimization/spectrahedron.h

@@ -0,0 +1,75 @@
+#pragma once
+
+#include <memory>
+#include <optional>
+#include <utility>
+#include <vector>
+
+#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 `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();
+
+  // 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<const Eigen::VectorXd>& x,
+                    double tol) const final;
+
+  void DoAddPointInSetConstraints(
+      solvers::MathematicalProgram* prog,
+      const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars)
+      const final;
+
+  std::vector<solvers::Binding<solvers::Constraint>>
+  DoAddPointInNonnegativeScalingConstraints(
+      solvers::MathematicalProgram* prog,
+      const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
+      const symbolic::Variable& t) const final;
+
+  std::vector<solvers::Binding<solvers::Constraint>>
+  DoAddPointInNonnegativeScalingConstraints(
+      solvers::MathematicalProgram* prog,
+      const Eigen::Ref<const Eigen::MatrixXd>& A,
+      const Eigen::Ref<const Eigen::VectorXd>& b,
+      const Eigen::Ref<const Eigen::VectorXd>& c, double d,
+      const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
+      const Eigen::Ref<const solvers::VectorXDecisionVariable>& t) const final;
+
+  std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
+      const final;
+
+  copyable_unique_ptr<solvers::MathematicalProgram> sdp_{};
+};
+
+}  // namespace optimization
+}  // namespace geometry
+}  // namespace drake