diff --git a/math/quadratic_form.cc b/math/quadratic_form.cc index b36ace5195f4..f481ce081aeb 100644 --- a/math/quadratic_form.cc +++ b/math/quadratic_form.cc @@ -6,13 +6,15 @@ #include #include +#include "drake/common/drake_throw.h" #include "drake/math/matrix_util.h" namespace drake { namespace math { -Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX( +std::optional MaybeDecomposePSDmatrixIntoXtransposeTimesX( const Eigen::Ref& Y, double zero_tol, - bool return_empty_if_not_psd) { + Eigen::MatrixXd* X) { + DRAKE_THROW_UNLESS(X != nullptr); if (Y.rows() != Y.cols()) { throw std::runtime_error("Y is not square."); } @@ -21,36 +23,46 @@ Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX( } Eigen::LLT llt_Y(Y); if (llt_Y.info() == Eigen::Success) { - return llt_Y.matrixU(); - } else { - // TODO(hongkai.dai) Switch to use robust Choleskly decomposition instead - // of Eigen value decomposition, when the bug in - // http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1479 is fixed. - Eigen::SelfAdjointEigenSolver es_Y(Y); - if (es_Y.info() == Eigen::Success) { - Eigen::MatrixXd X(Y.rows(), Y.cols()); - int X_row_count = 0; - for (int i = 0; i < es_Y.eigenvalues().rows(); ++i) { - if (es_Y.eigenvalues()(i) < -zero_tol) { - if (return_empty_if_not_psd) { - return Eigen::MatrixXd::Zero(0, Y.cols()); - } - throw std::runtime_error(fmt::format( - "Y is not positive semidefinite. It has an eigenvalue {} " - "that is less than the tolerance {}.", - es_Y.eigenvalues()(i), -zero_tol)); - } else if (es_Y.eigenvalues()(i) > zero_tol) { - X.row(X_row_count++) = std::sqrt(es_Y.eigenvalues()(i)) * - es_Y.eigenvectors().col(i).transpose(); - } + *X = llt_Y.matrixU(); + return std::nullopt; + } + // TODO(hongkai.dai) Switch to use robust Choleskly decomposition instead + // of Eigen value decomposition, when the bug in + // http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1479 is fixed. + Eigen::SelfAdjointEigenSolver es_Y(Y); + if (es_Y.info() == Eigen::Success) { + Eigen::MatrixXd X_full(Y.rows(), Y.cols()); + int X_row_count = 0; + for (int i = 0; i < es_Y.eigenvalues().rows(); ++i) { + if (es_Y.eigenvalues()(i) < -zero_tol) { + return fmt::format( + "Y is not positive semidefinite. It has an eigenvalue {} " + "that is less than the tolerance {}.", + es_Y.eigenvalues()(i), -zero_tol); + } else if (es_Y.eigenvalues()(i) > zero_tol) { + X_full.row(X_row_count++) = std::sqrt(es_Y.eigenvalues()(i)) * + es_Y.eigenvectors().col(i).transpose(); } - return X.topRows(X_row_count); } + *X = X_full.topRows(X_row_count); + return std::nullopt; } - if (return_empty_if_not_psd) { - return Eigen::MatrixXd::Zero(0, Y.cols()); + return std::string{"Y is not PSD."}; +} + +Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX( + const Eigen::Ref& Y, double zero_tol, + bool return_empty_if_not_psd) { + Eigen::MatrixXd X; + const std::optional error = + MaybeDecomposePSDmatrixIntoXtransposeTimesX(Y, zero_tol, &X); + if (error.has_value()) { + if (return_empty_if_not_psd) { + return Eigen::MatrixXd::Zero(0, Y.cols()); + } + throw std::runtime_error(*error); } - throw std::runtime_error("Y is not PSD."); + return X; } std::pair DecomposePositiveQuadraticForm( diff --git a/math/quadratic_form.h b/math/quadratic_form.h index 3346d7d47b0c..9533d4626a72 100644 --- a/math/quadratic_form.h +++ b/math/quadratic_form.h @@ -1,11 +1,39 @@ #pragma once +#include +#include #include #include namespace drake { namespace math { +/** + * For a symmetric positive semidefinite matrix Y, decompose it into XᵀX, where + * the number of rows in X equals to the rank of Y. + * Notice that this decomposition is not unique. For any orthonormal matrix U, + * s.t UᵀU = identity, X_prime = UX also satisfies X_primeᵀX_prime = Y. Here + * we only return one valid decomposition. + * @param Y A symmetric positive semidefinite matrix. + * @param zero_tol We will need to check if some value (for example, the + * absolute value of Y's eigenvalues) is smaller than zero_tol. If it is, then + * we deem that value as 0. + * @param[out] X On success, the matrix X that satisfies XᵀX = Y and + * X.rows() = rank(Y). On failure, the contents are unspecified. + * @return std::nullopt on success. Otherwise, returns a human-readable error + * string describing why the decomposition failed (typically because Y is not + * positive semidefinite). + * @pre 1. zero_tol is non-negative. + * 2. X != nullptr. + * @throws std::exception when the pre-conditions are not satisfied, or when Y + * is not square. + * @note We only use the lower triangular part of Y. + * @see DecomposePSDmatrixIntoXtransposeTimesX + */ +std::optional MaybeDecomposePSDmatrixIntoXtransposeTimesX( + const Eigen::Ref& Y, double zero_tol, + Eigen::MatrixXd* X); + /** * For a symmetric positive semidefinite matrix Y, decompose it into XᵀX, where * the number of rows in X equals to the rank of Y. @@ -27,6 +55,7 @@ namespace math { * 2. zero_tol is non-negative. * @throws std::exception when the pre-conditions are not satisfied. * @note We only use the lower triangular part of Y. + * @see MaybeDecomposePSDmatrixIntoXtransposeTimesX */ Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX( const Eigen::Ref& Y, double zero_tol, diff --git a/math/test/quadratic_form_test.cc b/math/test/quadratic_form_test.cc index 101db7963b3e..bcb3f2038b1c 100644 --- a/math/test/quadratic_form_test.cc +++ b/math/test/quadratic_form_test.cc @@ -1,5 +1,9 @@ #include "drake/math/quadratic_form.h" +#include +#include + +#include #include #include "drake/common/test_utilities/eigen_matrix_compare.h" @@ -27,6 +31,12 @@ void CheckDecomposePSDmatrixIntoXtransposeTimesX( GTEST_TEST(TestDecomposePSDmatrixIntoXtransposeTimesX, Test0) { CheckDecomposePSDmatrixIntoXtransposeTimesX(Eigen::Matrix3d::Identity(), kDefaultZeroTol); + Eigen::MatrixXd X; + EXPECT_FALSE(MaybeDecomposePSDmatrixIntoXtransposeTimesX( + Eigen::Matrix3d::Identity(), kDefaultZeroTol, &X) + .has_value()); + EXPECT_TRUE(CompareMatrices(X.transpose() * X, Eigen::Matrix3d::Identity(), + 1E-14, MatrixCompareType::absolute)); } GTEST_TEST(TestDecomposePSDmatrixIntoXtransposeTimesX, Test1) { @@ -87,6 +97,14 @@ GTEST_TEST(TestDecomposePSDmatrixIntoXtransposeTimesX, negativeY) { "Y is not positive semidefinite. It has an eigenvalue -1.* that is less" " than the tolerance -0.*."); + // Maybe... returns the same error details without throwing. + Eigen::MatrixXd X_maybe; + const std::optional error = + MaybeDecomposePSDmatrixIntoXtransposeTimesX(-Eigen::Matrix3d::Identity(), + 0, &X_maybe); + ASSERT_TRUE(error.has_value()); + EXPECT_THAT(*error, testing::HasSubstr("Y is not positive semidefinite")); + // If return_empty_if_not_psd is true, the function should return an empty // matrix. Eigen::MatrixXd X = DecomposePSDmatrixIntoXtransposeTimesX( diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 735a20f0eeeb..1a4071153c0e 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -1885,6 +1885,7 @@ drake_cc_googletest( ":semidefinite_program_examples", ":sos_examples", "//common:temp_directory", + "//common/test_utilities:expect_throws_message", ], ) diff --git a/solvers/create_constraint.cc b/solvers/create_constraint.cc index 875a42778212..5054196bc8c7 100644 --- a/solvers/create_constraint.cc +++ b/solvers/create_constraint.cc @@ -2,7 +2,9 @@ #include #include +#include #include +#include #include @@ -796,8 +798,16 @@ ParseQuadraticAsRotatedLorentzConeConstraint( const Eigen::Ref& Q, const Eigen::Ref& b, double c, double zero_tol) { // [-bᵀx-c, 1, Fx] is in the rotated Lorentz cone, where FᵀF = 0.5 * Q - const Eigen::MatrixXd F = math::DecomposePSDmatrixIntoXtransposeTimesX( - (Q + Q.transpose()) / 4, zero_tol); + Eigen::MatrixXd F; + const std::optional psd_error = + math::MaybeDecomposePSDmatrixIntoXtransposeTimesX( + (Q + Q.transpose()) / 4, zero_tol, &F); + if (psd_error.has_value()) { + throw std::runtime_error(fmt::format( + "ParseQuadraticAsRotatedLorentzConeConstraint: The matrix Q is not " + "positive semidefinite. {}", + *psd_error)); + } if (F.rows() == 0) { throw std::runtime_error( "AddQuadraticAsRotatedLorentzConeConstraint: The quadratic terms is " diff --git a/solvers/mosek_solver_internal.cc b/solvers/mosek_solver_internal.cc index 5c1d77cad943..72062eb3b541 100644 --- a/solvers/mosek_solver_internal.cc +++ b/solvers/mosek_solver_internal.cc @@ -5,7 +5,10 @@ #include #include #include +#include +#include +#include #include #include "drake/common/never_destroyed.h" @@ -1142,8 +1145,37 @@ MSKrescodee MosekSolverProgram::AddQuadraticCostAsLinearCost( // license, which is incompatible with Drake's license, so we convert the // sparse Q_lower matrix to a dense matrix, and use the dense Cholesky // decomposition instead. - const Eigen::MatrixXd L = math::DecomposePSDmatrixIntoXtransposeTimesX( - Eigen::MatrixXd(Q_lower), std::numeric_limits::epsilon()); + Eigen::MatrixXd L; + const std::optional psd_error = + math::MaybeDecomposePSDmatrixIntoXtransposeTimesX( + Eigen::MatrixXd(Q_lower), std::numeric_limits::epsilon(), + &L); + if (psd_error.has_value()) { + // Identify which quadratic costs are not PSD so the message names them. + std::string non_psd_costs; + for (const auto& binding : prog.quadratic_costs()) { + Eigen::MatrixXd unused; + if (math::MaybeDecomposePSDmatrixIntoXtransposeTimesX( + binding.evaluator()->Q(), + std::numeric_limits::epsilon(), &unused) + .has_value()) { + non_psd_costs += fmt::format("\n{}", binding.to_string()); + } + } + if (non_psd_costs.empty()) { + throw std::runtime_error(fmt::format( + "Failed to convert quadratic costs to a second-order cone " + "constraint because the aggregated Hessian is not positive " + "semidefinite. {}", + *psd_error)); + } + throw std::runtime_error(fmt::format( + "Failed to convert quadratic costs to a second-order cone " + "constraint because a quadratic cost Hessian is not positive " + "semidefinite. {} The following quadratic cost(s) may not be " + "positive semidefinite:{}", + *psd_error, non_psd_costs)); + } MSKint32t num_mosek_vars = 0; rescode = MSK_getnumvar(task_, &num_mosek_vars); if (rescode != MSK_RES_OK) { diff --git a/solvers/scs_solver.cc b/solvers/scs_solver.cc index 3314da6e5809..4e1dc1631754 100644 --- a/solvers/scs_solver.cc +++ b/solvers/scs_solver.cc @@ -87,8 +87,15 @@ void ParseQuadraticCostWithRotatedLorentzCone( } // Decompose Q to Cᵀ*C const Eigen::MatrixXd& Q = cost.evaluator()->Q(); - const Eigen::MatrixXd C = - math::DecomposePSDmatrixIntoXtransposeTimesX(Q, 1E-10); + Eigen::MatrixXd C; + const std::optional psd_error = + math::MaybeDecomposePSDmatrixIntoXtransposeTimesX(Q, 1E-10, &C); + if (psd_error.has_value()) { + throw std::runtime_error(fmt::format( + "The Hessian in the quadratic cost {} is not positive " + "semidefinite. {}", + cost.to_string(), *psd_error)); + } for (int i = 0; i < C.rows(); ++i) { for (int j = 0; j < C.cols(); ++j) { if (C(i, j) != 0) { diff --git a/solvers/test/scs_solver_test.cc b/solvers/test/scs_solver_test.cc index 33f94a34e479..de572a1a6bdc 100644 --- a/solvers/test/scs_solver_test.cc +++ b/solvers/test/scs_solver_test.cc @@ -9,6 +9,7 @@ #include "drake/common/temp_directory.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" #include "drake/solvers/mathematical_program.h" #include "drake/solvers/test/exponential_cone_program_examples.h" #include "drake/solvers/test/l2norm_cost_examples.h" @@ -639,6 +640,31 @@ GTEST_TEST(TestOptions, StandaloneReproduction) { EXPECT_THAT(repro_str, HasSubstr("solve")); } } + +// Regression test for #20892: when rewriting a non-PSD quadratic cost for SCS, +// the error message should name the quadratic cost. +GTEST_TEST(TestQuadraticCostPsdError, NonPsdHessianNamesCost) { + MathematicalProgram prog; + const auto x = prog.NewContinuousVariables<2>("x"); + Eigen::Matrix2d Q; + // clang-format off + Q << 1, 0, + 0, -1; + // clang-format on + auto cost = + prog.AddQuadraticCost(Q, Eigen::Vector2d::Zero(), x, true /* is_convex */); + cost.evaluator()->set_description("nonpsd_quad_cost"); + + ScsSolver solver; + if (!solver.available()) { + return; + } + // Unconstrained QP is rewritten via rotated Lorentz cones, which requires + // decomposing Q. + DRAKE_EXPECT_THROWS_MESSAGE( + solver.Solve(prog, {}, {}), + ".*quadratic cost.*nonpsd_quad_cost.*not positive semidefinite.*"); +} } // namespace test } // namespace solvers } // namespace drake