Return dual solution for PSD constraint in SCS and Clarabel. (#22147)

Include dual for both PositiveSemidefiniteConstraint and LinearMatrixInequalityConstraint.

solvers/aggregate_costs_constraints.cc

@@ -681,7 +681,10 @@ void ParseExponentialConeConstraints(
 void ParsePositiveSemidefiniteConstraints(
     const MathematicalProgram& prog, bool upper_triangular,
     std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
-    int* A_row_count, std::vector<int>* psd_cone_length) {
+    int* A_row_count, std::vector<std::optional<int>>* psd_cone_length,
+    std::vector<std::optional<int>>* lmi_cone_length,
+    std::vector<std::optional<int>>* psd_y_start_indices,
+    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
@@ -693,6 +696,9 @@ void ParsePositiveSemidefiniteConstraints(
     }
   }
   DRAKE_ASSERT(psd_cone_length->empty());
+  DRAKE_ASSERT(lmi_cone_length->empty());
+  DRAKE_ASSERT(psd_y_start_indices->empty());
+  DRAKE_ASSERT(lmi_y_start_indices->empty());
   const double sqrt2 = std::sqrt(2);
   for (const auto& psd_constraint : prog.positive_semidefinite_constraints()) {
     psd_constraint.evaluator()->WarnOnSmallMatrixSize();
@@ -734,8 +740,9 @@ void ParsePositiveSemidefiniteConstraints(
         ++x_index_count;
       }
     }
-    (*A_row_count) += X_rows * (X_rows + 1) / 2;
     psd_cone_length->push_back(X_rows);
+    psd_y_start_indices->push_back(*A_row_count);
+    (*A_row_count) += X_rows * (X_rows + 1) / 2;
   }
   for (const auto& lmi_constraint :
        prog.linear_matrix_inequality_constraints()) {
@@ -796,8 +803,9 @@ void ParsePositiveSemidefiniteConstraints(
         ++A_cone_row_count;
       }
     }
+    lmi_cone_length->push_back(F_rows);
+    lmi_y_start_indices->push_back(*A_row_count);
     *A_row_count += F_rows * (F_rows + 1) / 2;
-    psd_cone_length->push_back(F_rows);
   }
 }
 

solvers/aggregate_costs_constraints.h

@@ -145,7 +145,7 @@ void ParseLinearCosts(const MathematicalProgram& prog, std::vector<double>* c,
 // @param[in/out] A_row_count The number of rows in A before and after calling
 // this function.
 // @param[out] linear_eq_y_start_indices linear_eq_y_start_indices[i] is the
-// starting index of the dual variable for the constraint
+// starting index of the dual variables for the constraint
 // prog.linear_equality_constraints()[i]. Namely y[linear_eq_y_start_indices[i]:
 // linear_eq_y_start_indices[i] +
 // prog.linear_equality_constraints()[i].evaluator()->num_constraints] are the
@@ -320,7 +320,7 @@ void ParseExponentialConeConstraints(
 // prog.linear_matrix_inequality_constraints() into SCS/Clarabel format.
 // A * x + s = b
 // s in K
-// Note that the SCS/Clarabel solver defines its psd cone with a √2 scaling on
+// Note that the SCS/Clarabel solver defines its PSD cone with a √2 scaling on
 // the off-diagonal terms in the positive semidefinite matrix. Refer to
 // https://www.cvxgrp.org/scs/api/cones.html#semidefinite-cones and
 // https://oxfordcontrol.github.io/ClarabelDocs/stable/examples/example_sdp/ for
@@ -336,13 +336,37 @@ void ParseExponentialConeConstraints(
 // prog.linear_matrix_inequality_constraints() will be appended to b.
 // @param[in/out] A_row_count The number of rows in A before and after calling
 // this function.
-// @param[out] psd_cone_length The length of all the psd cones from
-// prog.positive_semidefinite_constraints() and
-// prog.linear_matrix_inequality_constraints().
+// @param[out] psd_cone_length psd_cone_length[i] is the length of the PSD cones
+// from prog.positive_semidefinite_constraints()[i]. If this
+// PositiveSemidefiniteConstraint is not parsed as a PSD cone constraint in the
+// solver (for example, it is parsed as a linear constraint or second order cone
+// constraint), then psd_cone_length[i] = std::nullopt. The input should be an
+// empty vector.
+// @param[out] lmi_cone_length lmi_cone_length[i] is the length of the PSD cones
+// from prog.linear_matrix_inequality_constraints()[i]. If this
+// LinearMatrixInequalityConstraint is not parsed as a PSD cone constraint in
+// the solver (for example, it is parsed as a linear constraint or second order
+// cone constraint), then lmi_cone_length[i] = std::nullopt. The input should be
+// an empty vector.
+// @param[out] psd_y_start_indices y[psd_y_start_indices[i]:
+// psd_y_start_indices[i] + psd_cone_length[i]] are the dual variables for
+// prog.positive_semidefinite_constraints()[i]. If
+// prog.positive_semidefinite_constraints()[i] is not parsed as a PSD cone
+// constraint in the solver, then psd_y_start_indices[i] is std::nullopt. The
+// input should be an empty vector.
+// @param[out] lmi_y_start_indices y[lmi_y_start_indices[i]:
+// lmi_y_start_indices[i] + lmi_cone_length[i]] are the dual variable for
+// prog.linear_matrix_inequality_constraints()[i]. If
+// prog.linear_matrix_inequality_constraints()[i] is not parsed as a PSD cone
+// constraint in the solver, then lmi_y_start_indices[i] is std::nullopt. The
+// input should be an empty vector.
 void ParsePositiveSemidefiniteConstraints(
     const MathematicalProgram& prog, bool upper_triangular,
     std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
-    int* A_row_count, std::vector<int>* psd_cone_length);
+    int* A_row_count, std::vector<std::optional<int>>* psd_cone_length,
+    std::vector<std::optional<int>>* lmi_cone_length,
+    std::vector<std::optional<int>>* psd_y_start_indices,
+    std::vector<std::optional<int>>* lmi_y_start_indices);
 }  // namespace internal
 }  // namespace solvers
 }  // namespace drake

