Scs and Clarabel parses 2x2 PSD/LMI constraints as second order cone …

…constraints. (#22219)

A second order cone problem is easier to solve than a positive semidefinite programming problem.

solvers/aggregate_costs_constraints.cc

@@ -687,12 +687,12 @@ void ParsePositiveSemidefiniteConstraints(
     std::vector<std::optional<int>>* lmi_y_start_indices) {
   DRAKE_ASSERT(ssize(*b) == *A_row_count);
   DRAKE_ASSERT(psd_cone_length != nullptr);
-  // Make sure that each triplet in A_triplets has row no larger than
+  // Make sure that each triplet in A_triplets has row smaller than
   // *A_row_count.
   // Use kDrakeAssertIsArmed to bypass the entire for loop in the release mode.
   if (kDrakeAssertIsArmed) {
     for (const auto& A_triplet : *A_triplets) {
-      DRAKE_DEMAND(A_triplet.row() <= *A_row_count);
+      DRAKE_DEMAND(A_triplet.row() < *A_row_count);
     }
   }
   DRAKE_ASSERT(psd_cone_length->empty());
@@ -701,8 +701,7 @@ void ParsePositiveSemidefiniteConstraints(
   DRAKE_ASSERT(lmi_y_start_indices->empty());
   const double sqrt2 = std::sqrt(2);
   for (const auto& psd_constraint : prog.positive_semidefinite_constraints()) {
-    if (psd_constraint.evaluator()->matrix_rows() > 1) {
-      psd_constraint.evaluator()->WarnOnSmallMatrixSize();
+    if (psd_constraint.evaluator()->matrix_rows() > 2) {
       // PositiveSemidefiniteConstraint encodes the matrix X being psd.
       // We convert it to SCS/Clarabel form
       // A * x + s = 0
@@ -752,8 +751,7 @@ void ParsePositiveSemidefiniteConstraints(
   }
   for (const auto& lmi_constraint :
        prog.linear_matrix_inequality_constraints()) {
-    if (lmi_constraint.evaluator()->matrix_rows() > 1) {
-      lmi_constraint.evaluator()->WarnOnSmallMatrixSize();
+    if (lmi_constraint.evaluator()->matrix_rows() > 2) {
       // LinearMatrixInequalityConstraint encodes
       // F₀ + x₁*F₁ + x₂*F₂ + ... + xₙFₙ is p.s.d
       // We convert this to SCS/Clarabel form as
@@ -830,12 +828,12 @@ void ParseScalarPositiveSemidefiniteConstraints(
   DRAKE_ASSERT(ssize(*b) == *A_row_count);
   DRAKE_ASSERT(new_positive_cone_length != nullptr);
   *new_positive_cone_length = 0;
-  // Make sure that each triplet in A_triplets has row no larger than
+  // Make sure that each triplet in A_triplets has row smaller than
   // *A_row_count.
   // Use kDrakeAssertIsArmed to bypass the entire for loop in the release mode.
   if (kDrakeAssertIsArmed) {
     for (const auto& A_triplet : *A_triplets) {
-      DRAKE_DEMAND(A_triplet.row() <= *A_row_count);
+      DRAKE_DEMAND(A_triplet.row() < *A_row_count);
     }
   }
   scalar_psd_dual_indices->reserve(
@@ -877,6 +875,102 @@ void ParseScalarPositiveSemidefiniteConstraints(
     }
   }
 }
+
+void Parse2x2PositiveSemidefiniteConstraints(
+    const MathematicalProgram& prog,
+    std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
+    int* A_row_count, int* num_new_second_order_cones,
+    std::vector<std::optional<int>>* twobytwo_psd_dual_start_indices,
+    std::vector<std::optional<int>>* twobytwo_lmi_dual_start_indices) {
+  DRAKE_ASSERT(ssize(*b) == *A_row_count);
+  DRAKE_ASSERT(num_new_second_order_cones != nullptr);
+  *num_new_second_order_cones = 0;
+  // Make sure that each triplet in A_triplets has row smaller than
+  // *A_row_count.
+  // Use kDrakeAssertIsArmed to bypass the entire for loop in the release mode.
+  if (kDrakeAssertIsArmed) {
+    for (const auto& A_triplet : *A_triplets) {
+      DRAKE_DEMAND(A_triplet.row() < *A_row_count);
+    }
+  }
+  twobytwo_psd_dual_start_indices->reserve(
+      prog.positive_semidefinite_constraints().size());
+  twobytwo_lmi_dual_start_indices->reserve(
+      prog.linear_matrix_inequality_constraints().size());
+  for (const auto& psd_constraint : prog.positive_semidefinite_constraints()) {
+    if (psd_constraint.evaluator()->matrix_rows() == 2) {
+      // The PSD constraint imposes
+      // [x(0) x(2)] is psd.
+      // [x(1) x(3)]
+      // where (x(0), x(1), x(2), x(3)) is psd_constraint.variables() (Note
+      // that x(1) and x(2) are the same).
+      // This is equivalent to
+      // [x(0) + x(3)]
+      // [x(0) - x(3)]  in lorentz cone.
+      // [  2*x(1)   ]
+      // Namely
+      // [-x(0) - x(3) + s(0)]   [0]
+      // [-x(0) + x(3) + s(1)] = [0]
+      // [ -2 * x(1) + s(2)  ]   [0]
+      // s in second order cone.
+      const int x0_index =
+          prog.FindDecisionVariableIndex(psd_constraint.variables()[0]);
+      const int x1_index =
+          prog.FindDecisionVariableIndex(psd_constraint.variables()[1]);
+      const int x3_index =
+          prog.FindDecisionVariableIndex(psd_constraint.variables()[3]);
+      A_triplets->emplace_back(*A_row_count, x0_index, -1);
+      A_triplets->emplace_back(*A_row_count, x3_index, -1);
+      A_triplets->emplace_back(*A_row_count + 1, x0_index, -1);
+      A_triplets->emplace_back(*A_row_count + 1, x3_index, 1);
+      A_triplets->emplace_back(*A_row_count + 2, x1_index, -2);
+      b->push_back(0);
+      b->push_back(0);
+      b->push_back(0);
+      ++(*num_new_second_order_cones);
+      twobytwo_psd_dual_start_indices->push_back(*A_row_count);
+      *A_row_count += 3;
+    } else {
+      twobytwo_psd_dual_start_indices->push_back(std::nullopt);
+    }
+  }
+  for (const auto& lmi_constraint :
+       prog.linear_matrix_inequality_constraints()) {
+    if (lmi_constraint.evaluator()->matrix_rows() == 2) {
+      // The constraint imposes
+      // F[0] + ∑ᵢF[1+i] * x(i) is psd.
+      // This is equivalent to
+      // [F[0](0, 0) + F[0](1, 1) + ∑ᵢ(F[1+i](0, 0) + F[1+i)(1, 1)) * x[i]]
+      // [F[0](0, 0) - F[0](1, 1) + ∑ᵢ(F[1+i](0, 0) - F[1+i)(1, 1)) * x[i]]
+      // [     2 * F[0](0, 1) + 2 * ∑ᵢF[1+i](0, 1)*x[i]                   ]
+      // is in Lorentz cone.
+      // Writing it in SCS/Clarabel format
+      // -∑ᵢ(F[1+i](0,0) + F[1+i](1,1))x[i] + s[0] = F[0](0, 0) + F[0](1, 1)
+      // -∑ᵢ(F[1+i](0,0) - F[1+i](1,1))x[i] + s[1] = F[0](0, 0) - F[0](1, 1)
+      //             -2*∑ᵢF[1+i](0, 1)*x[i] + s[2] = 2 * F[0](0, 1)
+      //  s in second order cone.
+      const auto& F = lmi_constraint.evaluator()->F();
+      for (int i = 0; i < lmi_constraint.variables().rows(); ++i) {
+        const int var_index =
+            prog.FindDecisionVariableIndex(lmi_constraint.variables()(i));
+        A_triplets->emplace_back(*A_row_count, var_index,
+                                 -F[1 + i](0, 0) - F[1 + i](1, 1));
+        A_triplets->emplace_back(*A_row_count + 1, var_index,
+                                 -F[1 + i](0, 0) + F[1 + i](1, 1));
+        A_triplets->emplace_back(*A_row_count + 2, var_index,
+                                 -2 * F[1 + i](0, 1));
+      }
+      b->push_back(F[0](0, 0) + F[0](1, 1));
+      b->push_back(F[0](0, 0) - F[0](1, 1));
+      b->push_back(2 * F[0](0, 1));
+      ++(*num_new_second_order_cones);
+      twobytwo_lmi_dual_start_indices->push_back(*A_row_count);
+      *A_row_count += 3;
+    } else {
+      twobytwo_lmi_dual_start_indices->push_back(std::nullopt);
+    }
+  }
+}
 }  // namespace internal
 }  // namespace solvers
 }  // namespace drake

solvers/aggregate_costs_constraints.h

@@ -385,6 +385,29 @@ void ParseScalarPositiveSemidefiniteConstraints(
     int* A_row_count, int* new_positive_cone_length,
     std::vector<std::optional<int>>* scalar_psd_dual_indices,
     std::vector<std::optional<int>>* scalar_lmi_dual_indices);
+
+// Similar to ParsePositiveSemidefiniteConstraints(), but ony parses the 2x2 PSD
+// constraints in prog.positive_semidefinite_constraints() and
+// prog.linear_matrix_inequality_constraints(). They are treated as second order
+// cone constraints.
+// @param[out] num_new_second_order_cones The number of new second order cones
+// to be imposed by all the 2x2 psd matrices in the
+// prog.positive_semidefinite_constraints() and
+// prog.linear_matrix_inequality_constraints().
+// @param[out] twobytwo_psd_dual_start_indices
+// twobytwo_psd_dual_start_indices[i] is the starting dual variable index for
+// prog.positive_semidefinite_constraints()[i]. It is std::nullopt if the psd
+// constraint is not on a 2x2 matrix.
+// @param[out] twobytwo_lmi_dual_start_indices
+// twobytwo_lmi_dual_start_indices[i] is the starting dual variable index for
+// prog.linear_matrix_inequality_constraints()[i]. It is std::nullopt if the psd
+// matrix is not a 2x2 matrix.
+void Parse2x2PositiveSemidefiniteConstraints(
+    const MathematicalProgram& prog,
+    std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
+    int* A_row_count, int* num_new_second_order_cones,
+    std::vector<std::optional<int>>* twobytwo_psd_dual_start_indices,
+    std::vector<std::optional<int>>* twobytwo_lmi_dual_start_indices);
 }  // namespace internal
 }  // namespace solvers
 }  // namespace drake

solvers/clarabel_solver.cc

@@ -458,6 +458,16 @@ void ClarabelSolver::DoSolve2(const MathematicalProgram& prog,
   for (const int soc_length : second_order_cone_length) {
     cones.push_back(clarabel::SecondOrderConeT<double>(soc_length));
   }
+  // Parse PSD/LMI constraints on 2x2 matrices as second order cone constraints.
+  int num_second_order_cones_from_psd{};
+  std::vector<std::optional<int>> twobytwo_psd_y_start_indices;
+  std::vector<std::optional<int>> twobytwo_lmi_y_start_indices;
+  internal::Parse2x2PositiveSemidefiniteConstraints(
+      prog, &A_triplets, &b, &A_row_count, &num_second_order_cones_from_psd,
+      &twobytwo_psd_y_start_indices, &twobytwo_lmi_y_start_indices);
+  for (int i = 0; i < num_second_order_cones_from_psd; ++i) {
+    cones.push_back(clarabel::SecondOrderConeT<double>(3));
+  }
 
   std::vector<std::optional<int>> psd_cone_length;
   std::vector<std::optional<int>> lmi_cone_length;
@@ -526,6 +536,7 @@ void ClarabelSolver::DoSolve2(const MathematicalProgram& prog,
       linear_eq_y_start_indices, lorentz_cone_y_start_indices,
       rotated_lorentz_cone_y_start_indices, psd_y_start_indices,
       lmi_y_start_indices, scalar_psd_dual_indices, scalar_lmi_dual_indices,
+      twobytwo_psd_y_start_indices, twobytwo_lmi_y_start_indices,
       /*upper_triangular_psd=*/true, result);
   if (solution.status == clarabel::SolverStatus::Solved ||
       solution.status == clarabel::SolverStatus::AlmostSolved) {

solvers/scs_clarabel_common.cc

@@ -56,6 +56,32 @@ void ScalePsdConeDualVariable(int matrix_rows, bool upper_triangular_psd,
     }
   }
 }
+
+// The PSD constraint on a 2x2 matrix
+// [x(0) x(1)] is psd
+// [x(1) x(2)]
+// is converted to a second order cone
+// [x(0) + x(2)]
+// [x(0) - x(2)] is in Lorentz cone.
+// [  2 * x(1) ]
+// We need to convert the dual of the second order cone back to the dual of the
+// PSD cone. Note that both Lorentz cone and PSD cone are self-dual.
+Eigen::Vector3d SecondOrderConeDualToPsdDual(const Eigen::Vector3d& soc_dual) {
+  // soc_dual = k * [y(0) + y(2), y(0) - y(2), 2 * y(1)] for the PSD dual
+  // solution [y(0) y(1)]
+  //          [y(1) y(2)]
+  // where k is a scaling constant, to guarantee that the complementarity
+  // slackness is the same for using PSD cone with its dual, and the second
+  // order cone with its dual, namely
+  // [x(0) + x(2), x(0) - x(2), 2 * x(1)].dot(soc_dual) =
+  // trace([x(0) x(1)] * [y(0) y(1)])
+  //       [x(1) x(2)]   [y(1) y(2)]
+  // and we can compute k = 0.5
+  // Hence
+  // y = [soc_dual(0) + soc_dual(1), soc_dual(2), soc_dual(0) - soc_dual(1)]
+  return Eigen::Vector3d(soc_dual(0) + soc_dual(1), soc_dual(2),
+                         soc_dual(0) - soc_dual(1));
+}
 }  // namespace
 void SetDualSolution(
     const MathematicalProgram& prog,
@@ -69,6 +95,8 @@ void SetDualSolution(
     const std::vector<std::optional<int>>& lmi_y_start_indices,
     const std::vector<std::optional<int>>& scalar_psd_y_indices,
     const std::vector<std::optional<int>>& scalar_lmi_y_indices,
+    const std::vector<std::optional<int>>& twobytwo_psd_y_start_indices,
+    const std::vector<std::optional<int>>& twobytwo_lmi_y_start_indices,
     bool upper_triangular_psd, MathematicalProgramResult* result) {
   for (int i = 0; i < ssize(prog.linear_constraints()); ++i) {
     Eigen::VectorXd lin_con_dual = Eigen::VectorXd::Zero(
@@ -162,6 +190,10 @@ void SetDualSolution(
       result->set_dual_solution(
           prog.positive_semidefinite_constraints()[i],
           Vector1d(dual(scalar_psd_y_indices[i].value())));
+    } else if (twobytwo_psd_y_start_indices[i].has_value()) {
+      result->set_dual_solution(prog.positive_semidefinite_constraints()[i],
+                                SecondOrderConeDualToPsdDual(dual.segment<3>(
+                                    twobytwo_psd_y_start_indices[i].value())));
     }
   }
   for (int i = 0; i < ssize(prog.linear_matrix_inequality_constraints()); ++i) {
@@ -178,6 +210,10 @@ void SetDualSolution(
       result->set_dual_solution(
           prog.linear_matrix_inequality_constraints()[i],
           Vector1d(dual(scalar_lmi_y_indices[i].value())));
+    } else if (twobytwo_lmi_y_start_indices[i].has_value()) {
+      result->set_dual_solution(prog.linear_matrix_inequality_constraints()[i],
+                                SecondOrderConeDualToPsdDual(dual.segment<3>(
+                                    twobytwo_lmi_y_start_indices[i].value())));
     }
   }
 }

solvers/scs_clarabel_common.h

@@ -60,6 +60,8 @@ void SetDualSolution(
     const std::vector<std::optional<int>>& lmi_y_start_indices,
     const std::vector<std::optional<int>>& scalar_psd_y_indices,
     const std::vector<std::optional<int>>& scalar_lmi_y_indices,
+    const std::vector<std::optional<int>>& twobytwo_psd_y_start_indices,
+    const std::vector<std::optional<int>>& twobytwo_lmi_y_start_indices,
     bool upper_triangular_psd, MathematicalProgramResult* result);
 }  // namespace internal
 }  // namespace solvers

solvers/scs_solver.cc

@@ -493,6 +493,20 @@ void ScsSolver::DoSolve2(const MathematicalProgram& prog,
       prog, &A_triplets, &b, &A_row_count, &second_order_cone_length,
       &lorentz_cone_y_start_indices, &rotated_lorentz_cone_y_start_indices);
 
+  // Add PSD or LMI constraint on 2x2 matrices. This must be called with the
+  // other second order cone constraint parsing code before
+  // ParsePositiveSemidefiniteConstraints() as the 2x2 PSD/LMI constraints are
+  // formulated as second order cones.
+  int num_second_order_cones_from_psd{};
+  std::vector<std::optional<int>> twobytwo_psd_dual_start_indices;
+  std::vector<std::optional<int>> twobytwo_lmi_dual_start_indices;
+  internal::Parse2x2PositiveSemidefiniteConstraints(
+      prog, &A_triplets, &b, &A_row_count, &num_second_order_cones_from_psd,
+      &twobytwo_psd_dual_start_indices, &twobytwo_lmi_dual_start_indices);
+  for (int i = 0; i < num_second_order_cones_from_psd; ++i) {
+    second_order_cone_length.push_back(3);
+  }
+
   // Add L2NormCost. L2NormCost should be parsed together with the other second
   // order cone constraints, since we introduce new second order cone
   // constraints to formulate the L2 norm cost.
@@ -624,6 +638,7 @@ void ScsSolver::DoSolve2(const MathematicalProgram& prog,
       linear_eq_y_start_indices, lorentz_cone_y_start_indices,
       rotated_lorentz_cone_y_start_indices, psd_y_start_indices,
       lmi_y_start_indices, scalar_psd_dual_indices, scalar_lmi_dual_indices,
+      twobytwo_psd_dual_start_indices, twobytwo_lmi_dual_start_indices,
       /*upper_triangular_psd=*/false, result);
   // Set the solution_result enum and the optimal cost based on SCS status.
   if (solver_details.scs_status == SCS_SOLVED ||