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