def rotation(self, rotation): """Set rotation of this coordinate This setter checkes the given rotation and set it this coordinate. Parameters ---------- rotation : list or numpy.ndarray we can take 3x3 rotation matrix or rpy angle [yaw, pitch, roll] or quaternion [w, x, y, z] order """ rotation = np.array(rotation) # Convert quaternions if rotation.shape == (4, ): self._q = np.array([q for q in rotation]) if np.abs(np.linalg.norm(self._q) - 1.0) > 1e-3: raise ValueError('Invalid quaternion. Must be ' 'norm 1.0, get {}'.format( np.linalg.norm(self._q))) rotation = quaternion2matrix(self._q) elif rotation.shape == (3, ): # Convert [yaw-pitch-roll] to rotation matrix self._q = rpy2quaternion(rotation) rotation = quaternion2matrix(self._q) else: self._q = matrix2quaternion(rotation) # Convert lists and tuples if type(rotation) in (list, tuple): rotation = np.array(rotation).astype(np.float32) _check_valid_rotation(rotation) self._rotation = rotation * 1.
def test__check_valid_rotation(self): valid_rotation = np.eye(3) testing.assert_equal(_check_valid_rotation(valid_rotation), valid_rotation) invalid_rotation = np.arange(9).reshape(3, 3) with self.assertRaises(ValueError): _check_valid_rotation(invalid_rotation)
def test_rotation_matrix_from_axis(self): x_axis = (1, 0, 0) y_axis = (0, 1, 0) rot = rotation_matrix_from_axis(x_axis, y_axis) _check_valid_rotation(rot) testing.assert_array_almost_equal(rot, np.eye(3)) x_axis = (1, 1, 1) y_axis = (0, 0, 1) rot = rotation_matrix_from_axis(x_axis, y_axis) testing.assert_array_almost_equal( rot, [[0.57735027, -0.40824829, 0.70710678], [0.57735027, -0.40824829, -0.70710678], [0.57735027, 0.81649658, 0.0]]) x_axis = (1, 1, 1) y_axis = (0, 0, -1) rot = rotation_matrix_from_axis(x_axis, y_axis) _check_valid_rotation(rot) testing.assert_array_almost_equal( rot, [[0.57735027, 0.40824829, -0.70710678], [0.57735027, 0.40824829, 0.70710678], [0.57735027, -0.81649658, 0.0]]) rot = rotation_matrix_from_axis(y_axis, x_axis, axes='yx') _check_valid_rotation(rot)