def test_rotation_angle(self):
rot = rpy_matrix(-1.220e-08, -5.195e-09, 1.333e-09)
with self.assertRaises(ValueError):
rotation_angle(rot)
rot = rpy_matrix(-1.220e-08, -5.195e-09, -1.333e-09)
with self.assertRaises(ValueError):
rotation_angle(rot)
self.assertEqual(rotation_angle(np.eye(3)), None)
def test_rodrigues(self):
mat = rpy_matrix(pi / 6, pi / 5, pi / 3)
theta, axis = rotation_angle(mat)
rec_mat = rodrigues(axis, theta)
testing.assert_array_almost_equal(mat, rec_mat)
mat = rpy_matrix(-pi / 6, -pi / 5, -pi / 3)
theta, axis = rotation_angle(mat)
rec_mat = rodrigues(axis, theta)
testing.assert_array_almost_equal(mat, rec_mat)
# case of theta is None
axis = np.array([np.pi, 0, 0], 'f')
rec_mat = rodrigues(axis)
testing.assert_array_almost_equal(
rpy_angle(rec_mat)[0], np.array([0.0, 0.0, -np.pi], 'f'))
def coordinates_distance(c1, c2, c=None):
if c is None:
c = c1.transformation(c2)
return np.linalg.norm(c.worldpos()), rotation_angle(c.worldrot())[0]