Add bindings for PlanarJoint

RobotLocomotion · Aug 4, 2020 · 96386f4 · 96386f4
1 parent 3cb51df
commit 96386f4
Show file tree

Hide file tree

Showing 2 changed files with 73 additions and 0 deletions.
diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py
@@ -26,6 +26,7 @@
     LinearSpringDamper_,
     ModelInstanceIndex,
     MultibodyForces_,
+    PlanarJoint_,
     PrismaticJoint_,
     RevoluteJoint_,
     RevoluteSpring_,
@@ -1230,6 +1231,14 @@ def make_ball_rpy_joint(plant, P, C):
                 damping=damping,
             )
 
+        def make_planar_joint(plant, P, C):
+            return PlanarJoint_[T](
+                name="planar",
+                frame_on_parent=P,
+                frame_on_child=C,
+                damping=[1., 2., 3.],
+            )
+
         def make_prismatic_joint(plant, P, C):
             return PrismaticJoint_[T](
                 name="prismatic",
@@ -1268,6 +1277,7 @@ def make_weld_joint(plant, P, C):
 
         make_joint_list = [
             make_ball_rpy_joint,
+            make_planar_joint,
             make_prismatic_joint,
             make_revolute_joint,
             make_universal_joint,
@@ -1311,6 +1321,31 @@ def make_weld_joint(plant, P, C):
                     set_point)
                 joint.set_random_angles_distribution(
                     uniform_random * np.array([1., 1., 1.]))
+            elif joint.name() == "planar":
+                self.assertEqual(len(joint.damping()), 3)
+                set_translation = array_T([1., 2.])
+                set_angle = T(3.)
+                joint.set_translation(context=context,
+                                      p_FoMo_F=set_translation)
+                numpy_compare.assert_equal(
+                    joint.get_translation(context=context), set_translation)
+                joint.set_rotation(context=context, theta=set_angle)
+                numpy_compare.assert_equal(joint.get_rotation(context=context),
+                                           set_angle)
+                joint.set_pose(context=context, p_FoMo_F=set_translation,
+                               theta=set_angle)
+                joint.set_translational_velocity(context=context,
+                                                 v_FoMo_F=set_translation)
+                numpy_compare.assert_equal(
+                    joint.get_translational_velocity(context=context),
+                    set_translation)
+                joint.set_angular_velocity(context=context,
+                                           theta_dot=set_angle)
+                numpy_compare.assert_equal(
+                    joint.get_angular_velocity(context=context), set_angle)
+                joint.set_random_pose_distribution(
+                    p_FoMo_F=uniform_random * np.array([1., 1.]),
+                    theta=uniform_random)
             elif joint.name() == "prismatic":
                 self.assertEqual(joint.damping(), damping)
                 numpy_compare.assert_equal(joint.translation_axis(), x_axis)

diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc
@@ -22,6 +22,7 @@
 #include "drake/multibody/tree/multibody_forces.h"
 #include "drake/multibody/tree/multibody_tree.h"  // `JacobianWrtVariable`
 #include "drake/multibody/tree/multibody_tree_indexes.h"
+#include "drake/multibody/tree/planar_joint.h"
 #include "drake/multibody/tree/prismatic_joint.h"
 #include "drake/multibody/tree/revolute_joint.h"
 #include "drake/multibody/tree/revolute_spring.h"
@@ -307,6 +308,43 @@ void DoScalarDependentDefinitions(py::module m, T) {
             cls_doc.set_angular_velocity.doc);
   }
 
+  // PlanarJoint
+  {
+    using Class = PlanarJoint<T>;
+    constexpr auto& cls_doc = doc.PlanarJoint;
+    auto cls = DefineTemplateClassWithDefault<Class, Joint<T>>(
+        m, "PlanarJoint", param, cls_doc.doc);
+    cls  // BR
+        .def(py::init<const string&, const Frame<T>&, const Frame<T>&,
+                 Vector3<double>>(),
+            py::arg("name"), py::arg("frame_on_parent"),
+            py::arg("frame_on_child"), py::arg("damping"), cls_doc.ctor.doc)
+        .def("damping", &Class::damping, cls_doc.damping.doc)
+        .def("get_translation", &Class::get_translation, py::arg("context"),
+            cls_doc.get_translation.doc)
+        .def("set_translation", &Class::set_translation, py::arg("context"),
+            py::arg("p_FoMo_F"), cls_doc.set_translation.doc)
+        .def("get_rotation", &Class::get_rotation, py::arg("context"),
+            cls_doc.get_rotation.doc)
+        .def("set_rotation", &Class::set_rotation, py::arg("context"),
+            py::arg("theta"), cls_doc.set_rotation.doc)
+        .def("set_pose", &Class::set_pose, py::arg("context"),
+            py::arg("p_FoMo_F"), py::arg("theta"), cls_doc.set_pose.doc)
+        .def("get_translational_velocity", &Class::get_translational_velocity,
+            py::arg("context"), cls_doc.get_translational_velocity.doc)
+        .def("set_translational_velocity", &Class::set_translational_velocity,
+            py::arg("context"), py::arg("v_FoMo_F"),
+            cls_doc.set_translational_velocity.doc)
+        .def("get_angular_velocity", &Class::get_angular_velocity,
+            py::arg("context"), cls_doc.get_angular_velocity.doc)
+        .def("set_angular_velocity", &Class::set_angular_velocity,
+            py::arg("context"), py::arg("theta_dot"),
+            cls_doc.set_angular_velocity.doc)
+        .def("set_random_pose_distribution",
+            &Class::set_random_pose_distribution, py::arg("p_FoMo_F"),
+            py::arg("theta"), cls_doc.set_random_pose_distribution.doc);
+  }
+
   // PrismaticJoint
   {
     using Class = PrismaticJoint<T>;