Improve function naming clarity and use skew_symmetric_matrix (#586)

- Replace outer_product_matrix with skew_symmetric_matrix
- Rename matrix_log to rotation_matrix_to_axis_angle_vector
- Rename matrix_exponent to axis_angle_vector_to_rotation_matrix
- Rename midrot to interpolate_rotation_matrices
- Add deprecation warnings for old function names
- Update all internal usages to new function names
- Update documentation to reflect changes

The new names better describe what each function does, improving
code readability and maintainability while preserving backward
compatibility through deprecated aliases.

Author: iory

docs/source/reference/functions.rst

@@ -20,17 +20,16 @@ Utilities functions
    random_rotation
    random_translation
    midpoint
-   midrot
+   interpolate_rotation_matrices
    transform
    rotation_matrix
    rotate_vector
    rotate_matrix
    rpy_matrix
    rpy_angle
    normalize_vector
-   matrix_log
-   matrix_exponent
-   outer_product_matrix
+   rotation_matrix_to_axis_angle_vector
+   axis_angle_vector_to_rotation_matrix
    skew_symmetric_matrix
    rotation_matrix_from_rpy
    rotation_matrix_from_axis
@@ -97,3 +96,17 @@ Geometry functions
    :nosignatures:
 
    rotate_points
+
+
+Deprecated Functions
+--------------------
+
+These functions are deprecated and will be removed in future versions.
+Please use their replacements instead.
+
+**Deprecated Functions:**
+
+- :func:`~skrobot.coordinates.math.matrix_log` → Use :func:`~skrobot.coordinates.math.rotation_matrix_to_axis_angle_vector` instead
+- :func:`~skrobot.coordinates.math.matrix_exponent` → Use :func:`~skrobot.coordinates.math.axis_angle_vector_to_rotation_matrix` instead
+- :func:`~skrobot.coordinates.math.midrot` → Use :func:`~skrobot.coordinates.math.interpolate_rotation_matrices` instead
+- :func:`~skrobot.coordinates.math.outer_product_matrix` → Use :func:`~skrobot.coordinates.math.skew_symmetric_matrix` instead

skrobot/coordinates/base.py

@@ -12,7 +12,6 @@
 from skrobot.coordinates.math import cross_product
 from skrobot.coordinates.math import matrix2quaternion
 from skrobot.coordinates.math import matrix2ypr
-from skrobot.coordinates.math import matrix_log
 from skrobot.coordinates.math import normalize_vector
 from skrobot.coordinates.math import quaternion2matrix
 from skrobot.coordinates.math import quaternion_multiply
@@ -21,6 +20,7 @@
 from skrobot.coordinates.math import rotate_matrix
 from skrobot.coordinates.math import rotation_angle
 from skrobot.coordinates.math import rotation_matrix
+from skrobot.coordinates.math import rotation_matrix_to_axis_angle_vector
 from skrobot.coordinates.math import rpy2quaternion
 from skrobot.coordinates.math import rpy_angle
 from skrobot.coordinates.math import wxyz2xyzw
@@ -969,13 +969,13 @@ def need_mirror_for_nearest_axis(coords0, coords1, ax):
             ax = rotation_axis[0]
             if not need_mirror_for_nearest_axis(self, coords, ax):
                 rot = rotate_matrix(rot, np.pi, ax)
-            dif_rot = matrix_log(np.matmul(self.worldrot().T, rot))
+            dif_rot = rotation_matrix_to_axis_angle_vector(np.matmul(self.worldrot().T, rot))
         elif rotation_axis is False or rotation_axis is None:
             dif_rot = np.array([0, 0, 0])
         elif rotation_axis is True:
             dif_rotmatrix = np.matmul(self.worldrot().T,
                                       coords.worldrot())
-            dif_rot = matrix_log(dif_rotmatrix)
+            dif_rot = rotation_matrix_to_axis_angle_vector(dif_rotmatrix)
         else:
             raise ValueError
         return dif_rot

skrobot/coordinates/geo.py

@@ -3,8 +3,8 @@
 from skrobot.coordinates import make_coords
 from skrobot.coordinates.math import angle_between_vectors
 from skrobot.coordinates.math import convert_to_axis_vector
+from skrobot.coordinates.math import interpolate_rotation_matrices
 from skrobot.coordinates.math import midpoint
-from skrobot.coordinates.math import midrot
 from skrobot.coordinates.math import normalize_vector
 
 
@@ -36,7 +36,7 @@ def midcoords(p, c1, c2):
     array([0.05, 0.  , 0.  ])
     """
     return make_coords(pos=midpoint(p, c1.worldpos(), c2.worldpos()),
-                       rot=midrot(p, c1.worldrot(), c2.worldrot()))
+                       rot=interpolate_rotation_matrices(p, c1.worldrot(), c2.worldrot()))
 
 
 def orient_coords_to_axis(target_coords, v, axis='z', eps=0.005):

skrobot/coordinates/math.py

@@ -446,9 +446,57 @@ def midpoint(p, a, b):
     return a + (b - a) * p
 
 
+def interpolate_rotation_matrices(p, r1, r2):
+    """Interpolate between two rotation matrices.
+
+    Performs spherical linear interpolation (SLERP) between two rotation
+    matrices. This gives a smooth rotation path from r1 to r2.
+
+    Parameters
+    ----------
+    p : float
+        Interpolation parameter in [0, 1]. When p=0, returns r1.
+        When p=1, returns r2. When p=0.5, returns the midpoint rotation.
+    r1 : numpy.ndarray
+        Starting 3x3 rotation matrix
+    r2 : numpy.ndarray
+        Ending 3x3 rotation matrix
+
+    Returns
+    -------
+    r : numpy.ndarray
+        Interpolated 3x3 rotation matrix
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from skrobot.coordinates.math import interpolate_rotation_matrices
+    >>> interpolate_rotation_matrices(0.5,
+            np.eye(3),
+            np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))
+    array([[ 0.70710678,  0.        ,  0.70710678],
+           [ 0.        ,  1.        ,  0.        ],
+           [-0.70710678,  0.        ,  0.70710678]])
+    >>> from skrobot.coordinates.math import matrix2ypr
+    >>> np.rad2deg(matrix2ypr(interpolate_rotation_matrices(0.5,
+                   np.eye(3),
+                   np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))))
+    array([ 0., 45.,  0.])
+    """
+    r1 = _check_valid_rotation(r1)
+    r2 = _check_valid_rotation(r2)
+    r = np.matmul(r1.T, r2)
+    omega = rotation_matrix_to_axis_angle_vector(r)
+    r = axis_angle_vector_to_rotation_matrix(omega, p)
+    return np.matmul(r1, r)
+
+
 def midrot(p, r1, r2):
     """Returns mid (or p) rotation matrix of given two matrix r1 and r2.
 
