def test_composition_with_operator():
X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T))
Y = SE3((SO3.rot_y(np.pi / 2), np.array([[-1, 0, 1]]).T))
Z = X @ Y
rot_expected = SO3.from_angle_axis(2 * np.pi / 3, np.array([[-1, 1, -1]]).T / np.sqrt(3))
t_expected = np.array([[0, 3, 3]]).T
np.testing.assert_almost_equal(Z.rotation.matrix, rot_expected.matrix, 14)
np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_construct_with_roll_pitch_yaw():
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(np.pi / 2, 0, 0).matrix,
SO3.rot_x(np.pi / 2).matrix, 14)
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(0, np.pi / 2, 0).matrix,
SO3.rot_y(np.pi / 2).matrix, 14)
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(0, 0, np.pi / 2).matrix,
SO3.rot_z(np.pi / 2).matrix, 14)
roll = np.pi
pitch = np.pi / 2
yaw = np.pi
so3 = SO3.from_roll_pitch_yaw(roll, pitch, yaw)
expected = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
np.testing.assert_almost_equal(so3.matrix, expected, 14)
def test_rot_y_works():
rot_90_y = SO3.rot_y(0.5 * np.pi)
expected_matrix = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
np.testing.assert_almost_equal(rot_90_y.matrix, expected_matrix, 14)
