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 test_normalize_vector(self):
testing.assert_almost_equal(
normalize_vector([5, 0, 0]),
np.array([1, 0, 0]))
testing.assert_almost_equal(
np.linalg.norm(normalize_vector([1, 1, 1])),
1.0)
def need_mirror_for_nearest_axis(coords0, coords1, ax):
a0 = coords0.axis(ax)
a1 = coords1.axis(ax)
a1_mirror = -a1
dr1 = np.arccos(np.dot(a0, a1)) * \
normalize_vector(np.cross(a0, a1))
dr1m = np.arccos(np.dot(a0, a1_mirror)) * \
normalize_vector(np.cross(a0, a1_mirror))
return np.linalg.norm(dr1) < np.linalg.norm(dr1m)
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)
def test_mul(self):
qr1 = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd1 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr1)
dq1 = DualQuaternion(qr1, qd1)
qr2 = normalize_vector(np.array([4, 3, 2, 1]))
x, y, z = np.array([3, 2, 1])
qd2 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr2)
dq2 = DualQuaternion(qr2, qd2)
dq = dq1 * dq2
testing.assert_almost_equal(dq.translation, [0, 4, 6])
testing.assert_almost_equal(dq.quaternion.q, [-0.4, 0.2, 0.8, 0.4])
def test_add(self):
qr1 = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd1 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr1)
dq1 = DualQuaternion(qr1, qd1)
qr2 = normalize_vector(np.array([4, 3, 2, 1]))
x, y, z = np.array([3, 2, 1])
qd2 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr2)
dq2 = DualQuaternion(qr2, qd2)
dq = (dq1 + dq2).normalize()
testing.assert_almost_equal(dq.translation, [2.0, 2.6, 2.0])
testing.assert_almost_equal(dq.quaternion.q, [0.5, 0.5, 0.5, 0.5])
def axis(self):
"""Return axis of this quaternion.
Note that this property return normalized axis.
Returns
-------
axis : numpy.ndarray
normalized axis vector
"""
if self.w > 1.0:
q = self.normalized
else:
q = self
# quaternion is normalized
# q.w is less than 1.0, so term always positive.
s = np.sqrt(1 - q.w**2)
if s < 0.001:
axis = q.xyz
else:
axis = q.xyz / s
axis = normalize_vector(axis)
return axis
def test_rotation(self):
qr = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr)
dq = DualQuaternion(qr, qd)
testing.assert_almost_equal(dq.rotation, quaternion2matrix(qr))
def test_angle_between_vectors(self):
v = (1., 1., 1.)
theta = angle_between_vectors(v, v)
testing.assert_almost_equal(theta, 0.0)
unit_v = normalize_vector(v)
theta = angle_between_vectors(unit_v, unit_v, normalize=False)
testing.assert_almost_equal(theta, 0.0)
def test_normalize(self):
qr1 = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd1 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr1)
dq1 = DualQuaternion(qr1, qd1)
dq1.normalize()
testing.assert_almost_equal(dq1.dq, [
0.18257419, 0.36514837, 0.54772256, 0.73029674, -1.82574186, 0.,
0.36514837, 0.18257419
])
def dual_quaternion(self):
"""Property of DualQuaternion
Return DualQuaternion representation of this coordinate.
Returns
-------
DualQuaternion : skrobot.coordinates.dual_quaternion.DualQuaternion
DualQuaternion representation of this coordinate
"""
qr = normalize_vector(self.quaternion)
x, y, z = self.translation
qd = quaternion_multiply(np.array([0, x, y, z]), qr) * 0.5
return DualQuaternion(qr, qd)
def rotate_points(points, a, b):
"""Rotate given points based on a starting and ending vector.
Axis vector k is calculated from the any two nonzero vectors a and b.
Rotated points are calculated from following Rodrigues rotation formula.
.. math::
`P_{rot} = P \\cos \\theta +
(k \\times P) \\sin \\theta + k (k \\cdot P) (1 - \\cos \\theta)`
Parameters
----------
points : numpy.ndarray
Input points. The shape should be (3, ) or (N, 3).
a : numpy.ndarray
nonzero vector.
b : numpy.ndarray
nonzero vector.
Returns
-------
points_rot : numpy.ndarray
rotated points.
"""
if points.ndim == 1:
points = points[None, :]
a = normalize_vector(a)
b = normalize_vector(b)
k = normalize_vector(np.cross(a, b))
theta = angle_between_vectors(a, b, normalize=False)
points_rot = points * np.cos(theta) \
+ np.cross(k, points) * np.sin(theta) \
+ k * np.dot(k, points.T).reshape(-1, 1) * (1 - np.cos(theta))
return points_rot
def test_angle_between_vectors(self):
v = (1., 1., 1.)
theta = angle_between_vectors(v, v)
testing.assert_almost_equal(theta, 0.0)
unit_v = normalize_vector(v)
theta = angle_between_vectors(unit_v, unit_v, normalize=False)
testing.assert_almost_equal(theta, 0.0)
# directed
theta = angle_between_vectors([1, 0, 0], [-1, 0, 0], directed=False)
testing.assert_almost_equal(theta, 0.0)
theta = angle_between_vectors([1, 0, 0], [-1, 0, 0], directed=True)
testing.assert_almost_equal(theta, np.pi)
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
def orient_coords_to_axis(target_coords, v, axis='z', eps=0.005):
"""Orient axis to the direction
Orient axis in target_coords to the direction specified by v.
Parameters
----------
target_coords : skrobot.coordinates.Coordinates
v : list or numpy.ndarray
position of target [x, y, z]
axis : list or string or numpy.ndarray
see _wrap_axis function
eps : float (optional)
eps
Returns
-------
target_coords : skrobot.coordinates.Coordinates
Examples
--------
>>> import numpy as np
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates.geo import orient_coords_to_axis
>>> c = Coordinates()
>>> oriented_coords = orient_coords_to_axis(c, [1, 0, 0])
>>> oriented_coords.translation
array([0., 0., 0.])
>>> oriented_coords.rpy_angle()
(array([0. , 1.57079633, 0. ]),
array([3.14159265, 1.57079633, 3.14159265]))
>>> c = Coordinates(pos=[0, 1, 0])
>>> oriented_coords = orient_coords_to_axis(c, [0, 1, 0])
>>> oriented_coords.translation
array([0., 1., 0.])
>>> oriented_coords.rpy_angle()
(array([ 0. , -0. , -1.57079633]),
array([ 3.14159265, -3.14159265, 1.57079633]))
>>> c = Coordinates(pos=[0, 1, 0]).rotate(np.pi / 3, 'y')
>>> oriented_coords = orient_coords_to_axis(c, [0, 1, 0])
>>> oriented_coords.translation
array([0., 1., 0.])
>>> oriented_coords.rpy_angle()
(array([-5.15256299e-17, 1.04719755e+00, -1.57079633e+00]),
array([3.14159265, 2.0943951 , 1.57079633]))
"""
v = np.array(v, 'f')
if np.linalg.norm(v) == 0.0:
v = np.array([0, 0, 1], 'f')
nv = normalize_vector(v)
axis = _wrap_axis(axis)
ax = target_coords.rotate_vector(axis)
rot_axis = np.cross(ax, nv)
rot_angle_cos = np.clip(np.dot(nv, ax), -1.0, 1.0)
if np.isclose(rot_angle_cos, 1.0, atol=eps):
return target_coords
elif np.isclose(rot_angle_cos, -1.0, atol=eps):
for rot_axis2 in [np.array([1, 0, 0]), np.array([0, 1, 0])]:
rot_angle_cos2 = np.dot(ax, rot_axis2)
if not np.isclose(abs(rot_angle_cos2), 1.0, atol=eps):
rot_axis = rot_axis2 - rot_angle_cos2 * ax
break
target_coords.rotate(
np.arccos(rot_angle_cos), rot_axis, 'world')
return target_coords
