Return the new cost and slack variables from AddMaximizeLogDeterminan…

…tSymmetricMatrixCost

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),

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())
 

solvers/mathematical_program.cc

@@ -522,19 +522,26 @@ 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);
+      if (i > j) {
+        Z_var(j, i) = Z_var(i, j);
+      }
       Z(i, j) = Z_lower(Z_lower_index++);
     }
     diag_Z(j, j) = Z(j, j);
@@ -554,7 +561,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, std::move(t), std::move(Z_var));
 }
 
 void MathematicalProgram::AddMaximizeGeometricMeanCost(

solvers/mathematical_program.h

@@ -1205,12 +1205,16 @@ 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 Z.
    * @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);
 
   /**

solvers/test/exponential_cone_program_examples.cc

@@ -95,8 +95,11 @@ 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>());
+  const auto linear_cost = std::get<0>(log_det_ret);
+  const auto log_det_t = std::get<1>(log_det_ret);
+  const auto log_det_Z = std::get<2>(log_det_ret);
   for (int i = 0; i < 4; ++i) {
     prog.AddLinearConstraint(
         pts.col(i).dot(S.cast<symbolic::Expression>() * pts.col(i)) +
@@ -122,6 +125,13 @@ 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);
+  const Eigen::MatrixXd log_det_Z_sol = result.GetSolution(log_det_Z);
+  EXPECT_TRUE(
+      (log_det_Z_sol.diagonal().array().log() >= log_det_t_sol.array() - tol)
+          .all());
   EXPECT_NEAR(-std::log(S_sol.determinant()), expected_cost, tol);
 }
 }  // namespace test