[solvers] Add SemidefiniteRelaxationOptions (RobotLocomotion#21500)

bindings/pydrake/solvers/solvers_py_semidefinite_relaxation.cc

@@ -12,16 +12,53 @@ void DefineSolversSemidefiniteRelaxation(py::module m) {
   using namespace drake::solvers;
   constexpr auto& doc = pydrake_doc.drake.solvers;
 
+  {
+    const auto& cls_doc = doc.SemidefiniteRelaxationOptions;
+    py::class_<SemidefiniteRelaxationOptions> options(
+        m, "SemidefiniteRelaxationOptions", cls_doc.doc);
+    options.def(ParamInit<SemidefiniteRelaxationOptions>())
+        .def_readwrite("add_implied_linear_equality_constraints",
+            &SemidefiniteRelaxationOptions::
+                add_implied_linear_equality_constraints,
+            cls_doc.add_implied_linear_equality_constraints.doc)
+        .def_readwrite("add_implied_linear_constraints",
+            &SemidefiniteRelaxationOptions::add_implied_linear_constraints,
+            cls_doc.add_implied_linear_constraints.doc)
+        .def_readwrite("preserve_convex_quadratic_constraints",
+            &SemidefiniteRelaxationOptions::
+                preserve_convex_quadratic_constraints,
+            cls_doc.preserve_convex_quadratic_constraints.doc)
+        .def("set_to_strongest",
+            &SemidefiniteRelaxationOptions::set_to_strongest,
+            cls_doc.set_to_strongest.doc)
+        .def("set_to_weakest", &SemidefiniteRelaxationOptions::set_to_weakest,
+            cls_doc.set_to_weakest.doc)
+        .def("__repr__", [](const SemidefiniteRelaxationOptions& self) {
+          return py::str(
+              "SemidefiniteRelaxationOptions("
+              "add_implied_linear_equality_constraints={}, "
+              "add_implied_linear_constraints={}, "
+              "preserve_convex_quadratic_constraints={})")
+              .format(self.add_implied_linear_equality_constraints,
+                  self.add_implied_linear_constraints,
+                  self.preserve_convex_quadratic_constraints);
+        });
+  }
+
   m.def("MakeSemidefiniteRelaxation",
-      py::overload_cast<const MathematicalProgram&>(
+      py::overload_cast<const MathematicalProgram&,
+          const SemidefiniteRelaxationOptions&>(
           &solvers::MakeSemidefiniteRelaxation),
-      py::arg("prog"), doc.MakeSemidefiniteRelaxation.doc_1args);
+      py::arg("prog"), py::arg("options") = SemidefiniteRelaxationOptions(),
+      doc.MakeSemidefiniteRelaxation.doc_2args);
   m.def("MakeSemidefiniteRelaxation",
       py::overload_cast<const MathematicalProgram&,
-          const std::vector<symbolic::Variables>&>(
+          const std::vector<symbolic::Variables>&,
+          const SemidefiniteRelaxationOptions&>(
           &solvers::MakeSemidefiniteRelaxation),
       py::arg("prog"), py::arg("variable_groups"),
-      doc.MakeSemidefiniteRelaxation.doc_2args);
+      py::arg("options") = SemidefiniteRelaxationOptions(),
+      doc.MakeSemidefiniteRelaxation.doc_3args);
 }
 
 }  // namespace internal

bindings/pydrake/solvers/test/semidefinite_relaxation_test.py

@@ -3,6 +3,7 @@
 
 from pydrake.solvers import (
     MakeSemidefiniteRelaxation,
+    SemidefiniteRelaxationOptions,
     MathematicalProgram,
 )
 from pydrake.symbolic import Variables
@@ -16,7 +17,8 @@ def test_MakeSemidefiniteRelaxation(self):
         lb = np.array([1.3, -.24, 0.25])
         ub = np.array([5.6, 0.1, 1.4])
         prog.AddLinearConstraint(A, lb, ub, y)
-        relaxation = MakeSemidefiniteRelaxation(prog=prog)
+        options = SemidefiniteRelaxationOptions()
+        relaxation = MakeSemidefiniteRelaxation(prog=prog, options=options)
 
         self.assertEqual(relaxation.num_vars(), 6)
         self.assertEqual(len(relaxation.positive_semidefinite_constraints()),
@@ -39,8 +41,9 @@ def test_MakeSemidefiniteRelaxationWithGroups(self):
         ub = np.array([5.6, 0.1, 1.4])
         prog.AddLinearConstraint(A, lb, ub, x)
         prog.AddQuadraticConstraint(A@A.T, lb, 0, 5, y)
+        options = SemidefiniteRelaxationOptions()
         relaxation = MakeSemidefiniteRelaxation(
-            prog=prog, variable_groups=[x_vars, y_vars])
+            prog=prog, variable_groups=[x_vars, y_vars], options=options)
 
         # The semidefinite variables have 4 and 3 rows respectively,
         # with the "1" variable double counted. Therefore, there are
