Merge pull request RobotLocomotion#7695 from christian-rauch/py_rbt

python bindings for 'RigidBodyTree::CalcBodyPoseInWorldFrame'
KyleFromKitware · Jan 5, 2018 · 8022000 · 8022000
2 parents af0d2ff + 0fdfa1c
commit 8022000
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/bindings/pydrake/rbtree_py.cc b/bindings/pydrake/rbtree_py.cc
@@ -103,6 +103,11 @@ PYBIND11_MODULE(_rbtree_py, m) {
                             const VectorXAutoDiffXd& v) {
       return tree.doKinematics(q, v);
     })
+    .def("CalcBodyPoseInWorldFrame", [](const RigidBodyTree<double>& tree,
+                                        const KinematicsCache<double> &cache,
+                                        const RigidBody<double> &body) {
+      return tree.CalcBodyPoseInWorldFrame(cache, body).matrix();
+    })
     .def("centerOfMass", &RigidBodyTree<double>::centerOfMass<double>,
          py::arg("cache"),
          py::arg("model_instance_id_set") =

diff --git a/bindings/pydrake/test/rbt_transform_points_test.py b/bindings/pydrake/test/rbt_transform_points_test.py
@@ -56,6 +56,24 @@ def test_relative_transform(self):
                                               [-1, 0, 0, 0],
                                               [0, 0, 0, 1]])))
 
+    def test_fk_pose_world_frame(self):
+        # load pendulum robot with fixed base
+        r = pydrake.rbtree.RigidBodyTree(os.path.join(
+                pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf"),
+                pydrake.rbtree.FloatingBaseType.kFixed
+            )
+        # set test configuration (Pendulum robot has single joint 'theta')
+        q = r.getZeroConfiguration()
+        q[0] = np.pi / 2
+        # do FK and compare pose of 'arm' with expected pose
+        k = r.doKinematics(q)
+        pose_fk = r.CalcBodyPoseInWorldFrame(k, r.FindBody("arm"))
+        pose_true = np.array([[0, 0, 1, 0],
+                              [0, 1, 0, 0],
+                              [-1, 0, 0, 0],
+                              [0, 0, 0, 1]])
+        self.assertTrue(np.allclose(pose_fk, pose_true))
+
 
 if __name__ == '__main__':
     unittest.main()