Return the new cost and slack variables from AddMaximizeLogDeterminan…

…tSymmetricMatrixCost
RobotLocomotion · Dec 31, 2021 · b27eca1 · b27eca1
1 parent 2ad76f8
commit b27eca1
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 7 deletions.
diff --git 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<void (MathematicalProgram::*)(
+          static_cast<std::tuple<Binding<LinearCost>,
+              VectorX<symbolic::Variable>, MatrixX<symbolic::Variable>> (
+              MathematicalProgram::*)(
               const Eigen::Ref<const MatrixX<symbolic::Expression>>& X)>(
               &MathematicalProgram::
                   AddMaximizeLogDeterminantSymmetricMatrixCost),

diff --git 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
@@ -522,19 +522,23 @@ Binding<Cost> MathematicalProgram::AddCost(const Expression& e) {
   return AddCost(internal::ParseCost(e));
 }
 
-void MathematicalProgram::AddMaximizeLogDeterminantSymmetricMatrixCost(
+std::tuple<Binding<LinearCost>, VectorX<symbolic::Variable>,
+           MatrixX<symbolic::Variable>>
+MathematicalProgram::AddMaximizeLogDeterminantSymmetricMatrixCost(
     const Eigen::Ref<const MatrixX<symbolic::Expression>>& X) {
   DRAKE_DEMAND(X.rows() == X.cols());
   const int X_rows = X.rows();
   auto Z_lower = NewContinuousVariables(X_rows * (X_rows + 1) / 2);
   MatrixX<symbolic::Expression> Z(X_rows, X_rows);
+  MatrixX<symbolic::Variable> Z_var(X_rows, X_rows);
   Z.setZero();
   // diag_Z is the diagonal matrix that only contains the diagonal entries of Z.
   MatrixX<symbolic::Expression> diag_Z(X_rows, X_rows);
   diag_Z.setZero();
   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<symbolic::Expression>(Z(i, i), 1, t(i)));
   }
 
-  AddLinearCost(-t.cast<symbolic::Expression>().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
@@ -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<Binding<LinearCost>, VectorX<symbolic::Variable>,
+             MatrixX<symbolic::Variable>>
+  AddMaximizeLogDeterminantSymmetricMatrixCost(
       const Eigen::Ref<const MatrixX<symbolic::Expression>>& X);
 
   /**

diff --git 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<symbolic::Expression>() / 2,
       b.cast<symbolic::Expression>().transpose() / 2, c;
   prog.AddPositiveSemidefiniteConstraint(ellipsoid_psd);
-  prog.AddMaximizeLogDeterminantSymmetricMatrixCost(
+  const auto log_det_ret = prog.AddMaximizeLogDeterminantSymmetricMatrixCost(
       S.cast<symbolic::Expression>());
   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