From 4664842b8840700ccda61f40878686ce6b91a544 Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Tue, 18 Sep 2018 22:30:06 -0700 Subject: [PATCH] Add scaled diagonally dominant matrix constraint. --- solvers/BUILD.bazel | 10 ++ solvers/mathematical_program.cc | 34 +++++ solvers/mathematical_program.h | 30 ++++ .../scaled_diagonally_dominant_matrix_test.cc | 128 ++++++++++++++++++ 4 files changed, 202 insertions(+) create mode 100644 solvers/test/scaled_diagonally_dominant_matrix_test.cc diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index e18f4e725c00..d35433c188c6 100644 --- a/solvers/BUILD.bazel +++ b/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 = [ diff --git a/solvers/mathematical_program.cc b/solvers/mathematical_program.cc index 6d760a0b7c6a..5534990dbc7e 100644 --- a/solvers/mathematical_program.cc +++ b/solvers/mathematical_program.cc @@ -796,6 +796,40 @@ MathematicalProgram::AddPositiveDiagonallyDominantMatrixConstraint( return Y; } +std::vector>> +MathematicalProgram::AddScaledDiagonallyDominantMatrixConstraint( + const Eigen::Ref>& X) { + const int n = X.rows(); + DRAKE_DEMAND(X.cols() == n); + std::vector>> 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( + 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. diff --git a/solvers/mathematical_program.h b/solvers/mathematical_program.h index 747d8c689c4f..deb6a6254011 100644 --- a/solvers/mathematical_program.h +++ b/solvers/mathematical_program.h @@ -2050,6 +2050,9 @@ 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) + * 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. * @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. @@ -2060,6 +2063,33 @@ class MathematicalProgram { MatrixX AddPositiveDiagonallyDominantMatrixConstraint( const Eigen::Ref>& 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 entriex 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, 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. + * @param X The symmetric matrix X. We will assume that @p X is already + * symmetric. It is the user's responsibility to guarantee the symmetry. + * @pre X(i, j) should be a linear expression of decision variables. + * @return M for i < j, M[i][j] is the 2 x 2 symmetric matrix + * [Mⁱʲ(i, i), Mⁱʲ(i, j)] + * [Mⁱʲ(i, j), Mⁱʲ(j, j)] + * Note that M[i][j](0, 1) = Mⁱʲ(i, j) = (X(i, j) + X(j, i)) / 2 + * for i >= j, M[i][j] is 0 matrix. + */ + std::vector>> + AddScaledDiagonallyDominantMatrixConstraint( + const Eigen::Ref>& 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 diff --git a/solvers/test/scaled_diagonally_dominant_matrix_test.cc b/solvers/test/scaled_diagonally_dominant_matrix_test.cc new file mode 100644 index 000000000000..171117b68ad0 --- /dev/null +++ b/solvers/test/scaled_diagonally_dominant_matrix_test.cc @@ -0,0 +1,128 @@ +#include + +#include + +#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()); + 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& 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()); + + 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& 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. + const int nx = X.rows(); + MathematicalProgram prog; + auto d = prog.NewContinuousVariables(nx); + MatrixX 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::infinity(), d); + + const auto result = prog.Solve(); + std::cout << result << "\n"; + 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