Add scaled diagonally dominant matrix constraint. (RobotLocomotion#9472)

Add scaled diagonally dominant matrix constraint.

solvers/BUILD.bazel

@@ -1354,6 +1354,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "scaled_diagonally_dominant_matrix_test",
+    tags = gurobi_test_tags() + mosek_test_tags(),
+    deps = [
+        ":mathematical_program",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:symbolic_test_util",
+    ],
+)
+
 # The extra_srcs are required here because add_lint_tests() doesn't understand
 # how to extract labels from select() functions yet.
 add_lint_tests(cpplint_extra_srcs = [

solvers/mathematical_program.cc

@@ -796,6 +796,40 @@ MathematicalProgram::AddPositiveDiagonallyDominantMatrixConstraint(
   return Y;
 }
 
+std::vector<std::vector<Matrix2<symbolic::Expression>>>
+MathematicalProgram::AddScaledDiagonallyDominantMatrixConstraint(
+    const Eigen::Ref<const MatrixX<symbolic::Expression>>& X) {
+  const int n = X.rows();
+  DRAKE_DEMAND(X.cols() == n);
+  std::vector<std::vector<Matrix2<symbolic::Expression>>> M(n);
+  for (int i = 0; i < n; ++i) {
+    M[i].resize(n);
+    for (int j = i + 1; j < n; ++j) {
+      // Since M[i][j](0, 1) = X(i, j), we only need to declare new variables
+      // for the diagonal entries of M[i][j].
+      auto M_ij_diagonal = NewContinuousVariables<2>(
+          "sdd_slack_M[" + std::to_string(i) + "][" + std::to_string(j) + "]");
+      M[i][j](0, 0) = M_ij_diagonal(0);
+      M[i][j](1, 1) = M_ij_diagonal(1);
+      M[i][j](0, 1) = (X(i, j) + X(j, i)) / 2;
+      M[i][j](1, 0) = M[i][j](0, 1);
+      AddRotatedLorentzConeConstraint(Vector3<symbolic::Expression>(
+          M[i][j](0, 0), M[i][j](1, 1), M[i][j](0, 1)));
+    }
+  }
+  for (int i = 0; i < n; ++i) {
+    symbolic::Expression diagonal_sum = 0;
+    for (int j = 0; j < i; ++j) {
+      diagonal_sum += M[j][i](1, 1);
+    }
+    for (int j = i + 1; j < n; ++j) {
+      diagonal_sum += M[i][j](0, 0);
+    }
+    AddLinearEqualityConstraint(X(i, i) - diagonal_sum, 0);
+  }
+  return M;
+}
+
 // Note that FindDecisionVariableIndex is implemented in
 // mathematical_program_api.cc instead of this file.
 

solvers/mathematical_program.h

@@ -2052,16 +2052,50 @@ class MathematicalProgram {
    * Internally we will create a matrix Y as slack variables, such that Y(i, j)
    * represents the absolute value |X(i, j)| ∀ j ≠ i. The diagonal entries
    * Y(i, i) = X(i, i)
-   * @param X The symmetric matrix X in the documentation above. We
-   * will assume that @p X is already symmetric. It is the user's responsibility
-   * to guarantee the symmetry.
+   * The users can refer to "DSOS and SDSOS Optimization: More Tractable
+   * Alternatives to Sum of Squares and Semidefinite Optimization" by Amir Ali
+   * Ahmadi and Anirudha Majumdar, with arXiv link
+   * https://arxiv.org/abs/1706.02586
+   * @param X The matrix X. We will use 0.5(X+Xᵀ) as the "symmetric version" of
+   * X.
    * @return Y The slack variable. Y(i, j) represents |X(i, j)| ∀ j ≠ i, with
    * the constraint Y(i, j) >= X(i, j) and Y(i, j) >= -X(i, j). Y is a symmetric
    * matrix. The diagonal entries Y(i, i) = X(i, i)
    */
   MatrixX<symbolic::Expression> AddPositiveDiagonallyDominantMatrixConstraint(
       const Eigen::Ref<const MatrixX<symbolic::Expression>>& X);
 
+  /**
+   * Adds the constraint that a symmetric matrix is scaled diagonally dominant
+   * (sdd). A matrix X is sdd if there exists a diagonal matrix D, such that
+   * the product DXD is diagonally dominant with non-negative diagonal entries,
+   * namely
+   * d(i)X(i, i) ≥ ∑ⱼ |d(j)X(i, j)| ∀ j ≠ i
+   * where d(i) = D(i, i).
+   * X being sdd is equivalent to the existence of symmetric matrices Mⁱʲ∈ ℝⁿˣⁿ
+   * i < j, such that all entries in Mⁱʲ are 0, except Mⁱʲ(i, i), Mⁱʲ(i, j),
+   * Mⁱʲ(j, j). (Mⁱʲ(i, i), Mⁱʲ(j, j), Mⁱʲ(i, j)) is in the rotated
+   * Lorentz cone, and X = ∑ᵢⱼ Mⁱʲ
+   * The users can refer to "DSOS and SDSOS Optimization: More Tractable
+   * Alternatives to Sum of Squares and Semidefinite Optimization" by Amir Ali
+   * Ahmadi and Anirudha Majumdar, with arXiv link
+   * https://arxiv.org/abs/1706.02586.
+   * @param X The matrix X. We will use 0.5(X+Xᵀ) as the "symmetric version" of
+   * X.
+   * @pre X(i, j) should be a linear expression of decision variables.
+   * @return M A vector of vectors of 2 x 2 symmetric matrices M. For i < j, 
+   * M[i][j] is 
+   * <pre>
+   * [Mⁱʲ(i, i), Mⁱʲ(i, j)]
+   * [Mⁱʲ(i, j), Mⁱʲ(j, j)].
+   * </pre>
+   * Note that M[i][j](0, 1) = Mⁱʲ(i, j) = (X(i, j) + X(j, i)) / 2
+   * for i >= j, M[i][j] is the zero matrix.
+   */
+  std::vector<std::vector<Matrix2<symbolic::Expression>>>
+  AddScaledDiagonallyDominantMatrixConstraint(
+      const Eigen::Ref<const MatrixX<symbolic::Expression>>& X);
+
   /**
    * Adds constraints that a given polynomial @p p is a sums-of-squares (SOS),
    * that is, @p p can be decomposed into `mᵀQm`, where m is the @p

solvers/test/scaled_diagonally_dominant_matrix_test.cc

@@ -0,0 +1,131 @@
+#include <limits>
+
+#include <gtest/gtest.h>
+
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/common/test_utilities/symbolic_test_util.h"
+#include "drake/solvers/mathematical_program.h"
+
+using drake::symbolic::test::ExprEqual;
+
+namespace drake {
+namespace solvers {
+GTEST_TEST(ScaledDiagonallyDominantMatrixTest, AddConstraint) {
+  MathematicalProgram prog;
+  auto X = prog.NewSymmetricContinuousVariables<4>();
+
+  auto M = prog.AddScaledDiagonallyDominantMatrixConstraint(
+      X.cast<symbolic::Expression>());
+  for (int i = 0; i < 4; ++i) {
+    for (int j = 0; j < 4; ++j) {
+      if (i >= j) {
+        for (int m = 0; m < 2; ++m) {
+          for (int n = 0; n < 2; ++n) {
+            EXPECT_EQ(M[i][j](m, n), 0);
+          }
+        }
+      } else {
+        EXPECT_PRED2(ExprEqual, M[i][j](0, 1).Expand(),
+                     symbolic::Expression(X(i, j)));
+      }
+    }
+  }
+}
+
+void CheckSDDMatrix(const Eigen::Ref<const Eigen::MatrixXd>& X_val,
+                    bool is_sdd) {
+  // Test if a sdd matrix satisfies the constraint.
+  const int nx = X_val.rows();
+  DRAKE_DEMAND(X_val.cols() == nx);
+  MathematicalProgram prog;
+  auto X = prog.NewSymmetricContinuousVariables(nx);
+  auto M = prog.AddScaledDiagonallyDominantMatrixConstraint(
+      X.cast<symbolic::Expression>());
+
+  for (int i = 0; i < nx; ++i) {
+    prog.AddBoundingBoxConstraint(X_val.col(i), X_val.col(i), X.col(i));
+  }
+
+  const auto result = prog.Solve();
+  if (is_sdd) {
+    EXPECT_EQ(result, SolutionResult::kSolutionFound);
+  } else {
+    EXPECT_TRUE(result == SolutionResult::kInfeasibleConstraints ||
+                result == SolutionResult::kInfeasible_Or_Unbounded);
+  }
+}
+
+bool IsMatrixSDD(const Eigen::Ref<Eigen::MatrixXd>& X) {
+  // A matrix X is scaled diagonally dominant, if there exists a positive vector
+  // d, such that the matrix A defined as A(i, j) = d(j) * X(i, j) is diagonally
+  // dominant with positive diagonals.
+  // This is explained as Remark 6 of "DSOS and SDSOS optimization: more
+  // tractable alternatives to sum of squares and semidefinite optimization" by
+  // Amir Ali Ahmadi and Anirudha Majumdar, with arXiv link
+  // https://arxiv.org/abs/1706.02586.
+  const int nx = X.rows();
+  MathematicalProgram prog;
+  auto d = prog.NewContinuousVariables(nx);
+  MatrixX<symbolic::Expression> A(nx, nx);
+  for (int i = 0; i < nx; ++i) {
+    for (int j = 0; j < nx; ++j) {
+      A(i, j) = d(j) * X(i, j);
+    }
+  }
+  prog.AddPositiveDiagonallyDominantMatrixConstraint(A);
+  prog.AddBoundingBoxConstraint(1, std::numeric_limits<double>::infinity(), d);
+
+  const auto result = prog.Solve();
+  return result == solvers::SolutionResult::kSolutionFound;
+}
+
+GTEST_TEST(ScaledDiagonallyDominantMatrixTest, TestSDDMatrix) {
+  Eigen::Matrix4d dd_X;
+  // A diagonally dominant matrix.
+  // clang-format off
+  dd_X << 1, -0.2, 0.3, -0.45,
+           -0.2, 2, 0.5, 1,
+           0.3, 0.5, 3, 2,
+           -0.45, 1, 2, 4;
+  // clang-format on
+  CheckSDDMatrix(dd_X, true);
+
+  Eigen::Matrix4d D = Eigen::Vector4d(1, 2, 3, 4).asDiagonal();
+  Eigen::Matrix4d sdd_X = D * dd_X * D;
+  CheckSDDMatrix(sdd_X, true);
+
+  D = Eigen::Vector4d(0.2, -1, -0.5, 1.2).asDiagonal();
+  sdd_X = D * dd_X * D;
+  CheckSDDMatrix(sdd_X, true);
+
+  // not_dd_X is not diagonally dominant (dd), but is scaled diagonally
+  // dominant.
+  Eigen::Matrix4d not_dd_X;
+  not_dd_X << 1, -0.2, 0.3, -0.55, -0.2, 2, 0.5, 1, 0.3, 0.5, 3, 2, -0.55, 1, 2,
+      4;
+  DRAKE_DEMAND(IsMatrixSDD(not_dd_X));
+  CheckSDDMatrix(not_dd_X, true);
+}
+
+GTEST_TEST(ScaledDiagonallyDominantMatrixTest, TestNotSDDMatrix) {
+  Eigen::Matrix4d not_sdd_X;
+  // Not a diagonally dominant matrix.
+  // clang-format off
+  not_sdd_X << 1, -0.2, 0.3, -1.55,
+               -0.2, 2, 0.5, 1,
+               0.3, 0.5, 3, 2,
+               -1.55, 1, 2, 4;
+  // clang-format on
+  DRAKE_DEMAND(!IsMatrixSDD(not_sdd_X));
+  CheckSDDMatrix(not_sdd_X, false);
+
+  Eigen::Matrix4d D = Eigen::Vector4d(1, 2, 3, 4).asDiagonal();
+  not_sdd_X = D * not_sdd_X * D;
+  CheckSDDMatrix(not_sdd_X, false);
+
+  D = Eigen::Vector4d(0.2, -1, -0.5, 1.2).asDiagonal();
+  not_sdd_X = D * not_sdd_X * D;
+  CheckSDDMatrix(not_sdd_X, false);
+}
+}  // namespace solvers
+}  // namespace drake