Python bindings for RBT, RigidBody, and some joints (RobotLocomotion#…

…10568)

- RigidBody constructor
- FixedJoint and RPYJoint
- CollisionDetectFromPoints
- MaxDepthCollisionPoints
- Tests for the above

bindings/pydrake/attic/multibody/collision_py.cc

@@ -27,6 +27,18 @@ PYBIND11_MODULE(collision, m) {
           doc.Element.ctor.doc_2args_geometry_T_element_to_local)
       .def("set_body", &Element::set_body, doc.Element.set_body.doc)
       .def("get_body", &Element::get_body, doc.Element.get_body.doc);
+
+  py::class_<PointPair<double>>(m, "PointPair")
+      .def_readonly(
+          "elementA", &PointPair<double>::elementA, py_reference_internal)
+      .def_readonly(
+          "elementB", &PointPair<double>::elementB, py_reference_internal)
+      .def_readonly("idA", &PointPair<double>::idA)
+      .def_readonly("idB", &PointPair<double>::idB)
+      .def_readonly("ptA", &PointPair<double>::ptA)
+      .def_readonly("ptB", &PointPair<double>::ptB)
+      .def_readonly("normal", &PointPair<double>::normal)
+      .def_readonly("distance", &PointPair<double>::distance);
 }
 
 }  // namespace pydrake

bindings/pydrake/attic/multibody/joints_py.cc

@@ -6,8 +6,10 @@
 #include "drake/bindings/pydrake/pydrake_pybind.h"
 #include "drake/multibody/joints/drake_joint.h"
 #include "drake/multibody/joints/fixed_axis_one_dof_joint.h"
+#include "drake/multibody/joints/fixed_joint.h"
 #include "drake/multibody/joints/prismatic_joint.h"
 #include "drake/multibody/joints/revolute_joint.h"
+#include "drake/multibody/joints/roll_pitch_yaw_floating_joint.h"
 
 namespace drake {
 namespace pydrake {
@@ -29,12 +31,23 @@ PYBIND11_MODULE(joints, m) {
           py::arg("name"), py::arg("transform_to_parent_body"),
           py::arg("translation_axis"), doc.PrismaticJoint.ctor.doc);
 
+  py::class_<FixedJoint, DrakeJoint>(m, "FixedJoint", doc.FixedJoint.doc)
+      .def(py::init<const std::string&, const Eigen::Isometry3d&>(),
+          py::arg("name"), py::arg("transform_to_parent_body"),
+          doc.FixedJoint.ctor.doc);
+
   py::class_<RevoluteJoint, DrakeJoint>(
       m, "RevoluteJoint", doc.RevoluteJoint.doc)
       .def(py::init<const std::string&, const Eigen::Isometry3d&,
                const Eigen::Vector3d&>(),
           py::arg("name"), py::arg("transform_to_parent_body"),
           py::arg("rotation_axis"), doc.RevoluteJoint.ctor.doc);
+
+  py::class_<RollPitchYawFloatingJoint, DrakeJoint>(
+      m, "RollPitchYawFloatingJoint", doc.RollPitchYawFloatingJoint.doc)
+      .def(py::init<const std::string&, const Eigen::Isometry3d&>(),
+          py::arg("name"), py::arg("transform_to_parent_body"),
+          doc.RollPitchYawFloatingJoint.ctor.doc);
 }
 
 }  // namespace pydrake

bindings/pydrake/attic/multibody/rigid_body_tree_py.cc

