def test_matrix_exponent(self):
m1 = rotate_matrix(
rotate_matrix(rotate_matrix(np.eye(3), 0.2, 'x'), 0.4, 'y'), 0.6,
'z')
testing.assert_almost_equal(matrix_exponent(matrix_log(m1)),
m1,
decimal=5)
def difference_rotation(self, coords, rotation_axis=True):
"""Return differences in rotation of given coords.
Parameters
----------
coords : skrobot.coordinates.Coordinates
given coordinates
rotation_axis : str or bool or None (optional)
we can take 'x', 'y', 'z', 'xx', 'yy', 'zz', 'xm', 'ym', 'zm',
'xy', 'yx', 'yz', 'zy', 'zx', 'xz', True or False(None).
Returns
-------
dif_rot : numpy.ndarray
difference rotation of self coordinates and coords
considering rotation_axis.
Examples
--------
>>> from numpy import pi
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates.math import rpy_matrix
>>> coord1 = Coordinates()
>>> coord2 = Coordinates(rot=rpy_matrix(pi / 2.0, pi / 3.0, pi / 5.0))
>>> coord1.difference_rotation(coord2)
array([-0.32855112, 1.17434985, 1.05738936])
>>> coord1.difference_rotation(coord2, rotation_axis=False)
array([0, 0, 0])
>>> coord1.difference_rotation(coord2, rotation_axis='x')
array([0. , 1.36034952, 0.78539816])
>>> coord1.difference_rotation(coord2, rotation_axis='y')
array([0.35398131, 0. , 0.97442695])
>>> coord1.difference_rotation(coord2, rotation_axis='z')
array([-0.88435715, 0.74192175, 0. ])
Using mirror option ['xm', 'ym', 'zm'], you can
allow differences of mirror direction.
>>> coord1 = Coordinates()
>>> coord2 = Coordinates().rotate(pi, 'x')
>>> coord1.difference_rotation(coord2, 'xm')
array([-2.99951957e-32, 0.00000000e+00, 0.00000000e+00])
>>> coord1 = Coordinates()
>>> coord2 = Coordinates().rotate(pi / 2.0, 'x')
>>> coord1.difference_rotation(coord2, 'xm')
array([-1.57079633, 0. , 0. ])
"""
def need_mirror_for_nearest_axis(coords0, coords1, ax):
a0 = coords0.axis(ax)
a1 = coords1.axis(ax)
a1_mirror = -a1
dr1 = angle_between_vectors(a0, a1, normalize=False) \
* normalize_vector(cross_product(a0, a1))
dr1m = angle_between_vectors(a0, a1_mirror, normalize=False) \
* normalize_vector(cross_product(a0, a1_mirror))
return np.linalg.norm(dr1) < np.linalg.norm(dr1m)
if rotation_axis in ['x', 'y', 'z']:
a0 = self.axis(rotation_axis)
a1 = coords.axis(rotation_axis)
if np.abs(np.linalg.norm(np.array(a0) - np.array(a1))) < 0.001:
dif_rot = np.array([0, 0, 0], 'f')
else:
dif_rot = np.matmul(
self.worldrot().T,
angle_between_vectors(a0, a1, normalize=False) *
normalize_vector(cross_product(a0, a1)))
elif rotation_axis in ['xx', 'yy', 'zz']:
ax = rotation_axis[0]
a0 = self.axis(ax)
a2 = coords.axis(ax)
if not need_mirror_for_nearest_axis(self, coords, ax):
a2 = -a2
dif_rot = np.matmul(
self.worldrot().T,
angle_between_vectors(a0, a2, normalize=False) *
normalize_vector(cross_product(a0, a2)))
elif rotation_axis in ['xy', 'yx', 'yz', 'zy', 'zx', 'xz']:
if rotation_axis in ['xy', 'yx']:
ax1 = 'z'
ax2 = 'x'
elif rotation_axis in ['yz', 'zy']:
ax1 = 'x'
ax2 = 'y'
else:
ax1 = 'y'
ax2 = 'z'
a0 = self.axis(ax1)
a1 = coords.axis(ax1)
dif_rot = np.matmul(
self.worldrot().T,
angle_between_vectors(a0, a1, normalize=False) *
normalize_vector(cross_product(a0, a1)))
norm = np.linalg.norm(dif_rot)
if np.isclose(norm, 0.0):
self_coords = self.copy_worldcoords()
else:
self_coords = self.copy_worldcoords().rotate(norm, dif_rot)
a0 = self_coords.axis(ax2)
a1 = coords.axis(ax2)
dif_rot = np.matmul(
self_coords.worldrot().T,
angle_between_vectors(a0, a1, normalize=False) *
normalize_vector(cross_product(a0, a1)))
elif rotation_axis in ['xm', 'ym', 'zm']:
rot = coords.worldrot()
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))
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)
else:
raise ValueError
return dif_rot