Merge pull request RobotLocomotion#9293 from mpetersen94/py_more_bind…

…ings pydrake: Add bindings for AddLinearEqualityConstraint
Gbravop · Aug 23, 2018 · 6001128 · 6001128
2 parents 557e31a + 4823a63
commit 6001128
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 0 deletions.
diff --git a/bindings/pydrake/solvers/mathematicalprogram_py.cc b/bindings/pydrake/solvers/mathematicalprogram_py.cc
@@ -307,6 +307,21 @@ PYBIND11_MODULE(_mathematicalprogram_py, m) {
       .def("AddLinearConstraint",
            static_cast<Binding<LinearConstraint> (MathematicalProgram::*)(
                const Formula&)>(&MathematicalProgram::AddLinearConstraint))
+      .def("AddLinearEqualityConstraint",
+           static_cast<Binding<LinearEqualityConstraint> (
+               MathematicalProgram::*)(
+               const Eigen::Ref<const Eigen::MatrixXd>&,
+               const Eigen::Ref<const Eigen::VectorXd>&,
+               const Eigen::Ref<const VectorXDecisionVariable>&)>(
+               &MathematicalProgram::AddLinearEqualityConstraint))
+      .def("AddLinearEqualityConstraint",
+           static_cast<Binding<LinearEqualityConstraint> (
+               MathematicalProgram::*)(const Expression&, double)>(
+               &MathematicalProgram::AddLinearEqualityConstraint))
+      .def("AddLinearEqualityConstraint",
+           static_cast<Binding<LinearEqualityConstraint> (
+               MathematicalProgram::*)(const Formula&)>(
+               &MathematicalProgram::AddLinearEqualityConstraint))
       .def("AddLorentzConeConstraint",
            static_cast<Binding<LorentzConeConstraint> (MathematicalProgram::*)(
                const Eigen::Ref<const VectorX<drake::symbolic::Expression>>&)>(

diff --git a/bindings/pydrake/solvers/test/mathematicalprogram_test.py b/bindings/pydrake/solvers/test/mathematicalprogram_test.py
@@ -301,6 +301,10 @@ def test_linear_constraints(self):
         prog.AddBoundingBoxConstraint(0., 1., x)
         prog.AddLinearConstraint(np.eye(2), np.zeros(2), np.ones(2), x)
 
+        prog.AddLinearEqualityConstraint(np.eye(2), np.zeros(2), x)
+        prog.AddLinearEqualityConstraint(x[0] == 1)
+        prog.AddLinearEqualityConstraint(x[0] + x[1], 1)
+
     def test_pycost_and_pyconstraint(self):
         prog = mp.MathematicalProgram()
         x = prog.NewContinuousVariables(1, 'x')

diff --git a/bindings/pydrake/util/eigen_geometry_py.cc b/bindings/pydrake/util/eigen_geometry_py.cc
@@ -247,6 +247,9 @@ PYBIND11_MODULE(eigen_geometry, m) {
       }, py::arg("position"))
       .def("inverse", [](const Class* self) {
         return self->inverse();
+      })
+      .def("conjugate", [](const Class* self) {
+        return self->conjugate();
       });
   }
 

diff --git a/bindings/pydrake/util/test/eigen_geometry_test.py b/bindings/pydrake/util/test/eigen_geometry_test.py
@@ -73,6 +73,8 @@ def test_quaternion(self):
             (q.multiply(position=[1, 2, 3]) == [3, 1, 2]).all())
         q_I = q.inverse().multiply(q)
         self.assertTrue(np.allclose(q_I.wxyz(), [1, 0, 0, 0]))
+        q_conj = q.conjugate()
+        self.assertTrue(np.allclose(q_conj.wxyz(), [0.5, -0.5, -0.5, -0.5]))
 
         # Test `type_caster`s.
         value = test_util.create_quaternion()