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.

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,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"],

geometry/optimization/spectrahedron.cc

@@ -0,0 +1,162 @@
+#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::VectorXDecisionVariable;
+using symbolic::Variable;
+
+
+Spectrahedron::Spectrahedron()
+    : ConvexSet(&ConvexSetCloner<Spectrahedron>, 0) {}
+
+Spectrahedron::Spectrahedron(const MathematicalProgram& prog)
+    : ConvexSet(&ConvexSetCloner<Spectrahedron>, prog.num_vars()) {
+  DRAKE_DEMAND(solvers::GetProgramType(prog) == solvers::ProgramType::kSDP);
+  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<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(),
+        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();
+
+  // 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 << 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 << 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 << 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_->lorentz_cone_constraints()) {
+    unused(binding);
+    throw std::runtime_error(
+        "Spectrahedron::PointInNonnegativeScalingConstraints() is not "
+        "implemented yet for Lorentz cone constraints.");
+  }
+  for (const auto& binding : sdp_->rotated_lorentz_cone_constraints()) {
+    unused(binding);
+    throw std::runtime_error(
+        "Spectrahedron::PointInNonnegativeScalingConstraints() is not "
+        "implemented yet for rotated Lorentz cone constraints.");
+  }
+  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(),
+        x(sdp_->FindDecisionVariableIndices(binding.variables()))
+            .reshaped(binding.evaluator()->matrix_rows(),
+                      binding.evaluator()->matrix_rows())));
+  }
+  return constraints;
+}
+
+std::vector<Binding<Constraint>>
+Spectrahedron::DoAddPointInNonnegativeScalingConstraints(
+    MathematicalProgram* prog, const Eigen::Ref<const MatrixXd>& A_x,
+    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 {
+  throw std::runtime_error(
+      "Spectrahedron::PointInNonnegativeScalingConstraints() is not "
+      "implemented yet for the case where A_x, 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,70 @@
+#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.
+  @pre GetProgramType(prog) must be kSDP.
+  @pre @p prog must have exactly one positive-semidefinite constraint which is
+  bound to *all* of the decision variables. */
+  explicit Spectrahedron(const solvers::MathematicalProgram& prog);
+
+  ~Spectrahedron() final;
+
+ 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_x,
+      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