+    .. deprecated::
+        This function is deprecated. Use `interpolate_rotation_matrices` instead.
+
     Parameters
     ----------
     p : float
@@ -479,12 +527,12 @@ def midrot(p, r1, r2):
                    np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])))[0])
     array([ 0., 45.,  0.])
     """
-    r1 = _check_valid_rotation(r1)
-    r2 = _check_valid_rotation(r2)
-    r = np.matmul(r1.T, r2)
-    omega = matrix_log(r)
-    r = matrix_exponent(omega, p)
-    return np.matmul(r1, r)
+    warnings.warn(
+        'Function `midrot` is deprecated. '
+        'Please use `interpolate_rotation_matrices` instead',
+        DeprecationWarning,
+        stacklevel=2)
+    return interpolate_rotation_matrices(p, r1, r2)
 
 
 def transform(m, v):
@@ -1050,8 +1098,11 @@ def quaternion2matrix(q, normalize=False):
     return m
 
 
-def matrix_log(m):
-    """Returns matrix log of given rotation matrix, it returns [-pi, pi]
+def rotation_matrix_to_axis_angle_vector(m):
+    """Convert rotation matrix to axis-angle representation as a vector.
+
+    The magnitude of the returned vector represents the rotation angle,
+    and its direction represents the rotation axis.
 
     Parameters
     ----------
@@ -1060,14 +1111,16 @@ def matrix_log(m):
 
     Returns
     -------
-    matrixlog : numpy.ndarray
-        vector of shape (3, )
+    axis_angle_vector : numpy.ndarray
+        Axis-angle representation as a vector of shape (3,).
+        The vector's magnitude is the rotation angle in radians [-pi, pi],
+        and its direction is the rotation axis.
 
     Examples
     --------
     >>> import numpy as np
-    >>> from skrobot.coordinates.math import matrix_log
-    >>> matrix_log(np.eye(3))
+    >>> from skrobot.coordinates.math import rotation_matrix_to_axis_angle_vector
+    >>> rotation_matrix_to_axis_angle_vector(np.eye(3))
     array([0., 0., 0.])
     """
     # calc logarithm of quaternion
@@ -1082,9 +1135,84 @@ def matrix_log(m):
     return theta * normalize_vector(q_xyz)
 
 
