def test_action_on_vectors_with_operator():
X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T))
x = np.array([[-1, 0, 1]]).T
vec_expected = np.array([[0, 3, 3]]).T
np.testing.assert_almost_equal(X * x, vec_expected, 14)
def test_adjoint():
np.testing.assert_equal(SE3().adjoint(), np.identity(6))
X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T))
Adj = X.adjoint()
np.testing.assert_almost_equal(Adj[:3, :3], X.rotation.matrix, 14)
np.testing.assert_almost_equal(Adj[3:, 3:], X.rotation.matrix, 14)
np.testing.assert_almost_equal(Adj[:3, 3:], SO3.hat(X.translation) @ X.rotation.matrix, 14)
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_composition():
X = SE3((SO3.rot_x(np.pi / 2), np.array([[1, 2, 3]]).T))
Y = SE3((SO3.rot_z(np.pi / 2), np.array([[0, 1, 0]]).T))
Z = X.compose(Y)
rot_expected = SO3.from_angle_axis(2 * np.pi / 3, np.array([[1, -1, 1]]).T / np.sqrt(3))
t_expected = np.array([[1, 2, 4]]).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_x_works():
rot_90_x = SO3.rot_x(0.5 * np.pi)
expected_matrix = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
np.testing.assert_almost_equal(rot_90_x.matrix, expected_matrix, 14)
