Add bindings for UniversalJoint

RobotLocomotion · May 8, 2020 · 42e9533 · 42e9533
1 parent 38eda89
commit 42e9533
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 0 deletions.
diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py
@@ -33,6 +33,7 @@
     SpatialInertia_,
     UniformGravityFieldElement_,
     UnitInertia_,
+    UniversalJoint_,
     WeldJoint_,
     world_index,
     world_model_instance,
@@ -1065,6 +1066,14 @@ def make_revolute_joint(plant, P, C):
                 damping=2.,
             )
 
+        def make_universal_joint(plant, P, C):
+            return UniversalJoint_[T](
+                name="universal",
+                frame_on_parent=P,
+                frame_on_child=C,
+                damping=2.,
+            )
+
         def make_weld_joint(plant, P, C):
             # TODO(eric.cousineau): Update WeldJoint arg names to be consistent
             # with other joints.
@@ -1079,6 +1088,7 @@ def make_weld_joint(plant, P, C):
             make_ball_rpy_joint,
             make_prismatic_joint,
             make_revolute_joint,
+            make_universal_joint,
             make_weld_joint,
         ]
 
@@ -1123,6 +1133,13 @@ def make_weld_joint(plant, P, C):
                 set_point = 1.
                 joint.set_angle(context=context, angle=set_point)
                 self.assertIsInstance(joint.get_angle(context=context), T)
+            elif joint.name() == "universal":
+                set_point = np.array([1., 2.])
+                joint.set_angles(context=context, angles=set_point)
+                self.assertEqual(len(joint.get_angles(context=context)), 2)
+                joint.set_angular_rates(context=context, theta_dot=set_point)
+                self.assertEqual(
+                    len(joint.get_angular_rates(context=context)), 2)
             elif joint.name() == "weld":
                 # No joint specifc methods to test
                 pass

diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc
@@ -26,6 +26,7 @@
 #include "drake/multibody/tree/revolute_joint.h"
 #include "drake/multibody/tree/revolute_spring.h"
 #include "drake/multibody/tree/rigid_body.h"
+#include "drake/multibody/tree/universal_joint.h"
 #include "drake/multibody/tree/weld_joint.h"
 
 namespace drake {
@@ -320,6 +321,30 @@ void DoScalarDependentDefinitions(py::module m, T) {
             cls_doc.set_random_angle_distribution.doc);
   }
 
+  // UniversalJoint
+  {
+    using Class = UniversalJoint<T>;
+    constexpr auto& cls_doc = doc.UniversalJoint;
+    auto cls = DefineTemplateClassWithDefault<Class, Joint<T>>(
+        m, "UniversalJoint", param, cls_doc.doc);
+    cls  // BR
+        .def(
+            py::init<const string&, const Frame<T>&, const Frame<T>&, double>(),
+            py::arg("name"), py::arg("frame_on_parent"),
+            py::arg("frame_on_child"), py::arg("damping") = 0, cls_doc.ctor.doc)
+        .def("get_angles", &Class::get_angles, py::arg("context"),
+            cls_doc.get_angles.doc)
+        .def("set_angles", &Class::set_angles, py::arg("context"),
+            py::arg("angles"), cls_doc.set_angles.doc)
+        .def("get_angular_rates", &Class::get_angular_rates, py::arg("context"),
+            cls_doc.get_angular_rates.doc)
+        .def("set_angular_rates", &Class::set_angular_rates, py::arg("context"),
+            py::arg("theta_dot"), cls_doc.set_angular_rates.doc)
+        .def("set_random_angles_distribution",
+            &Class::set_random_angles_distribution, py::arg("angles"),
+            cls_doc.set_random_angles_distribution.doc);
+  }
+
   // WeldJoint
   {
     using Class = WeldJoint<T>;