@@ -185,6 +185,22 @@ PYBIND11_MODULE(rigid_body_tree, m) {
       .def("DefineCollisionFilterGroup",
           &RigidBodyTree<double>::DefineCollisionFilterGroup, py::arg("name"),
           doc.RigidBodyTree.DefineCollisionFilterGroup.doc)
+      .def("collisionDetectFromPoints",
+          [](RigidBodyTree<double>& tree, const KinematicsCache<double>& cache,
+              const Eigen::Matrix3Xd& points, bool use_margins) {
+            Eigen::VectorXd phi;
+            Eigen::Matrix3Xd normal;
+            Eigen::Matrix3Xd x;
+            Eigen::Matrix3Xd body_x;
+            std::vector<int> body_idx;
+            tree.collisionDetectFromPoints(
+                cache, points, phi, normal, x, body_x, body_idx, use_margins);
+            return std::tuple<Eigen::VectorXd, Eigen::Matrix3Xd,
+                Eigen::Matrix3Xd, Eigen::Matrix3Xd, std::vector<int>>(
+                phi, normal, x, body_x, body_idx);
+          },
+          py::arg("cache"), py::arg("points"), py::arg("use_margins"),
+          doc.RigidBodyTree.collisionDetectFromPoints.doc)
       .def("FindCollisionElement", &RigidBodyTree<double>::FindCollisionElement,
           py::arg("id"), py::return_value_policy::reference,
           doc.RigidBodyTree.FindCollisionElement.doc)
@@ -422,9 +438,13 @@ PYBIND11_MODULE(rigid_body_tree, m) {
         .def("doKinematics",
             [](const RigidBodyTree<double>& tree, const VectorX<T>& q,
                 const VectorX<T>& v) { return tree.doKinematics(q, v); },
-            doc.RigidBodyTree.doKinematics.doc_3args);
-    // CreateKinematicsCacheWithType
-    // ComputeMaximumDepthCollisionPoints
+            doc.RigidBodyTree.doKinematics.doc_3args)
+        // CreateKinematicsCacheWithType
+        .def("ComputeMaximumDepthCollisionPoints",
+            &RigidBodyTree<double>::ComputeMaximumDepthCollisionPoints<T>,
+            py::arg("cache"), py::arg("use_margins") = true,
+            py::arg("throw_if_missing_gradient") = true,
+            doc.RigidBodyTree.ComputeMaximumDepthCollisionPoints.doc);
     // Type (b) methods:
     tree_cls
         .def("transformPoints",

bindings/pydrake/multibody/test/rigid_body_tree_test.py

@@ -10,7 +10,12 @@
 from pydrake.common import FindResourceOrThrow
 from pydrake.forwarddiff import jacobian
 from pydrake.attic.multibody.collision import CollisionElement
-from pydrake.attic.multibody.joints import PrismaticJoint, RevoluteJoint
+from pydrake.attic.multibody.joints import (
+    FixedJoint,
+    PrismaticJoint,
+    RevoluteJoint,
+    RollPitchYawFloatingJoint
+    )
 from pydrake.attic.multibody.parsers import PackageMap
 from pydrake.attic.multibody.rigid_body import RigidBody
 from pydrake.attic.multibody.rigid_body_tree import (
@@ -518,6 +523,20 @@ def test_joints_api(self):
         self.assertEqual(revolute_joint_isom.get_num_positions(), 1)
         self.assertEqual(revolute_joint_isom.get_name(), name)
 
+        name = "fixed"
+        fixed_joint_np = FixedJoint(name, np.eye(4))
+        fixed_joint_isom = FixedJoint(name, Isometry3.Identity())
+        self.assertEqual(fixed_joint_isom.get_num_positions(), 0)
+        self.assertEqual(fixed_joint_isom.get_name(), name)
+
+        name = "rpy"
+        rpy_floating_joint_np = RollPitchYawFloatingJoint(
+            name, np.eye(4))
+        rpy_floating_joint_isom = RollPitchYawFloatingJoint(
+            name, Isometry3.Identity())
+        self.assertEqual(rpy_floating_joint_isom.get_num_positions(), 6)
+        self.assertEqual(rpy_floating_joint_isom.get_name(), name)
+
     def test_collision_element_api(self):
         # Verify construction from both Isometry3d and 4x4 arrays.
         box_element = shapes.Box([1.0, 1.0, 1.0])
@@ -581,3 +600,66 @@ def test_rigid_body_tree_programmatic_construction(self):
         self.assertIsNotNone(
             rbt.FindCollisionElement(
                 body_2.get_collision_element_ids()[0]))
+
+    def test_rigid_body_tree_collision_api(self):
+        # This test creates a simple RBT with two spheres in simple
+        # collision and verifies collision-detection APIs.
+        rbt = RigidBodyTree()
+        world_body = rbt.world()
+
+        # Both bodies are unit-diameter spheres
+        # spaced 0.5m from each other along the x axis.
+        for k in range(2):
+            body = RigidBody()
+            body.set_name("body_{}".format(k))
+            joint = PrismaticJoint("x", np.eye(4),
+                                   np.array([1.0, 0., 0.]))
+            body.add_joint(world_body, joint)
+            body.set_spatial_inertia(np.eye(6))
+            rbt.add_rigid_body(body)
+
+            sphere_element = shapes.Sphere(0.5)
+            sphere_collision_element = CollisionElement(
+                sphere_element, np.eye(4))
+            sphere_collision_element.set_body(body)
+            rbt.addCollisionElement(sphere_collision_element, body, "default")
+
+        rbt.compile()
+        self.assertTrue(rbt.initialized())
+        self.assertEqual(rbt.get_num_positions(), 2)
+
+        q0 = np.array([-0.25, 0.25])
+        kinsol = rbt.doKinematics(q0)
+        # One point in each region of overlap:
+        # Sphere one is (), sphere two is []
+        # p0  (  p2  [  p3  ) p4 ] p5
+        points = np.array([[-1., -0.6, 0., 0.6, 1.],
+                           [0., 0., 0., 0., 0],
+                           [0., 0., 0., 0., 0]])
+        n_pts = points.shape[1]
+
+        phi, normal, x, body_x, body_idx = rbt.collisionDetectFromPoints(
+            cache=kinsol, points=points, use_margins=False)
+        # Check phi, but leave the other fields as API checks.
+        self.assertTrue(
+            np.allclose(phi, np.array([0.25, -0.15, -0.25, -0.15, 0.25])))
+        self.assertEqual(phi.shape[0], n_pts)
+        self.assertEqual(normal.shape, (3, n_pts))
+        self.assertEqual(x.shape, (3, n_pts))
+        self.assertEqual(normal.shape, (3, n_pts))
+
+        point_pairs = rbt.ComputeMaximumDepthCollisionPoints(
+            cache=kinsol, use_margins=False,
+            throw_if_missing_gradient=True)
+        self.assertEqual(len(point_pairs), 1)
+        pp = point_pairs[0]
+        self.assertEqual(rbt.FindCollisionElement(pp.idA), pp.elementA)
+        self.assertEqual(rbt.FindCollisionElement(pp.idB), pp.elementB)
+        possible_points = [np.array([0.5, 0., 0.]), np.array([-0.5, 0.0, 0.0])]
+        for pt in [pp.ptA, pp.ptB]:
+            self.assertTrue(
+                np.allclose(pt, np.array([0.5, 0., 0.])) or
+                np.allclose(pt, np.array([-0.5, 0., 0.])))
+        self.assertTrue(
+            np.allclose(np.abs(pp.normal), np.array([1., 0., 0.])))
+        self.assertEqual(pp.distance, -0.5)