Add bindings for random rotations (RobotLocomotion#13993)

beamiter · Sep 2, 2020 · fa3bfaa · fa3bfaa
1 parent 5d1447f
commit fa3bfaa
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 0 deletions.
diff --git a/bindings/pydrake/math_py.cc b/bindings/pydrake/math_py.cc
@@ -23,6 +23,7 @@
 #include "drake/math/matrix_util.h"
 #include "drake/math/orthonormal_basis.h"
 #include "drake/math/quadratic_form.h"
+#include "drake/math/random_rotation.h"
 #include "drake/math/rigid_transform.h"
 #include "drake/math/roll_pitch_yaw.h"
 #include "drake/math/rotation_matrix.h"
@@ -333,6 +334,25 @@ void DoScalarIndependentDefinitions(py::module m) {
       .def("MeshValuesFrom", &BarycentricMesh<T>::MeshValuesFrom,
           doc.BarycentricMesh.MeshValuesFrom.doc);
 
+  // Random Rotations
+  m  // BR
+      .def("UniformlyRandomQuaternion",
+          overload_cast_explicit<Eigen::Quaternion<T>, RandomGenerator*>(
+              &UniformlyRandomQuaternion),
+          py::arg("generator"), doc.UniformlyRandomQuaternion.doc)
+      .def("UniformlyRandomAngleAxis",
+          overload_cast_explicit<Eigen::AngleAxis<T>, RandomGenerator*>(
+              &UniformlyRandomAngleAxis),
+          py::arg("generator"), doc.UniformlyRandomAngleAxis.doc)
+      .def("UniformlyRandomRotationMatrix",
+          overload_cast_explicit<RotationMatrix<T>, RandomGenerator*>(
+              &UniformlyRandomRotationMatrix),
+          py::arg("generator"), doc.UniformlyRandomRotationMatrix.doc)
+      .def("UniformlyRandomRPY",
+          overload_cast_explicit<Vector3<T>, RandomGenerator*>(
+              &UniformlyRandomRPY),
+          py::arg("generator"), doc.UniformlyRandomRPY.doc);
+
   // Matrix Util.
   m  // BR
       .def(

diff --git a/bindings/pydrake/test/math_test.py b/bindings/pydrake/test/math_test.py
@@ -1,6 +1,7 @@
 import pydrake.math as mut
 import pydrake.math._test as mtest
 from pydrake.math import (BarycentricMesh, wrap_to)
+from pydrake.common import RandomGenerator
 from pydrake.common.cpp_param import List
 from pydrake.common.eigen_geometry import Isometry3_, Quaternion_, AngleAxis_
 from pydrake.common.value import Value
@@ -346,6 +347,18 @@ def test_orthonormal_basis(self):
         self.assertAlmostEqual(np.linalg.det(R), 1.0)
         self.assertTrue(np.allclose(R.dot(R.T), np.eye(3)))
 
+    def test_random_rotations(self):
+        g = RandomGenerator()
+        quat = mut.UniformlyRandomQuaternion(g)
+        self.assertIsInstance(quat, Quaternion_[float])
+        angle_axis = mut.UniformlyRandomAngleAxis(g)
+        self.assertIsInstance(angle_axis, AngleAxis_[float])
+        rot_mat = mut.UniformlyRandomRotationMatrix(g)
+        self.assertIsInstance(rot_mat, mut.RotationMatrix)
+        rpy = mut.UniformlyRandomRPY(g)
+        self.assertIsInstance(rpy, np.ndarray)
+        self.assertEqual(len(rpy), 3)
+
     def test_matrix_util(self):
         A = np.array([[1, 2], [3, 4]])