diff --git a/bindings/pydrake/attic/multibody/collision_py.cc b/bindings/pydrake/attic/multibody/collision_py.cc index 2307945641a9..8e770ea0e6c4 100644 --- a/bindings/pydrake/attic/multibody/collision_py.cc +++ b/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_>(m, "PointPair") + .def_readonly( + "elementA", &PointPair::elementA, py_reference_internal) + .def_readonly( + "elementB", &PointPair::elementB, py_reference_internal) + .def_readonly("idA", &PointPair::idA) + .def_readonly("idB", &PointPair::idB) + .def_readonly("ptA", &PointPair::ptA) + .def_readonly("ptB", &PointPair::ptB) + .def_readonly("normal", &PointPair::normal) + .def_readonly("distance", &PointPair::distance); } } // namespace pydrake diff --git a/bindings/pydrake/attic/multibody/joints_py.cc b/bindings/pydrake/attic/multibody/joints_py.cc index c1c3c4f82429..623e39a60e5f 100644 --- a/bindings/pydrake/attic/multibody/joints_py.cc +++ b/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_(m, "FixedJoint", doc.FixedJoint.doc) + .def(py::init(), + py::arg("name"), py::arg("transform_to_parent_body"), + doc.FixedJoint.ctor.doc); + py::class_( m, "RevoluteJoint", doc.RevoluteJoint.doc) .def(py::init(), py::arg("name"), py::arg("transform_to_parent_body"), py::arg("rotation_axis"), doc.RevoluteJoint.ctor.doc); + + py::class_( + m, "RollPitchYawFloatingJoint", doc.RollPitchYawFloatingJoint.doc) + .def(py::init(), + py::arg("name"), py::arg("transform_to_parent_body"), + doc.RollPitchYawFloatingJoint.ctor.doc); } } // namespace pydrake diff --git a/bindings/pydrake/attic/multibody/rigid_body_tree_py.cc b/bindings/pydrake/attic/multibody/rigid_body_tree_py.cc index c982cc09dbca..e0da488ade9c 100644 --- a/bindings/pydrake/attic/multibody/rigid_body_tree_py.cc +++ b/bindings/pydrake/attic/multibody/rigid_body_tree_py.cc @@ -185,6 +185,22 @@ PYBIND11_MODULE(rigid_body_tree, m) { .def("DefineCollisionFilterGroup", &RigidBodyTree::DefineCollisionFilterGroup, py::arg("name"), doc.RigidBodyTree.DefineCollisionFilterGroup.doc) + .def("collisionDetectFromPoints", + [](RigidBodyTree& tree, const KinematicsCache& cache, + const Eigen::Matrix3Xd& points, bool use_margins) { + Eigen::VectorXd phi; + Eigen::Matrix3Xd normal; + Eigen::Matrix3Xd x; + Eigen::Matrix3Xd body_x; + std::vector body_idx; + tree.collisionDetectFromPoints( + cache, points, phi, normal, x, body_x, body_idx, use_margins); + return std::tuple>( + phi, normal, x, body_x, body_idx); + }, + py::arg("cache"), py::arg("points"), py::arg("use_margins"), + doc.RigidBodyTree.collisionDetectFromPoints.doc) .def("FindCollisionElement", &RigidBodyTree::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& tree, const VectorX& q, const VectorX& v) { return tree.doKinematics(q, v); }, - doc.RigidBodyTree.doKinematics.doc_3args); - // CreateKinematicsCacheWithType - // ComputeMaximumDepthCollisionPoints + doc.RigidBodyTree.doKinematics.doc_3args) + // CreateKinematicsCacheWithType + .def("ComputeMaximumDepthCollisionPoints", + &RigidBodyTree::ComputeMaximumDepthCollisionPoints, + 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", diff --git a/bindings/pydrake/multibody/test/rigid_body_tree_test.py b/bindings/pydrake/multibody/test/rigid_body_tree_test.py index 616e65fb9ee6..af4125e6bfff 100644 --- a/bindings/pydrake/multibody/test/rigid_body_tree_test.py +++ b/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)