From 597504de566b829323f50c1823074de56bece70e Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Fri, 31 Dec 2021 11:45:52 -0800 Subject: [PATCH] Return the new cost and slack variables from AddMaximizeLogDeterminantSymmetricMatrixCost --- .../pydrake/solvers/mathematicalprogram_py.cc | 4 +++- .../solvers/test/mathematicalprogram_test.py | 5 ++++- solvers/mathematical_program.cc | 7 +++++-- solvers/mathematical_program.h | 11 ++++++++++- .../test/exponential_cone_program_examples.cc | 18 ++++++++++++++++-- 5 files changed, 38 insertions(+), 7 deletions(-) diff --git a/bindings/pydrake/solvers/mathematicalprogram_py.cc b/bindings/pydrake/solvers/mathematicalprogram_py.cc index 7e83a499d353..1c248de5c114 100644 --- a/bindings/pydrake/solvers/mathematicalprogram_py.cc +++ b/bindings/pydrake/solvers/mathematicalprogram_py.cc @@ -875,7 +875,9 @@ void BindMathematicalProgram(py::module m) { py::arg("b"), py::arg("vars"), doc.MathematicalProgram.AddL2NormCostUsingConicConstraint.doc) .def("AddMaximizeLogDeterminantSymmetricMatrixCost", - static_cast, + VectorX, MatrixX> ( + MathematicalProgram::*)( const Eigen::Ref>& X)>( &MathematicalProgram:: AddMaximizeLogDeterminantSymmetricMatrixCost), diff --git a/bindings/pydrake/solvers/test/mathematicalprogram_test.py b/bindings/pydrake/solvers/test/mathematicalprogram_test.py index 8e4af7f165a5..fcb9341048e7 100644 --- a/bindings/pydrake/solvers/test/mathematicalprogram_test.py +++ b/bindings/pydrake/solvers/test/mathematicalprogram_test.py @@ -686,7 +686,10 @@ def test_log_determinant(self): for i in range(3): pt = pts[i, :] prog.AddLinearConstraint(pt.dot(X.dot(pt)) <= 1) - prog.AddMaximizeLogDeterminantSymmetricMatrixCost(X) + linear_cost, log_det_t, log_det_Z = \ + prog.AddMaximizeLogDeterminantSymmetricMatrixCost(X=X) + self.assertEqual(log_det_t.shape, (2,)) + self.assertEqual(log_det_Z.shape, (2, 2)) result = mp.Solve(prog) self.assertTrue(result.is_success()) diff --git a/solvers/mathematical_program.cc b/solvers/mathematical_program.cc index 121f59ecdb73..1b99b6141113 100644 --- a/solvers/mathematical_program.cc +++ b/solvers/mathematical_program.cc @@ -522,7 +522,9 @@ Binding MathematicalProgram::AddCost(const Expression& e) { return AddCost(internal::ParseCost(e)); } -void MathematicalProgram::AddMaximizeLogDeterminantSymmetricMatrixCost( +std::tuple, VectorX, + MatrixX> +MathematicalProgram::AddMaximizeLogDeterminantSymmetricMatrixCost( const Eigen::Ref>& X) { DRAKE_DEMAND(X.rows() == X.cols()); const int X_rows = X.rows(); @@ -554,7 +556,8 @@ void MathematicalProgram::AddMaximizeLogDeterminantSymmetricMatrixCost( Vector3(Z(i, i), 1, t(i))); } - AddLinearCost(-t.cast().sum()); + const auto cost = AddLinearCost(-Eigen::VectorXd::Ones(t.rows()), t); + return std::make_tuple(cost, std::move(t), std::move(Z)); } void MathematicalProgram::AddMaximizeGeometricMeanCost( diff --git a/solvers/mathematical_program.h b/solvers/mathematical_program.h index b249371b5e5c..11b40bf3bbf9 100644 --- a/solvers/mathematical_program.h +++ b/solvers/mathematical_program.h @@ -1205,12 +1205,21 @@ class MathematicalProgram { * and we will minimize -∑ᵢt(i). * @param X A symmetric positive semidefinite matrix X, whose log(det(X)) will * be maximized. + * @return (cost, t, Z) cost is -∑ᵢt(i), we also return the newly created + * slack variables t and the lower triangular matrix Z. Note that Z is not a + * matrix of symbolic::Variable but symbolic::Expression, because the + * upper-diagonal entries of Z are not variable, but expression 0. * @pre X is a symmetric matrix. + * @note We implicitly require that `X` being positive semidefinite (psd) (as + * X is the diagonal entry of the big psd matrix above). If your `X` is not + * necessarily psd, then don't call this function. * @note The constraint log(Z(i, i)) >= t(i) is imposed as an exponential cone * constraint. Please make sure your have a solver that supports exponential * cone constraint (currently SCS does). */ - void AddMaximizeLogDeterminantSymmetricMatrixCost( + std::tuple, VectorX, + MatrixX> + AddMaximizeLogDeterminantSymmetricMatrixCost( const Eigen::Ref>& X); /** diff --git a/solvers/test/exponential_cone_program_examples.cc b/solvers/test/exponential_cone_program_examples.cc index e3053f196d5c..3b6ba3617726 100644 --- a/solvers/test/exponential_cone_program_examples.cc +++ b/solvers/test/exponential_cone_program_examples.cc @@ -95,8 +95,9 @@ void MinimalEllipsoidCoveringPoints(const SolverInterface& solver, double tol) { ellipsoid_psd << S, b.cast() / 2, b.cast().transpose() / 2, c; prog.AddPositiveSemidefiniteConstraint(ellipsoid_psd); - prog.AddMaximizeLogDeterminantSymmetricMatrixCost( - S.cast()); + const auto [linear_cost, log_det_t, log_det_Z] = + prog.AddMaximizeLogDeterminantSymmetricMatrixCost( + S.cast()); for (int i = 0; i < 4; ++i) { prog.AddLinearConstraint( pts.col(i).dot(S.cast() * pts.col(i)) + @@ -122,6 +123,19 @@ void MinimalEllipsoidCoveringPoints(const SolverInterface& solver, double tol) { const double expected_cost = -std::log(0.25 / std::pow(scaling_factor(0) * scaling_factor(1), 2)); EXPECT_NEAR(result.get_optimal_cost(), expected_cost, tol); + EXPECT_NEAR(result.EvalBinding(linear_cost)(0), expected_cost, tol); + const Eigen::VectorXd log_det_t_sol = result.GetSolution(log_det_t); + EXPECT_NEAR(log_det_t_sol.sum(), -expected_cost, tol); + Eigen::MatrixXd log_det_Z_sol = + ExtractDoubleOrThrow(result.GetSolution(log_det_Z)); + EXPECT_TRUE( + (log_det_Z_sol.diagonal().array().log() >= log_det_t_sol.array() - tol) + .all()); + EXPECT_TRUE(CompareMatrices( + Eigen::TriangularView( + log_det_Z_sol) + .toDenseMatrix(), + Eigen::MatrixXd::Zero(log_det_Z_sol.rows(), log_det_Z_sol.cols()))); EXPECT_NEAR(-std::log(S_sol.determinant()), expected_cost, tol); } } // namespace test