Add return_angular_velocity option for rotation_angle (inverse rodrig…

…ues) (#312)

skrobot/coordinates/math.py

@@ -1078,7 +1078,7 @@ def rodrigues(axis, theta=None):
     return mat
 
 
-def rotation_angle(mat):
+def rotation_angle(mat, return_angular_velocity=False):
     """Inverse Rodrigues formula Convert Rotation-Matirx to Axis-Angle.
 
     Return theta and axis.
@@ -1088,11 +1088,16 @@ def rotation_angle(mat):
     ----------
     mat : numpy.ndarray
         rotation matrix, shape (3, 3)
+    return_angular_velocity : bool, optional
+        If True, returns the angular velocity vector
+        instead of the axis-angle representation.
 
     Returns
     -------
-    theta, axis : tuple(float, numpy.ndarray)
-        rotation angle in radian and rotation axis
+    theta, axis or angular_velocity : tuple(float, numpy.ndarray)
+    or numpy.ndarray
+        Rotation angle in radian and rotation axis,
+        or angular velocity vector if `return_angular_velocity` is True.
 
     Examples
     --------
@@ -1105,7 +1110,10 @@ def rotation_angle(mat):
     """
     mat = _check_valid_rotation(mat)
     if np.array_equal(mat, np.eye(3)):
-        return None
+        if return_angular_velocity:
+            return None
+        else:
+            return None, None
     theta = np.arccos((np.trace(mat) - 1) / 2)
     if abs(theta) < _EPS:
         raise ValueError('Rotation Angle is too small. \nvalue : {}'.
@@ -1114,6 +1122,11 @@ def rotation_angle(mat):
         * np.array([mat[2, 1] - mat[1, 2],
                     mat[0, 2] - mat[2, 0],
                     mat[1, 0] - mat[0, 1]])
+
+    if return_angular_velocity:
+        angular_velocity = axis * theta
+        return angular_velocity
+
     return theta, axis
 
 

tests/skrobot_tests/coordinates_tests/test_math.py

@@ -206,7 +206,22 @@ def test_rotation_angle(self):
         with self.assertRaises(ValueError):
             rotation_angle(rot)
 
-        self.assertEqual(rotation_angle(np.eye(3)), None)
+        # Coordinates().rotate(np.pi / 2.0, 'y').rotation
+        rot = [[0, 0, 1],
+               [0, 1, 0],
+               [-1, 0, 0]]
+        theta, axis = rotation_angle(rot)
+        testing.assert_array_almost_equal(theta, np.pi / 2.0)
+        testing.assert_array_almost_equal(axis, [0, 1, 0])
+        angular_velocity_vector = rotation_angle(
+            rot, return_angular_velocity=True)
+        testing.assert_array_almost_equal(
+            angular_velocity_vector, [0, np.pi / 2.0, 0])
+
+        self.assertEqual(rotation_angle(np.eye(3)), (None, None))
+        self.assertEqual(
+            rotation_angle(np.eye(3), return_angular_velocity=True),
+            None)
 
     def test_outer_product_matrix(self):
         testing.assert_array_equal(outer_product_matrix([1, 2, 3]),