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