+def matrix_log(m):
+    """Returns matrix log of given rotation matrix, it returns [-pi, pi]
+
+    .. deprecated::
+        This function is deprecated. Use `rotation_matrix_to_axis_angle_vector` instead.
+
+    Parameters
+    ----------
+    m : list or numpy.ndarray
+        3x3 rotation matrix
+
+    Returns
+    -------
+    matrixlog : numpy.ndarray
+        vector of shape (3, )
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from skrobot.coordinates.math import matrix_log
+    >>> matrix_log(np.eye(3))
+    array([0., 0., 0.])
+    """
+    warnings.warn(
+        'Function `matrix_log` is deprecated. '
+        'Please use `rotation_matrix_to_axis_angle_vector` instead',
+        DeprecationWarning,
+        stacklevel=2)
+    return rotation_matrix_to_axis_angle_vector(m)
+
+
+def axis_angle_vector_to_rotation_matrix(omega, p=1.0):
+    """Convert axis-angle vector representation to rotation matrix.
+
+    Converts an axis-angle representation (where the vector direction
+    is the rotation axis and magnitude is the rotation angle) to a
+    rotation matrix using Rodrigues' formula.
+
+    Parameters
+    ----------
+    omega : list or numpy.ndarray
+        Axis-angle vector of shape (3,). The direction represents
+        the rotation axis and the magnitude represents the rotation
+        angle in radians.
+    p : float, optional
+        Interpolation parameter (default: 1.0). When p=1.0, returns
+        the full rotation. When 0<p<1, returns partial rotation.
+
+    Returns
+    -------
+    rot : numpy.ndarray
+        3x3 rotation matrix
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from skrobot.coordinates.math import axis_angle_vector_to_rotation_matrix
+    >>> axis_angle_vector_to_rotation_matrix([1, 1, 1])
+    array([[ 0.22629564, -0.18300792,  0.95671228],
+           [ 0.95671228,  0.22629564, -0.18300792],
+           [-0.18300792,  0.95671228,  0.22629564]])
+    >>> axis_angle_vector_to_rotation_matrix([1, 0, 0])
+    array([[ 1.        ,  0.        ,  0.        ],
+           [ 0.        ,  0.54030231, -0.84147098],
+           [ 0.        ,  0.84147098,  0.54030231]])
+    """
+    w = np.linalg.norm(omega)
+    amat = skew_symmetric_matrix(normalize_vector(omega))
+    return np.eye(3) + np.sin(w * p) * amat + \
+        (1.0 - np.cos(w * p)) * np.matmul(amat, amat)
+
+
 def matrix_exponent(omega, p=1.0):
     """Returns exponent of given omega.
 
+    .. deprecated::
+        This function is deprecated. Use `axis_angle_vector_to_rotation_matrix` instead.
+
     This function is similar to cv2.Rodrigues.
     Convert rvec (which is log quaternion) to rotation matrix.
 
@@ -1111,10 +1239,12 @@ def matrix_exponent(omega, p=1.0):
            [ 0.        ,  0.54030231, -0.84147098],
            [ 0.        ,  0.84147098,  0.54030231]])
     """
-    w = np.linalg.norm(omega)
-    amat = skew_symmetric_matrix(normalize_vector(omega))
-    return np.eye(3) + np.sin(w * p) * amat + \
-        (1.0 - np.cos(w * p)) * np.matmul(amat, amat)
+    warnings.warn(
+        'Function `matrix_exponent` is deprecated. '
+        'Please use `axis_angle_vector_to_rotation_matrix` instead',
+        DeprecationWarning,
+        stacklevel=2)
+    return axis_angle_vector_to_rotation_matrix(omega, p)
 
 
 def skew_symmetric_matrix(v):

skrobot/model/robot_model.py

@@ -24,12 +24,12 @@
 from skrobot.coordinates import rpy_angle
 from skrobot.coordinates.math import jacobian_inverse
 from skrobot.coordinates.math import matrix2quaternion
-from skrobot.coordinates.math import matrix_log
 from skrobot.coordinates.math import quaternion2matrix
 from skrobot.coordinates.math import quaternion2rpy
 from skrobot.coordinates.math import quaternion_inverse
 from skrobot.coordinates.math import quaternion_multiply
 from skrobot.coordinates.math import rodrigues
+from skrobot.coordinates.math import rotation_matrix_to_axis_angle_vector
 from skrobot.coordinates.math import rpy2quaternion
 from skrobot.model.joint import calc_dif_with_axis
 from skrobot.model.joint import calc_target_joint_dimension
@@ -2881,7 +2881,7 @@ def _batch_ik_step_with_constraints(
 
                 # Calculate error with original target
                 original_error_matrix = np.matmul(current_rot_matrix.T, target_rot_matrix)
-                original_error = matrix_log(original_error_matrix)
+                original_error = rotation_matrix_to_axis_angle_vector(original_error_matrix)
                 original_error_norm = np.linalg.norm(original_error)
 
                 # Calculate error with mirrored target (180° rotation around axis)
@@ -2894,7 +2894,7 @@ def _batch_ik_step_with_constraints(
 
                 mirrored_target_rot = np.matmul(target_rot_matrix, mirror_matrix)
                 mirrored_error_matrix = np.matmul(current_rot_matrix.T, mirrored_target_rot)
-                mirrored_error = matrix_log(mirrored_error_matrix)
+                mirrored_error = rotation_matrix_to_axis_angle_vector(mirrored_error_matrix)
                 mirrored_error_norm = np.linalg.norm(mirrored_error)
 
                 # Use the target orientation that gives smaller error