Add python binding for CalcJacobianAngular/TranslationalVelocity (Rob…

…otLocomotion#12229) Add python binding for CalcJacobianAngular/TranslationalVelocity, and initializeAutoDiffGivenGradientMatrix.
Tomato1107 · Oct 22, 2019 · 9efdaee · 9efdaee
1 parent a87d835
commit 9efdaee
Show file tree

Hide file tree

Showing 4 changed files with 52 additions and 0 deletions.
diff --git a/bindings/pydrake/autodiffutils_py.cc b/bindings/pydrake/autodiffutils_py.cc
@@ -123,6 +123,13 @@ PYBIND11_MODULE(autodiffutils, m) {
       },
       py::arg("autodiff_matrix"), doc.autoDiffToGradientMatrix.doc);
 
+  m.def("initializeAutoDiffGivenGradientMatrix",
+      [](const Eigen::VectorXd& val, const Eigen::MatrixXd& gradient) {
+        return initializeAutoDiffGivenGradientMatrix(val, gradient);
+      },
+      py::arg("val"), py::arg("gradient"),
+      doc.initializeAutoDiffGivenGradientMatrix.doc_2args);
+
   ExecuteExtraPythonCode(m);
 }
 

diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc
@@ -396,6 +396,33 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py::arg("context"), py::arg("with_respect_to"), py::arg("frame_B"),
             py::arg("p_BP"), py::arg("frame_A"), py::arg("frame_E"),
             cls_doc.CalcJacobianSpatialVelocity.doc)
+        .def("CalcJacobianAngularVelocity",
+            [](const Class* self, const Context<T>& context,
+                JacobianWrtVariable with_respect_to, const Frame<T>& frame_B,
+                const Frame<T>& frame_A, const Frame<T>& frame_E) {
+              Matrix3X<T> Js_w_AB_E(
+                  3, GetVariableSize<T>(*self, with_respect_to));
+              self->CalcJacobianAngularVelocity(context, with_respect_to,
+                  frame_B, frame_A, frame_E, &Js_w_AB_E);
+              return Js_w_AB_E;
+            },
+            py::arg("context"), py::arg("with_respect_to"), py::arg("frame_B"),
+            py::arg("frame_A"), py::arg("frame_E"),
+            cls_doc.CalcJacobianAngularVelocity.doc)
+        .def("CalcJacobianTranslationalVelocity",
+            [](const Class* self, const Context<T>& context,
+                JacobianWrtVariable with_respect_to, const Frame<T>& frame_B,
+                const Eigen::Ref<const Matrix3X<T>>& p_BoBi_B,
+                const Frame<T>& frame_A, const Frame<T>& frame_E) {
+              Matrix3X<T> Js_v_ABi_E(
+                  3, GetVariableSize<T>(*self, with_respect_to));
+              self->CalcJacobianTranslationalVelocity(context, with_respect_to,
+                  frame_B, p_BoBi_B, frame_A, frame_E, &Js_v_ABi_E);
+              return Js_v_ABi_E;
+            },
+            py::arg("context"), py::arg("with_respect_to"), py::arg("frame_B"),
+            py::arg("p_BoBi_B"), py::arg("frame_A"), py::arg("frame_E"),
+            cls_doc.CalcJacobianTranslationalVelocity.doc)
         .def("CalcSpatialAccelerationsFromVdot",
             [](const Class* self, const Context<T>& context,
                 const VectorX<T>& known_vdot) {

diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py
@@ -405,6 +405,16 @@ def test_multibody_tree_kinematics(self, T):
                 frame_E=world_frame)
             self.assert_sane(Jw_ABp_E)
             self.assertEqual(Jw_ABp_E.shape, (6, nw))
+            Jw_w_AB_E = plant.CalcJacobianAngularVelocity(
+                context=context, with_respect_to=wrt, frame_B=base_frame,
+                frame_A=world_frame, frame_E=world_frame)
+            self.assert_sane(Jw_w_AB_E)
+            self.assertEqual(Jw_w_AB_E.shape, (3, nw))
+            Jw_v_AB_E = plant.CalcJacobianTranslationalVelocity(
+                context=context, with_respect_to=wrt, frame_B=base_frame,
+                p_BoBi_B=np.zeros(3), frame_A=world_frame, frame_E=world_frame)
+            self.assert_sane(Jw_v_AB_E)
+            self.assertEqual(Jw_v_AB_E.shape, (3, nw))
 
         # Compute body pose.
         X_WBase = plant.EvalBodyPoseInWorld(context, base)

diff --git a/bindings/pydrake/test/autodiffutils_test.py b/bindings/pydrake/test/autodiffutils_test.py
@@ -8,6 +8,7 @@
     autoDiffToValueMatrix,
     AutoDiffXd,
     initializeAutoDiff,
+    initializeAutoDiffGivenGradientMatrix,
     initializeAutoDiffTuple,
 )
 
@@ -236,3 +237,10 @@ def test_math_utils(self):
                                       np.eye(1, 3))
         np.testing.assert_array_equal(autoDiffToGradientMatrix(b),
                                       np.hstack((np.zeros((2, 1)), np.eye(2))))
+
+        c_grad = [[2, 4, 5], [1, -1, 0]]
+        c = initializeAutoDiffGivenGradientMatrix([2, 3], c_grad)
+        np.testing.assert_array_equal(autoDiffToValueMatrix(c),
+                                      np.array([2, 3]).reshape((2, 1)))
+        np.testing.assert_array_equal(autoDiffToGradientMatrix(c),
+                                      np.array(c_grad))