solvers/clarabel_solver.cc

@@ -1,6 +1,7 @@
 #include "drake/solvers/clarabel_solver.h"
 
 #include <fstream>
+#include <optional>
 #include <unordered_map>
 #include <utility>
 #include <vector>
@@ -446,12 +447,23 @@ void ClarabelSolver::DoSolve2(const MathematicalProgram& prog,
     cones.push_back(clarabel::SecondOrderConeT<double>(soc_length));
   }
 
-  std::vector<int> psd_cone_length;
+  std::vector<std::optional<int>> psd_cone_length;
+  std::vector<std::optional<int>> lmi_cone_length;
+  std::vector<std::optional<int>> psd_y_start_indices;
+  std::vector<std::optional<int>> lmi_y_start_indices;
   internal::ParsePositiveSemidefiniteConstraints(
       prog, /* upper triangular = */ true, &A_triplets, &b, &A_row_count,
-      &psd_cone_length);
-  for (const int length : psd_cone_length) {
-    cones.push_back(clarabel::PSDTriangleConeT<double>(length));
+      &psd_cone_length, &lmi_cone_length, &psd_y_start_indices,
+      &lmi_y_start_indices);
+  for (const auto& length : psd_cone_length) {
+    if (length.has_value()) {
+      cones.push_back(clarabel::PSDTriangleConeT<double>(*length));
+    }
+  }
+  for (const auto& length : lmi_cone_length) {
+    if (length.has_value()) {
+      cones.push_back(clarabel::PSDTriangleConeT<double>(*length));
+    }
   }
 
   internal::ParseExponentialConeConstraints(prog, &A_triplets, &b,
@@ -497,10 +509,11 @@ void ClarabelSolver::DoSolve2(const MathematicalProgram& prog,
       Eigen::Map<Eigen::VectorXd>(solution.x.data(), prog.num_vars()));
 
   SetBoundingBoxDualSolution(prog, solution.z, bbcon_dual_indices, result);
-  internal::SetDualSolution(prog, solution.z, linear_constraint_dual_indices,
-                            linear_eq_y_start_indices,
-                            lorentz_cone_y_start_indices,
-                            rotated_lorentz_cone_y_start_indices, result);
+  internal::SetDualSolution(
+      prog, solution.z, linear_constraint_dual_indices,
+      linear_eq_y_start_indices, lorentz_cone_y_start_indices,
+      rotated_lorentz_cone_y_start_indices, psd_y_start_indices,
+      lmi_y_start_indices, /*upper_triangular_psd=*/true, result);
   if (solution.status == clarabel::SolverStatus::Solved ||
       solution.status == clarabel::SolverStatus::AlmostSolved) {
     solution_result = SolutionResult::kSolutionFound;

solvers/scs_clarabel_common.cc

@@ -5,6 +5,58 @@
 namespace drake {
 namespace solvers {
 namespace internal {
+namespace {
+// Return the index of S(i, j) in the flattened vector consisting of upper
+// triangular part of matrix S (using column major).
+int IndexInUpperTrianglePart(int i, int j) {
+  return (1 + j) * j / 2 + i;
+}
+
+// SCS and Clarabel takes the upper (or lower) triangular part of a symmetric
+// matrix, scales the off-diagonal term by sqrt(2), and then returns the
+// flattened scaled triangular part as the dual. This function scales the
+// off-diagonal terms back. Also if upper_triangular_psd is true, we need to
+// re-order `dual` so that it stores the flattened lower triangular part (as
+// MathematicalProgramResult::GetDualSolution() returns the lower triangular
+// part).
+void ScalePsdConeDualVariable(int matrix_rows, bool upper_triangular_psd,
+                              EigenPtr<Eigen::VectorXd> dual) {
+  DRAKE_ASSERT(dual->rows() == matrix_rows * (matrix_rows + 1) / 2);
+  const double sqrt2 = std::sqrt(2);
+  int count = 0;
+  for (int j = 0; j < matrix_rows; ++j) {
+    for (int i = (upper_triangular_psd ? 0 : j);
+         i < (upper_triangular_psd ? j + 1 : matrix_rows); ++i) {
+      if (i != j) {
+        (*dual)(count) /= sqrt2;
+      }
+      ++count;
+    }
+  }
+  if (upper_triangular_psd) {
+    // dual is the flattend upper triangular part. We need to re-order it to
+    // store the flatted lower triangular part.
+    // TODO(hongkai.dai): investigate if we can do the in-place swap without
+    // creating a new vector dual_lower_triangular.
+    Eigen::VectorXd dual_lower_triangular(dual->rows());
+    int lower_triangular_count = 0;
+    for (int j = 0; j < matrix_rows; ++j) {
+      for (int i = j; i < matrix_rows; ++i) {
+        // For a symmetric matrix S (the dual psd matrix), S(i, j) in the lower
+        // triangular part (with i >= j) is the same as S(j, i) in the upper
+        // triangular part, hence we compute the index of S(j, i) in the upper
+        // triangular flat vector.
+        dual_lower_triangular(lower_triangular_count++) =
+            (*dual)(IndexInUpperTrianglePart(j, i));
+      }
+    }
+    // Copy dual_lower_triangular to dual
+    for (int i = 0; i < dual->rows(); ++i) {
+      (*dual)(i) = dual_lower_triangular(i);
+    }
+  }
+}
+}  // namespace
 void SetDualSolution(
     const MathematicalProgram& prog,
     const Eigen::Ref<const Eigen::VectorXd>& dual,
@@ -13,7 +65,9 @@ void SetDualSolution(
     const std::vector<int>& linear_eq_y_start_indices,
     const std::vector<int>& lorentz_cone_y_start_indices,
     const std::vector<int>& rotated_lorentz_cone_y_start_indices,
-    MathematicalProgramResult* result) {
+    const std::vector<std::optional<int>>& psd_y_start_indices,
+    const std::vector<std::optional<int>>& 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(
         prog.linear_constraints()[i].evaluator()->num_constraints());
@@ -93,6 +147,29 @@ void SetDualSolution(
     result->set_dual_solution(prog.rotated_lorentz_cone_constraints()[i],
                               rotated_lorentz_cone_dual);
   }
+  for (int i = 0; i < ssize(prog.positive_semidefinite_constraints()); ++i) {
+    const int matrix_rows =
+        prog.positive_semidefinite_constraints()[i].evaluator()->matrix_rows();
+    if (psd_y_start_indices[i].has_value()) {
+      Eigen::VectorXd psd_dual = dual.segment(
+          *(psd_y_start_indices[i]), matrix_rows * (matrix_rows + 1) / 2);
+      ScalePsdConeDualVariable(matrix_rows, upper_triangular_psd, &psd_dual);
+      result->set_dual_solution(prog.positive_semidefinite_constraints()[i],
+                                psd_dual);
+    }
+  }
+  for (int i = 0; i < ssize(prog.linear_matrix_inequality_constraints()); ++i) {
+    const int matrix_rows = prog.linear_matrix_inequality_constraints()[i]
+                                .evaluator()
+                                ->matrix_rows();
+    if (lmi_y_start_indices[i].has_value()) {
+      Eigen::VectorXd lmi_dual = dual.segment(
+          *(lmi_y_start_indices[i]), matrix_rows * (matrix_rows + 1) / 2);
+      ScalePsdConeDualVariable(matrix_rows, upper_triangular_psd, &lmi_dual);
+      result->set_dual_solution(prog.linear_matrix_inequality_constraints()[i],
+                                lmi_dual);
+    }
+  }
 }
 
 }  // namespace internal

solvers/scs_clarabel_common.h

@@ -2,6 +2,7 @@
 // some common functions for ClarabelSolver and ScsSolver
 #pragma once
 
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -39,6 +40,14 @@ namespace internal {
 // y[rotated_lorentz_cone_y_start_indices[i]:
 //   rotated_lorentz_cone_y_start_indices[i] + second_order_cone_length[i]]
 // are the dual variables for prog.rotated_lorentz_cone_constraints()[i].
+// @param psd_y_start_indices y[psd_y_start_indices[i]: psd_y_start_indices[i] +
+// (matrix_rows+1)*matrix_rows/2] are the dual variables for
+// prog.positive_semidefinite_constraints()[i]
+// @param lmi_y_start_indices y[lmi_y_start_indices[i]: lmi_y_start_indices[i] +
+// (matrix_rows+1)*matrix_rows/2] are the dual variables for
+// prog.linear_matrix_inequality_constraints()[i]
+// @param upper_triangular_psd Use upper-triangular (for Clarabel) or
+// lower-triangular (for SCS) part of the symmetric matrix in the dual variable.
 void SetDualSolution(
     const MathematicalProgram& prog,
     const Eigen::Ref<const Eigen::VectorXd>& dual,
@@ -47,7 +56,9 @@ void SetDualSolution(
     const std::vector<int>& linear_eq_y_start_indices,
     const std::vector<int>& lorentz_cone_y_start_indices,
     const std::vector<int>& rotated_lorentz_cone_y_start_indices,
-    MathematicalProgramResult* result);
+    const std::vector<std::optional<int>>& psd_y_start_indices,
+    const std::vector<std::optional<int>>& lmi_y_start_indices,
+    bool upper_triangular_psd, MathematicalProgramResult* result);
 }  // namespace internal
 }  // namespace solvers
 }  // namespace drake

solvers/scs_solver.cc

@@ -516,17 +516,42 @@ void ScsSolver::DoSolve2(const MathematicalProgram& prog,
   }
 
   // Parse PositiveSemidefiniteConstraint and LinearMatrixInequalityConstraint.
-  std::vector<int> psd_cone_length;
+  std::vector<std::optional<int>> psd_cone_length;
+  std::vector<std::optional<int>> lmi_cone_length;
+  std::vector<std::optional<int>> psd_y_start_indices;
+  std::vector<std::optional<int>> lmi_y_start_indices;
   internal::ParsePositiveSemidefiniteConstraints(
       prog, /* upper_triangular = */ false, &A_triplets, &b, &A_row_count,
-      &psd_cone_length);
+      &psd_cone_length, &lmi_cone_length, &psd_y_start_indices,
+      &lmi_y_start_indices);
   // Set the psd cone length in the SCS cone.
-  cone->ssize = psd_cone_length.size();
+  // Note that when psd_cone_length[i] = std::nullopt or lmi_cone_length[i] =
+  // std::nullopt, the constraint will not be parsed as PSD cone in SCS. So we
+  // should filter out these entries with value std::nullopt.
+  cone->ssize = 0;
+  for (const auto& length : psd_cone_length) {
+    if (length.has_value()) {
+      ++(cone->ssize);
+    }
+  }
+  for (const auto& length : lmi_cone_length) {
+    if (length.has_value()) {
+      cone->ssize++;
+    }
+  }
   // This scs_calloc doesn't need to accompany a ScopeExit since cone->s will be
   // cleaned up recursively by freeing up cone in scs_free_data()
   cone->s = static_cast<scs_int*>(scs_calloc(cone->ssize, sizeof(scs_int)));
-  for (int i = 0; i < cone->ssize; ++i) {
-    cone->s[i] = psd_cone_length[i];
+  int scs_psd_cone_count = 0;
+  for (int i = 0; i < ssize(psd_cone_length); ++i) {
+    if (psd_cone_length[i].has_value()) {
+      cone->s[scs_psd_cone_count++] = *(psd_cone_length[i]);
+    }
+  }
+  for (int i = 0; i < ssize(lmi_cone_length); ++i) {
+    if (lmi_cone_length[i].has_value()) {
+      cone->s[scs_psd_cone_count++] = *(lmi_cone_length[i]);
+    }
   }
 
   // Parse ExponentialConeConstraint.
@@ -581,7 +606,8 @@ void ScsSolver::DoSolve2(const MathematicalProgram& prog,
   internal::SetDualSolution(
       prog, solver_details.y, linear_constraint_dual_indices,
       linear_eq_y_start_indices, lorentz_cone_y_start_indices,
-      rotated_lorentz_cone_y_start_indices, result);
+      rotated_lorentz_cone_y_start_indices, psd_y_start_indices,
+      lmi_y_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 ||
       solver_details.scs_status == SCS_SOLVED_INACCURATE) {

solvers/test/aggregate_costs_constraints_test.cc

@@ -852,10 +852,19 @@ GTEST_TEST(ParsePositiveSemidefiniteConstraints, TestPsd) {
     int A_row_count_old = 2;
     std::vector<double> b(A_row_count_old);
     int A_row_count = A_row_count_old;
-    std::vector<int> psd_cone_length;
-    ParsePositiveSemidefiniteConstraints(prog, upper_triangular, &A_triplets,
-                                         &b, &A_row_count, &psd_cone_length);
-    EXPECT_EQ(psd_cone_length, std::vector<int>({3, 2}));
+    std::vector<std::optional<int>> psd_cone_length;
+    std::vector<std::optional<int>> lmi_cone_length;
+    std::vector<std::optional<int>> psd_y_start_indices;
+    std::vector<std::optional<int>> lmi_y_start_indices;
+    ParsePositiveSemidefiniteConstraints(
+        prog, upper_triangular, &A_triplets, &b, &A_row_count, &psd_cone_length,
+        &lmi_cone_length, &psd_y_start_indices, &lmi_y_start_indices);
+    EXPECT_EQ(psd_cone_length, std::vector<std::optional<int>>({{3}, {2}}));
+    EXPECT_TRUE(lmi_cone_length.empty());
+    EXPECT_EQ(psd_y_start_indices,
+              std::vector<std::optional<int>>(
+                  {{A_row_count_old}, {A_row_count_old + 3 * (3 + 1) / 2}}));
+    EXPECT_TRUE(lmi_y_start_indices.empty());
     // We add 3 * (3+1) / 2 = 6 rows in A for "X is psd", and 2 * (2+1) / 2 = 3
     // rows in A for "Y is psd".
     EXPECT_EQ(A_row_count, A_row_count_old + 3 * (3 + 1) / 2 + 2 * (2 + 1) / 2);
@@ -969,11 +978,19 @@ GTEST_TEST(ParsePositiveSemidefiniteConstraints, TestLmi) {
     const int A_row_count_old = 2;
     std::vector<double> b(A_row_count_old, 0);
     int A_row_count = A_row_count_old;
-    std::vector<int> psd_cone_length;
-    ParsePositiveSemidefiniteConstraints(prog, upper_triangular, &A_triplets,
-                                         &b, &A_row_count, &psd_cone_length);
+    std::vector<std::optional<int>> psd_cone_length;
+    std::vector<std::optional<int>> lmi_cone_length;
+    std::vector<std::optional<int>> psd_y_start_indices;
+    std::vector<std::optional<int>> lmi_y_start_indices;
+    ParsePositiveSemidefiniteConstraints(
+        prog, upper_triangular, &A_triplets, &b, &A_row_count, &psd_cone_length,
+        &lmi_cone_length, &psd_y_start_indices, &lmi_y_start_indices);
     EXPECT_EQ(A_row_count, A_row_count_old + 3 * (3 + 1) / 2);
-    EXPECT_EQ(psd_cone_length, std::vector<int>({3}));
+    EXPECT_TRUE(psd_cone_length.empty());
+    EXPECT_EQ(lmi_cone_length, std::vector<std::optional<int>>({{3}}));
+    EXPECT_TRUE(psd_y_start_indices.empty());
+    EXPECT_EQ(lmi_y_start_indices,
+              std::vector<std::optional<int>>({{A_row_count_old}}));
     Eigen::SparseMatrix<double> A(A_row_count_old + 6, prog.num_vars());
     A.setFromTriplets(A_triplets.begin(), A_triplets.end());
     Eigen::MatrixXd A_expected(A_row_count_old + 6, prog.num_vars());