@@ -50,3 +53,20 @@ def test_MakeSemidefiniteRelaxationWithGroups(self):
                          2)
         self.assertEqual(len(relaxation.linear_equality_constraints()), 1)
         self.assertEqual(len(relaxation.linear_constraints()), 2+1)
+
+    def test_MakeSemidefiniteRelaxationOptions(self):
+        options = SemidefiniteRelaxationOptions()
+        options.add_implied_linear_equality_constraints = True
+        options.add_implied_linear_constraints = True
+        options.preserve_convex_quadratic_constraints = True
+        self.assertTrue(options.add_implied_linear_equality_constraints)
+        self.assertTrue(options.add_implied_linear_constraints)
+        self.assertTrue(options.preserve_convex_quadratic_constraints)
+
+        options.set_to_weakest()
+        self.assertFalse(options.add_implied_linear_equality_constraints)
+        self.assertFalse(options.add_implied_linear_constraints)
+        self.assertFalse(options.preserve_convex_quadratic_constraints)
+
+        options.set_to_strongest()
+        self.assertTrue(options.add_implied_linear_constraints)

solvers/BUILD.bazel

@@ -1078,7 +1078,6 @@ drake_cc_library(
     hdrs = ["semidefinite_relaxation.h"],
     interface_deps = [
         ":mathematical_program",
-        "//math:matrix_util",
     ],
     deps = [
         ":semidefinite_relaxation_internal",
@@ -1287,6 +1286,7 @@ drake_cc_googletest(
         ":semidefinite_relaxation",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
+        "//math:matrix_util",
     ],
 )
 

solvers/semidefinite_relaxation.cc

@@ -1,19 +1,9 @@
 #include "drake/solvers/semidefinite_relaxation.h"
 
-#include <algorithm>
-#include <functional>
-#include <initializer_list>
-#include <limits>
 #include <map>
 #include <optional>
 #include <set>
-#include <string>
-#include <utility>
-#include <vector>
 
-#include "drake/common/fmt_eigen.h"
-#include "drake/math/matrix_util.h"
-#include "drake/solvers/program_attribute.h"
 #include "drake/solvers/semidefinite_relaxation_internal.h"
 
 namespace drake {
@@ -29,80 +19,75 @@ using symbolic::Variables;
 
 namespace {
 
-// TODO(AlexandreAmice) Move all these methods to
-// semidefinite_relaxation_internal.
+// Validate that we can construct the semidefinite relaxation of this program
+// and prepare a relaxation program. The relaxation program will be initialized
+// as a clone of the program prog, with the additional variable "one"
+// constrained to be equal to one. This relaxation is processed by
+// DoMakeSemidefiniteRelaxation in different ways.
+std::unique_ptr<MathematicalProgram> InitializeRelaxation(
+    const MathematicalProgram& prog, const Variable& one) {
+  internal::ValidateProgramIsSupported(prog);
+  auto relaxation = prog.Clone();
+  relaxation->AddDecisionVariables(Vector1<Variable>(one));
+  relaxation->AddLinearEqualityConstraint(one, 1);
+  return relaxation;
+}
 
-// Constructs the semidefinite relaxation of the program prog and adds it to
-// relaxation. We assume that the program attributes of prog are already
-// validated and that relaxation already contains all the variables and
-// constraints of prog. The variable one is already constrained to be equal to
-// one. This is passed so it can be re-used across semidefinite variables in the
-// sparse version of MakeSemidefiniteRelaxation. Returns the X matrix of the
-// semidefinite relaxation.
+// Constructs the semidefinite relaxation of the program sub_prog and adds it to
+// relaxation. We assume that sub_prog is a sub-program of some larger program
+// prog and relaxation was initialized by a call to InitializeRelaxation on
+// prog. By sub-program, we mean that sub_prog contains only variables and
+// constraints found in prog.
+//
+// This is equivalent to assuming that the program attributes of sub_prog are
+// already validated, that relaxation already contains all the variables and
+// constraints of sub_prog, and that the variable one is already constrained to
+// be equal to one. The variable one is passed so it can be re-used across
+// semidefinite variables in the sparse version of MakeSemidefiniteRelaxation.
+//
+// Returns the X matrix of the semidefinite relaxation.
 MatrixXDecisionVariable DoMakeSemidefiniteRelaxation(
-    const MathematicalProgram& prog, const Variable& one,
+    const MathematicalProgram& sub_prog, const Variable& one,
+    const SemidefiniteRelaxationOptions& options,
     MathematicalProgram* relaxation,
     std::optional<int> group_number = std::nullopt) {
-  // Build a symmetric matrix X of decision variables using the original
-  // program variables (so that GetSolution, etc, works using the original
-  // variables).
-  relaxation->AddDecisionVariables(prog.decision_variables());
-  MatrixX<Variable> X(prog.num_vars() + 1, prog.num_vars() + 1);
-  // X = xxᵀ; x = [prog.decision_vars(); 1].
-  std::string name =
-      group_number.has_value() ? fmt::format("Y{}", group_number.value()) : "Y";
-  X.topLeftCorner(prog.num_vars(), prog.num_vars()) =
-      relaxation->NewSymmetricContinuousVariables(prog.num_vars(), name);
-  // We sort the variables so that the matrix X is ordered in a predictable way.
-  // This makes it easier when using the sparsity groups to make the
-  // semidefinite matrices agree.
-  VectorX<Variable> sorted_variables = prog.decision_variables();
-  std::sort(sorted_variables.data(),
-            sorted_variables.data() + sorted_variables.size(),
-            std::less<Variable>{});
-  X.topRightCorner(prog.num_vars(), 1) = sorted_variables;
-  X.bottomLeftCorner(1, prog.num_vars()) = sorted_variables.transpose();
-
+  MatrixX<Variable> X;
   std::map<Variable, int> variables_to_sorted_indices;
-  int i = 0;
-  for (const auto& v : sorted_variables) {
-    variables_to_sorted_indices[v] = i++;
-  }
-
-  // X(-1,-1) = 1.
-  X(prog.num_vars(), prog.num_vars()) = one;
-
-  // X ≽ 0.
-  relaxation->AddPositiveSemidefiniteConstraint(X);
+  internal::InitializeSemidefiniteRelaxationForProg(
+      sub_prog, one, relaxation, &X, &variables_to_sorted_indices,
+      group_number);
 
   internal::DoLinearizeQuadraticCostsAndConstraints(
-      prog, X, variables_to_sorted_indices, relaxation);
-  internal::DoAddImpliedLinearConstraints(prog, X, variables_to_sorted_indices,
-                                          relaxation);
-  internal::DoAddImpliedLinearEqualityConstraints(
-      prog, X, variables_to_sorted_indices, relaxation);
+      sub_prog, X, variables_to_sorted_indices, relaxation,
+      options.preserve_convex_quadratic_constraints);
+
+  if (options.add_implied_linear_constraints) {
+    internal::DoAddImpliedLinearConstraints(
+        sub_prog, X, variables_to_sorted_indices, relaxation);
+  }
+  if (options.add_implied_linear_equality_constraints) {
+    internal::DoAddImpliedLinearEqualityConstraints(
+        sub_prog, X, variables_to_sorted_indices, relaxation);
+  }
   return X;
 }
 }  // namespace
 
 std::unique_ptr<MathematicalProgram> MakeSemidefiniteRelaxation(
-    const MathematicalProgram& prog) {
-  internal::ValidateProgramIsSupported(prog);
-  auto relaxation = prog.Clone();
+    const MathematicalProgram& prog,
+    const SemidefiniteRelaxationOptions& options) {
   const Variable one("one");
-  relaxation->AddDecisionVariables(Vector1<Variable>(one));
-  relaxation->AddLinearEqualityConstraint(one, 1);
-  DoMakeSemidefiniteRelaxation(prog, one, relaxation.get());
+  auto relaxation = InitializeRelaxation(prog, one);
+  DoMakeSemidefiniteRelaxation(prog, one, options, relaxation.get());
   return relaxation;
 }
 
 std::unique_ptr<MathematicalProgram> MakeSemidefiniteRelaxation(
     const MathematicalProgram& prog,
-    const std::vector<symbolic::Variables>& variable_groups) {
-  auto relaxation = prog.Clone();
+    const std::vector<symbolic::Variables>& variable_groups,
+    const SemidefiniteRelaxationOptions& options) {
   const Variable one("one");
-  relaxation->AddDecisionVariables(Vector1<Variable>(one));
-  relaxation->AddLinearEqualityConstraint(one, 1);
+  auto relaxation = InitializeRelaxation(prog, one);
 
   // The semidefinite relaxation of each variable group will be computed
   // individually and any variables which overlap in the programs will later be
@@ -153,7 +138,7 @@ std::unique_ptr<MathematicalProgram> MakeSemidefiniteRelaxation(
   int group_number = 0;
   for (const auto& [group, container_program] : groups_to_container_programs) {
     groups_to_psd_variables.emplace(
-        group, DoMakeSemidefiniteRelaxation(container_program, one,
+        group, DoMakeSemidefiniteRelaxation(container_program, one, options,
                                             relaxation.get(), group_number));
     ++group_number;
   }

solvers/semidefinite_relaxation.h

@@ -11,6 +11,57 @@ namespace solvers {
 // TODO(russt): Add an option for using diagonal dominance and/or
 // scaled-diagonal dominance instead of the PSD constraint.
 
+/** Configuration options for the MakeSemidefiniteRelaxation. Throughout these
+ * options, we refer to the variables of the original optimization program as y,
+ * and the semidefinite variable of the associate relaxation as X.
+ *
+ * X has the structure
+ * X = [Y,    y]
+ *     [yᵀ, one]
+ */
+struct SemidefiniteRelaxationOptions {
+  /** Given a program with the linear constraints Ay ≤ b, sets whether to add
+   * the implied linear constraints [A,-b]X[A,-b]ᵀ ≤ 0 to the semidefinite
+   * relaxation.*/
+  bool add_implied_linear_equality_constraints = true;
+
+  /** Given a program with the linear equality constraints Ay = b, sets whether
+   * to add the implied linear constraints [A, -b]X = 0 to the semidefinite
+   * relaxation.*/
+  bool add_implied_linear_constraints = true;
+
+  // TODO(Alexandre.Amice): change this to true by default.
+
+  /** Given a program with convex quadratic constraints, sets whether
+   * equivalent rotated second order cone constraints are enforced on the last
+   * column of X in the semidefinite relaxation. This ensures that the last
+   * column of X, which are a relaxation of the original program's variables
+   * satisfy the original progam's quadratic constraints.*/
+  bool preserve_convex_quadratic_constraints = false;
+
+  /** Configure the semidefinite relaxation options to provide the strongest
+   * possible semidefinite relaxation that we currently support. This in general
+   * will give the tightest convex relaxation we support, but the longest solve
+   * times.*/
+  void set_to_strongest() {
+    add_implied_linear_equality_constraints = true;
+    add_implied_linear_constraints = true;
+    preserve_convex_quadratic_constraints = true;
+  }
+
+  /** Configure the semidefinite relaxation options to provide the weakest
+   * semidefinite relaxation that we currently support. This in general will
+   * create the loosest convex relaxation we support, but the shortest solve
+   * times. This is equivalent to the standard Shor Relaxation (see Quadratic
+   * Optimization Problems by NZ Shor or Semidefinite Programming by
+   * Vandenberghe and Boyd). */
+  void set_to_weakest() {
+    add_implied_linear_equality_constraints = false;
+    add_implied_linear_constraints = false;
+    preserve_convex_quadratic_constraints = false;
+  }
+};
+
 // TODO(russt): Consider adding Y as an optional argument return value, to help
 // users know the decision variables associated with Y.
 
@@ -30,7 +81,8 @@ namespace solvers {
  linear nor quadratic.
  */
 std::unique_ptr<MathematicalProgram> MakeSemidefiniteRelaxation(
-    const MathematicalProgram& prog);
+    const MathematicalProgram& prog,
+    const SemidefiniteRelaxationOptions& options = {});
 
 /** A version of MakeSemidefiniteRelaxation that allows for specifying the
  * sparsity of the relaxation.
@@ -105,7 +157,8 @@ std::unique_ptr<MathematicalProgram> MakeSemidefiniteRelaxation(
  */
 std::unique_ptr<MathematicalProgram> MakeSemidefiniteRelaxation(
     const MathematicalProgram& prog,
-    const std::vector<symbolic::Variables>& variable_groups);
+    const std::vector<symbolic::Variables>& variable_groups,
+    const SemidefiniteRelaxationOptions& options = {});
 
 }  // namespace solvers
 }  // namespace drake