skrobot.coordinates.math.interpolate_rotation_matrices

skrobot.coordinates.math.interpolate_rotation_matrices(p, r1, r2)[source]

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 – Interpolated 3x3 rotation matrix

Return type:

numpy.ndarray

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.])