From b27eca15971fb569a48d4224e14d3ac5f336a824 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 --- bindings/pydrake/solvers/mathematicalprogram_py.cc | 4 +++- .../pydrake/solvers/test/mathematicalprogram_test.py | 5 ++++- solvers/mathematical_program.cc | 9 +++++++-- solvers/mathematical_program.h | 7 +++++-- solvers/test/exponential_cone_program_examples.cc | 6 +++++- 5 files changed, 24 insertions(+), 7 deletions(-) diff --git a/bindings/pydrake/solvers/mathematicalprogram_py.cc b/bindings/pydrake/solvers/mathematicalprogram_py.cc index 7e83a499d353..0ba8e6a816c0 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..32f2350f719f 100644 --- a/solvers/mathematical_program.cc +++ b/solvers/mathematical_program.cc @@ -522,12 +522,15 @@ 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(); auto Z_lower = NewContinuousVariables(X_rows * (X_rows + 1) / 2); MatrixX Z(X_rows, X_rows); + MatrixX Z_var(X_rows, X_rows); Z.setZero(); // diag_Z is the diagonal matrix that only contains the diagonal entries of Z. MatrixX diag_Z(X_rows, X_rows); @@ -535,6 +538,7 @@ void MathematicalProgram::AddMaximizeLogDeterminantSymmetricMatrixCost( int Z_lower_index = 0; for (int j = 0; j < X_rows; ++j) { for (int i = j; i < X_rows; ++i) { + Z_var(i, j) = Z_lower(Z_lower_index); Z(i, j) = Z_lower(Z_lower_index++); } diag_Z(j, j) = Z(j, j); @@ -554,7 +558,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, t, Z_var); } void MathematicalProgram::AddMaximizeGeometricMeanCost( diff --git a/solvers/mathematical_program.h b/solvers/mathematical_program.h index b249371b5e5c..1f9a908e7d5e 100644 --- a/solvers/mathematical_program.h +++ b/solvers/mathematical_program.h @@ -1204,13 +1204,16 @@ class MathematicalProgram { * log(Z(i, i)) >= t(i) * 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 Z. be maximized. * @pre X is a symmetric matrix. * @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..12414d3190d8 100644 --- a/solvers/test/exponential_cone_program_examples.cc +++ b/solvers/test/exponential_cone_program_examples.cc @@ -95,7 +95,7 @@ void MinimalEllipsoidCoveringPoints(const SolverInterface& solver, double tol) { ellipsoid_psd << S, b.cast() / 2, b.cast().transpose() / 2, c; prog.AddPositiveSemidefiniteConstraint(ellipsoid_psd); - prog.AddMaximizeLogDeterminantSymmetricMatrixCost( + const auto log_det_ret = prog.AddMaximizeLogDeterminantSymmetricMatrixCost( S.cast()); for (int i = 0; i < 4; ++i) { prog.AddLinearConstraint( @@ -122,6 +122,10 @@ 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(std::get<0>(log_det_ret))(0), expected_cost, + tol); + EXPECT_NEAR(result.GetSolution(std::get<1>(log_det_ret)).sum(), + -expected_cost, tol); EXPECT_NEAR(-std::log(S_sol.determinant()), expected_cost, tol); } } // namespace test