diff --git a/solvers/aggregate_costs_constraints.cc b/solvers/aggregate_costs_constraints.cc index c2978f5147ef..4dd695205839 100644 --- a/solvers/aggregate_costs_constraints.cc +++ b/solvers/aggregate_costs_constraints.cc @@ -687,12 +687,12 @@ void ParsePositiveSemidefiniteConstraints( std::vector>* 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>* A_triplets, std::vector* b, + int* A_row_count, int* num_new_second_order_cones, + std::vector>* twobytwo_psd_dual_start_indices, + std::vector>* 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 diff --git a/solvers/aggregate_costs_constraints.h b/solvers/aggregate_costs_constraints.h index 9f3dcbcc4444..b1155da4b4ec 100644 --- a/solvers/aggregate_costs_constraints.h +++ b/solvers/aggregate_costs_constraints.h @@ -385,6 +385,29 @@ void ParseScalarPositiveSemidefiniteConstraints( int* A_row_count, int* new_positive_cone_length, std::vector>* scalar_psd_dual_indices, std::vector>* 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>* A_triplets, std::vector* b, + int* A_row_count, int* num_new_second_order_cones, + std::vector>* twobytwo_psd_dual_start_indices, + std::vector>* twobytwo_lmi_dual_start_indices); } // namespace internal } // namespace solvers } // namespace drake diff --git a/solvers/clarabel_solver.cc b/solvers/clarabel_solver.cc index 330372f74df5..633ea1edf757 100644 --- a/solvers/clarabel_solver.cc +++ b/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(soc_length)); } + // Parse PSD/LMI constraints on 2x2 matrices as second order cone constraints. + int num_second_order_cones_from_psd{}; + std::vector> twobytwo_psd_y_start_indices; + std::vector> 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(3)); + } std::vector> psd_cone_length; std::vector> 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) { diff --git a/solvers/scs_clarabel_common.cc b/solvers/scs_clarabel_common.cc index efc5218a7db9..1579f40fa50e 100644 --- a/solvers/scs_clarabel_common.cc +++ b/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>& lmi_y_start_indices, const std::vector>& scalar_psd_y_indices, const std::vector>& scalar_lmi_y_indices, + const std::vector>& twobytwo_psd_y_start_indices, + const std::vector>& 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()))); } } } diff --git a/solvers/scs_clarabel_common.h b/solvers/scs_clarabel_common.h index d40cfb363333..d5b94c9f2fac 100644 --- a/solvers/scs_clarabel_common.h +++ b/solvers/scs_clarabel_common.h @@ -60,6 +60,8 @@ void SetDualSolution( const std::vector>& lmi_y_start_indices, const std::vector>& scalar_psd_y_indices, const std::vector>& scalar_lmi_y_indices, + const std::vector>& twobytwo_psd_y_start_indices, + const std::vector>& twobytwo_lmi_y_start_indices, bool upper_triangular_psd, MathematicalProgramResult* result); } // namespace internal } // namespace solvers diff --git a/solvers/scs_solver.cc b/solvers/scs_solver.cc index 8643711e8c74..534a368d3a4f 100644 --- a/solvers/scs_solver.cc +++ b/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> twobytwo_psd_dual_start_indices; + std::vector> 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 || diff --git a/solvers/test/aggregate_costs_constraints_test.cc b/solvers/test/aggregate_costs_constraints_test.cc index 26c1109f576e..275f453291c5 100644 --- a/solvers/test/aggregate_costs_constraints_test.cc +++ b/solvers/test/aggregate_costs_constraints_test.cc @@ -859,19 +859,20 @@ GTEST_TEST(ParsePositiveSemidefiniteConstraints, TestPsd) { 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>({{3}, {2}})); + EXPECT_EQ(psd_cone_length, + std::vector>({{3}, {std::nullopt}})); EXPECT_TRUE(lmi_cone_length.empty()); - EXPECT_EQ(psd_y_start_indices, - std::vector>( - {{A_row_count_old}, {A_row_count_old + 3 * (3 + 1) / 2}})); + EXPECT_EQ(psd_y_start_indices, std::vector>( + {{A_row_count_old}, {std::nullopt}})); 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); + // We add 3 * (3+1) / 2 = 6 rows in A for "X is psd", and 0 + // rows in A for "Y is psd" since Y is a 2x2 matrix, which should be imposed + // as a second order cone constraint. + EXPECT_EQ(A_row_count, A_row_count_old + 3 * (3 + 1) / 2); const double sqrt2 = std::sqrt(2); Eigen::SparseMatrix A(A_row_count, prog.num_vars()); A.setFromTriplets(A_triplets.begin(), A_triplets.end()); - Eigen::MatrixXd A_expected(A_row_count_old + 9, 9); + Eigen::MatrixXd A_expected(A_row_count_old + 6, prog.num_vars()); A_expected.setZero(); // [1 √2 √2] // [√2 1 √2] @@ -900,16 +901,6 @@ GTEST_TEST(ParsePositiveSemidefiniteConstraints, TestPsd) { A_expected(A_row_count_old + 3, 2 /* X(0, 2) */) = -sqrt2; A_expected(A_row_count_old + 4, 4 /* X(1, 2) */) = -sqrt2; A_expected(A_row_count_old + 5, 5 /* X(2, 2) */) = -1; - // For the 2 by 2 matrix Y to be psd, we need the constraint - // -Y(0, 0) + s(6) = 0 - // -√2Y(0, 1) + s(7) = 0 - // -Y(1, 1) + s(8) = 0 - // and - // [s(6) s(7)] is psd - // [s(7) s(8)] - A_expected(A_row_count_old + 6, 6) = -1; - A_expected(A_row_count_old + 7, 7) = -sqrt2; - A_expected(A_row_count_old + 8, 8) = -1; } else { // If we use the lower triangular part of the symmetric matrix, to impose // the constraint that 3 by 3 matrix X is psd, we need the constraint @@ -931,16 +922,6 @@ GTEST_TEST(ParsePositiveSemidefiniteConstraints, TestPsd) { A_expected(A_row_count_old + 3, 3) = -1; A_expected(A_row_count_old + 4, 4) = -sqrt2; A_expected(A_row_count_old + 5, 5) = -1; - // For the 2 by 2 matrix Y to be psd, we need the constraint - // -Y(0, 0) + s(6) = 0 - // -√2Y(0, 1) + s(7) = 0 - // -Y(1, 1) + s(8) = 0 - // and - // [s(6) s(7)] is psd - // [s(7) s(8)] - A_expected(A_row_count_old + 6, 6) = -1; - A_expected(A_row_count_old + 7, 7) = -sqrt2; - A_expected(A_row_count_old + 8, 8) = -1; } EXPECT_TRUE(CompareMatrices(A.toDense(), A_expected)); EXPECT_EQ(b.size(), A_row_count); @@ -1093,6 +1074,89 @@ GTEST_TEST(ParseScalarPositiveSemidefiniteConstraints, Test) { EXPECT_FALSE(psd_y_start_indices[0].has_value()); EXPECT_FALSE(lmi_y_start_indices[1].has_value()); } + +GTEST_TEST(Parse2x2PositiveSemidefiniteConstraints, TestPSD) { + // A PositiveSemidefiniteConstraint on a 2 x 2 matrix. + MathematicalProgram prog; + auto x = prog.NewContinuousVariables<2>(); + // X has repeated entries. + Matrix2 X; + // clang-format off + X << x(0), x(1), + x(1), x(0); + // clang-format on + auto psd_con = prog.AddPositiveSemidefiniteConstraint(X); + int A_row_count = 0; + std::vector> A_triplets{}; + std::vector b{}; + int num_new_second_order_cones{0}; + std::vector> twobytwo_psd_dual_start_indices; + std::vector> twobytwo_lmi_dual_start_indices; + Parse2x2PositiveSemidefiniteConstraints( + prog, &A_triplets, &b, &A_row_count, &num_new_second_order_cones, + &twobytwo_psd_dual_start_indices, &twobytwo_lmi_dual_start_indices); + EXPECT_EQ(A_row_count, 3); + EXPECT_EQ(num_new_second_order_cones, 1); + EXPECT_EQ(b, std::vector({0, 0, 0})); + Eigen::SparseMatrix A(A_row_count, prog.num_vars()); + A.setFromTriplets(A_triplets.begin(), A_triplets.end()); + Eigen::Matrix A_expected; + // clang-format off + A_expected << -2, 0, + 0, 0, + 0, -2; + // clang-format on + EXPECT_TRUE(CompareMatrices(A.toDense(), A_expected)); + EXPECT_EQ(twobytwo_psd_dual_start_indices, + std::vector>({{0}})); + EXPECT_TRUE(twobytwo_lmi_dual_start_indices.empty()); +} + +GTEST_TEST(Parse2x2PositiveSemidefiniteConstraints, TestLMI) { + // A LinearMatrixInequalityConstraint on a 2x2 matrix. + MathematicalProgram prog; + auto x = prog.NewContinuousVariables<2>(); + std::vector F; + F.push_back((Eigen::Matrix2d() << 1, 2, 2, 4).finished()); + F.push_back((Eigen::Matrix2d() << 0, 1, 1, -2).finished()); + F.push_back((Eigen::Matrix2d() << 1, 0, 0, -1).finished()); + F.push_back((Eigen::Matrix2d() << 2, 1, 1, 1).finished()); + auto lmi_con = prog.AddLinearMatrixInequalityConstraint( + F, Vector3(x(0), x(1), x(0))); + std::vector> A_triplets; + std::vector b; + int A_row_count = 0; + int num_new_second_order_cones{}; + std::vector> twobytwo_psd_dual_start_indices; + std::vector> twobytwo_lmi_dual_start_indices; + Parse2x2PositiveSemidefiniteConstraints( + prog, &A_triplets, &b, &A_row_count, &num_new_second_order_cones, + &twobytwo_psd_dual_start_indices, &twobytwo_lmi_dual_start_indices); + EXPECT_EQ(A_row_count, 3); + EXPECT_EQ(num_new_second_order_cones, 1); + Eigen::SparseMatrix A(3, prog.num_vars()); + A.setFromTriplets(A_triplets.begin(), A_triplets.end()); + // Since A*x + s = b, we have s = -A*x+b + VectorX s = + -A.toDense() * prog.decision_variables() + + Eigen::Map(b.data(), b.size()); + // Now evaluate F[0] + ∑ᵢF[1+i] * x(i) + Matrix2 lmi_expected = F[0]; + for (int i = 0; i < lmi_con.variables().rows(); ++i) { + lmi_expected += lmi_con.evaluator()->F()[1 + i] * lmi_con.variables()(i); + } + EXPECT_PRED2(symbolic::test::ExprEqual, + (lmi_expected(0, 0) + lmi_expected(1, 1)).Expand(), + s(0).Expand()); + EXPECT_PRED2(symbolic::test::ExprEqual, + (lmi_expected(0, 0) - lmi_expected(1, 1)).Expand(), + s(1).Expand()); + EXPECT_PRED2(symbolic::test::ExprEqual, (2 * lmi_expected(0, 1)).Expand(), + s(2).Expand()); + EXPECT_TRUE(twobytwo_psd_dual_start_indices.empty()); + EXPECT_EQ(twobytwo_lmi_dual_start_indices, + std::vector>({{0}})); +} } // namespace internal } // namespace solvers } // namespace drake diff --git a/solvers/test/clarabel_solver_test.cc b/solvers/test/clarabel_solver_test.cc index 98c7676c5d44..e014f60e3d6b 100644 --- a/solvers/test/clarabel_solver_test.cc +++ b/solvers/test/clarabel_solver_test.cc @@ -576,7 +576,7 @@ GTEST_TEST(TestOptions, StandaloneReproduction) { prog.AddLorentzConeConstraint(Vector2(x(0), x(1))); prog.AddExponentialConeConstraint( Vector3(x(2), x(0), x(1))); - const auto Y = prog.NewSymmetricContinuousVariables<2>("Y"); + const auto Y = prog.NewSymmetricContinuousVariables<3>("Y"); prog.AddPositiveSemidefiniteConstraint(Y); ClarabelSolver solver; @@ -612,7 +612,7 @@ GTEST_TEST(TestOptions, EmptyCones) { prog.AddLorentzConeConstraint(Vector2(x(0), x(1))); prog.AddExponentialConeConstraint( Vector3(x(2), x(0), x(1))); - const auto Y = prog.NewSymmetricContinuousVariables<2>("Y"); + const auto Y = prog.NewSymmetricContinuousVariables<3>("Y"); prog.AddPositiveSemidefiniteConstraint(Y); ClarabelSolver solver; diff --git a/solvers/test/scs_solver_test.cc b/solvers/test/scs_solver_test.cc index 67e67b79ea92..2eea03f4da3b 100644 --- a/solvers/test/scs_solver_test.cc +++ b/solvers/test/scs_solver_test.cc @@ -464,7 +464,7 @@ GTEST_TEST(TestSemidefiniteProgram, TestTrivial1x1LMI) { GTEST_TEST(TestSemidefiniteProgram, Test2X2LMI) { ScsSolver solver; if (solver.available()) { - Test2x2LMI(solver, 1E-7, /*check_dual=*/true, /*dual_tol=*/1E-7); + Test2x2LMI(solver, 1E-5, /*check_dual=*/true, /*dual_tol=*/1E-5); } }