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 test_quaternion2matrix(self):
testing.assert_array_equal(quaternion2matrix([1, 0, 0, 0]), np.eye(3))
testing.assert_almost_equal(
quaternion2matrix([1.0 / np.sqrt(2), 1.0 / np.sqrt(2), 0, 0]),
np.array([[1., 0., 0.], [0., 0., -1.], [0., 1., 0.]]))
testing.assert_almost_equal(
quaternion2matrix(
normalize_vector(
[1.0, 1 / np.sqrt(2), 1 / np.sqrt(2), 1 / np.sqrt(2)])),
np.array([[0.2000000, -0.1656854, 0.9656854],
[0.9656854, 0.2000000, -0.1656854],
[-0.1656854, 0.9656854, 0.2000000]]))
testing.assert_almost_equal(
quaternion2matrix(
normalize_vector(
[1.0, -1 / np.sqrt(2), 1 / np.sqrt(2), -1 / np.sqrt(2)])),
np.array([[0.2000000, 0.1656854, 0.9656854],
[-0.9656854, 0.2000000, 0.1656854],
[-0.1656854, -0.9656854, 0.2000000]]))
testing.assert_almost_equal(
quaternion2matrix([0.925754, 0.151891, 0.159933, 0.307131]),
rotate_matrix(
rotate_matrix(rotate_matrix(np.eye(3), 0.2, 'x'), 0.4, 'y'),
0.6, 'z'),
decimal=5)
def rotate(self, theta, axis, wrt='local'):
"""Rotate this coordinate.
Rotate this coordinate relative to axis by theta radians
with respect to wrt.
Parameters
----------
theta : float
radian
axis : str or numpy.ndarray
'x', 'y', 'z' or vector
wrt : str or Coordinates
Returns
-------
self
"""
if isinstance(axis, list) or isinstance(axis, np.ndarray):
return self.rotate_with_matrix(rotation_matrix(theta, axis), wrt)
if isinstance(axis, np.ndarray) and axis.shape == (3, 3):
return self.rotate_with_matrix(theta, wrt)
if wrt == 'local' or wrt == self:
self.rotation = rotate_matrix(self.rotation, theta, axis)
return self.newcoords(self.rotation, self.translation)
elif wrt == 'parent' or wrt == self.parent:
self.rotation = rotate_matrix(self.rotation, theta, axis)
return self.newcoords(self.rotation, self.translation)
else:
return self.rotate_with_matrix(rotation_matrix(theta, axis), wrt)
def rotate(self, theta, axis=None, wrt='local'):
"""Rotate this coordinate by given theta and axis.
Parameters
----------
theta : float
radian
wrt : string or skrobot.coordinates.Coordinates
Returns
-------
self : skrobot.coordinates.Coordinates
"""
if isinstance(axis, list) or isinstance(axis, np.ndarray):
self.rotate_with_matrix(rotation_matrix(theta, axis), wrt)
elif axis is None or axis is False:
self.rotate_with_matrix(theta, wrt)
elif wrt == 'local' or wrt == self:
self.rotation = rotate_matrix(self.rotation, theta, axis)
elif wrt == 'parent' or wrt == 'world':
self.rotation = rotate_matrix(self.rotation, theta, axis, True)
elif isinstance(wrt, Coordinates): # C1'=C2*R*C2(-1)*C1
self.rotate_with_matrix(rotation_matrix(theta, axis), wrt)
else:
raise ValueError('wrt {} not supported'.format(wrt))
return self.newcoords(self.rotation, self.translation)
def test_midrot(self):
m1 = rotate_matrix(rotate_matrix(rotate_matrix(
np.eye(3), 0.2, 'x'), 0.4, 'y'), 0.6, 'z')
testing.assert_almost_equal(
midrot(0.5, m1, np.eye(3)),
np.array([[0.937735, -0.294516, 0.184158],
[0.319745, 0.939037, -0.126384],
[-0.135709, 0.177398, 0.974737]]),
decimal=5)
def rotate(self, theta, axis=None, wrt='local'):
"""Rotate this coordinate by given theta and axis.
This coordinate system is rotated relative to theta radians
around the `axis` axis.
Note that this function does not change a position of this coordinate.
If you want to rotate this coordinates around with world frame,
you can use `transform` function.
Please see examples.
Parameters
----------
theta : float
relartive rotation angle in radian.
axis : str or None or numpy.ndarray
axis of rotation.
The value of `axis` is represented as `wrt` frame.
wrt : str or skrobot.coordinates.Coordinates
Returns
-------
self : skrobot.coordinates.Coordinates
Examples
--------
>>> from skrobot.coordinates import Coordinates
>>> from numpy import pi
>>> c = Coordinates()
>>> c.translate((1.0, 0, 0))
>>> c.rotate(pi / 2.0, 'z', wrt='local')
>>> c.translation
array([1., 0., 0.])
>>> c.transform(Coordinates().rotate(np.pi / 2.0, 'z'), wrt='world')
>>> c.translation
array([0., 1., 0.])
"""
if isinstance(axis, list) or isinstance(axis, np.ndarray):
self.rotate_with_matrix(rotation_matrix(theta, axis), wrt)
elif axis is None or axis is False:
self.rotate_with_matrix(theta, wrt)
elif wrt == 'local' or wrt == self:
self.rotation = rotate_matrix(self.rotation, theta, axis)
elif wrt == 'parent' or wrt == 'world':
self.rotation = rotate_matrix(self.rotation, theta, axis, True)
elif isinstance(wrt, Coordinates): # C1'=C2*R*C2(-1)*C1
self.rotate_with_matrix(rotation_matrix(theta, axis), wrt)
else:
raise ValueError('wrt {} not supported'.format(wrt))
return self.newcoords(self.rotation, self.translation)
def test_matrix2quaternion(self):
testing.assert_almost_equal(matrix2quaternion(np.eye(3)),
np.array([1, 0, 0, 0]))
m = rotate_matrix(
rotate_matrix(rotate_matrix(np.eye(3), 0.2, 'x'), 0.4, 'y'), 0.6,
'z')
testing.assert_almost_equal(quaternion2matrix(matrix2quaternion(m)), m)
testing.assert_almost_equal(matrix2quaternion(
np.array([[0.428571, 0.514286, -0.742857],
[-0.857143, -0.028571, -0.514286],
[-0.285714, 0.857143, 0.428571]])),
normalize_vector(np.array([4, 3, -1, -3])),
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
