From 9d5b2e49535a137aa3d3f9aafe92a2cc50c8fb3d Mon Sep 17 00:00:00 2001
From: Hongkai Dai <daihongkai@gmail.com>
Date: Tue, 8 Jun 2021 21:46:08 -0400
Subject: [PATCH] Add python binding for LinearMatrixInequalityConstraint.
 (#15146)

---
 .../pydrake/solvers/mathematicalprogram_py.cc | 21 +++++++++++++++++++
 .../solvers/test/mathematicalprogram_test.py  | 12 +++++++++++
 2 files changed, 33 insertions(+)

diff --git a/bindings/pydrake/solvers/mathematicalprogram_py.cc b/bindings/pydrake/solvers/mathematicalprogram_py.cc
index 6ef07a3b4fad..b9c366b3c60f 100644
--- a/bindings/pydrake/solvers/mathematicalprogram_py.cc
+++ b/bindings/pydrake/solvers/mathematicalprogram_py.cc
@@ -34,6 +34,7 @@ using solvers::LinearComplementarityConstraint;
 using solvers::LinearConstraint;
 using solvers::LinearCost;
 using solvers::LinearEqualityConstraint;
+using solvers::LinearMatrixInequalityConstraint;
 using solvers::LorentzConeConstraint;
 using solvers::MathematicalProgram;
 using solvers::MathematicalProgramResult;
@@ -980,6 +981,15 @@ top-level documentation for :py:mod:`pydrake.math`.
           },
           doc.MathematicalProgram.AddPositiveSemidefiniteConstraint
               .doc_1args_constEigenMatrixBase)
+      .def(
+          "AddLinearMatrixInequalityConstraint",
+          [](MathematicalProgram* self,
+              const std::vector<Eigen::Ref<const Eigen::MatrixXd>>& F,
+              const Eigen::Ref<const VectorXDecisionVariable>& vars) {
+            return self->AddLinearMatrixInequalityConstraint(F, vars);
+          },
+          py::arg("F"), py::arg("vars"),
+          doc.MathematicalProgram.AddLinearMatrixInequalityConstraint.doc)
       .def("AddSosConstraint",
           static_cast<MatrixXDecisionVariable (MathematicalProgram::*)(
               const Polynomial&, const Eigen::Ref<const VectorX<Monomial>>&)>(
@@ -1449,6 +1459,15 @@ for every column of ``prog_var_vals``. )""")
       std::shared_ptr<PositiveSemidefiniteConstraint>>(m,
       "PositiveSemidefiniteConstraint", doc.PositiveSemidefiniteConstraint.doc);
 
+  py::class_<LinearMatrixInequalityConstraint, Constraint,
+      std::shared_ptr<LinearMatrixInequalityConstraint>>(m,
+      "LinearMatrixInequalityConstraint",
+      doc.LinearMatrixInequalityConstraint.doc)
+      .def("F", &LinearMatrixInequalityConstraint::F,
+          doc.LinearMatrixInequalityConstraint.F.doc)
+      .def("matrix_rows", &LinearMatrixInequalityConstraint::matrix_rows,
+          doc.LinearMatrixInequalityConstraint.matrix_rows.doc);
+
   py::class_<LinearComplementarityConstraint, Constraint,
       std::shared_ptr<LinearComplementarityConstraint>>(m,
       "LinearComplementarityConstraint",
@@ -1467,6 +1486,8 @@ for every column of ``prog_var_vals``. )""")
   RegisterBinding<BoundingBoxConstraint>(&m, "BoundingBoxConstraint");
   RegisterBinding<PositiveSemidefiniteConstraint>(
       &m, "PositiveSemidefiniteConstraint");
+  RegisterBinding<LinearMatrixInequalityConstraint>(
+      &m, "LinearMatrixInequalityConstraint");
   RegisterBinding<LinearComplementarityConstraint>(
       &m, "LinearComplementarityConstraint");
   RegisterBinding<ExponentialConeConstraint>(&m, "ExponentialConeConstraint");
diff --git a/bindings/pydrake/solvers/test/mathematicalprogram_test.py b/bindings/pydrake/solvers/test/mathematicalprogram_test.py
index 9970520afc06..83916641997b 100644
--- a/bindings/pydrake/solvers/test/mathematicalprogram_test.py
+++ b/bindings/pydrake/solvers/test/mathematicalprogram_test.py
@@ -952,6 +952,18 @@ def test_add_rotated_lorentz_cone_constraint(self):
             linear_expression1=x[0]+1, linear_expression2=x[0]+x[1],
             quadratic_expression=x[0]*x[0] + 2*x[0] + x[1]*x[1] + 5)
 
+    def test_add_linear_matrix_inequality_constraint(self):
+        prog = mp.MathematicalProgram()
+        F = [np.eye(2), np.array([[0, 1], [1., 0.]])]
+        x = prog.NewContinuousVariables(1)
+        cnstr = prog.AddLinearMatrixInequalityConstraint(F=F, vars=x)
+        self.assertIsInstance(
+            cnstr.evaluator(), mp.LinearMatrixInequalityConstraint)
+        self.assertEqual(cnstr.evaluator().matrix_rows(), 2)
+        self.assertEqual(len(cnstr.evaluator().F()), 2)
+        np.testing.assert_array_equal(cnstr.evaluator().F()[0], F[0])
+        np.testing.assert_array_equal(cnstr.evaluator().F()[1], F[1])
+
     def test_solver_options(self):
         prog = mp.MathematicalProgram()