[solvers] Name quadratic cost when PSD decomposition fails

math/quadratic_form.cc

@@ -6,13 +6,15 @@
 #include <Eigen/Eigenvalues>
 #include <fmt/format.h>
 
+#include "drake/common/drake_throw.h"
 #include "drake/math/matrix_util.h"
 
 namespace drake {
 namespace math {
-Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX(
+std::optional<std::string> MaybeDecomposePSDmatrixIntoXtransposeTimesX(
     const Eigen::Ref<const Eigen::MatrixXd>& 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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<const Eigen::MatrixXd>& Y, double zero_tol,
+    bool return_empty_if_not_psd) {
+  Eigen::MatrixXd X;
+  const std::optional<std::string> 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<Eigen::MatrixXd, Eigen::MatrixXd> DecomposePositiveQuadraticForm(

math/quadratic_form.h

@@ -1,11 +1,39 @@
 #pragma once
 
+#include <optional>
+#include <string>
 #include <utility>
 
 #include <Eigen/Core>
 
 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<std::string> MaybeDecomposePSDmatrixIntoXtransposeTimesX(
+    const Eigen::Ref<const Eigen::MatrixXd>& 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<const Eigen::MatrixXd>& Y, double zero_tol,

math/test/quadratic_form_test.cc

@@ -1,5 +1,9 @@
 #include "drake/math/quadratic_form.h"
 
+#include <optional>
+#include <string>
+
+#include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
 #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<std::string> 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(

solvers/BUILD.bazel

@@ -1885,6 +1885,7 @@ drake_cc_googletest(
         ":semidefinite_program_examples",
         ":sos_examples",
         "//common:temp_directory",
+        "//common/test_utilities:expect_throws_message",
     ],
 )
 

solvers/create_constraint.cc

@@ -2,7 +2,9 @@
 
 #include <algorithm>
 #include <cmath>
+#include <optional>
 #include <sstream>
+#include <string>
 
 #include <fmt/format.h>
 
@@ -796,8 +798,16 @@ ParseQuadraticAsRotatedLorentzConeConstraint(
     const Eigen::Ref<const Eigen::MatrixXd>& Q,
     const Eigen::Ref<const Eigen::VectorXd>& 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<std::string> 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 "

solvers/mosek_solver_internal.cc

@@ -5,7 +5,10 @@
 #include <atomic>
 #include <cstdio>
 #include <limits>
+#include <optional>
+#include <string>
 
+#include <fmt/format.h>
 #include <fmt/ostream.h>
 
 #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<double>::epsilon());
+  Eigen::MatrixXd L;
+  const std::optional<std::string> psd_error =
+      math::MaybeDecomposePSDmatrixIntoXtransposeTimesX(
+          Eigen::MatrixXd(Q_lower), std::numeric_limits<double>::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<double>::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) {

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<std::string> 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) {

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