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)