"""Property tests for spatial.py. Run with: python -m pytest -q""" from __future__ import annotations import numpy as np import pytest from scipy.spatial.transform import Rotation from spatial import (R_to_6d, R_to_axis_angle, R_to_quat, R_to_rpy, axis_angle_to_R, is_rotation, make_T, nlerp, quat_angle, quat_conj, quat_from_axis_angle, quat_mul, quat_rotate, quat_to_R, rotx, roty, rotz, rpy_to_R, sixd_to_R, skew, slerp, transform_inverse, transform_point) RNG = np.random.default_rng(2026) def random_quats(n: int) -> np.ndarray: q = RNG.normal(size=(n, 4)) return q / np.linalg.norm(q, axis=1, keepdims=True) QUATS = random_quats(200) ANGLES = RNG.uniform(-np.pi, np.pi, size=(200, 3)) @pytest.mark.parametrize("fn", [rotx, roty, rotz]) def test_elementary_rotations_are_rotations(fn) -> None: for a in np.linspace(-np.pi, np.pi, 37): assert is_rotation(fn(a)) def test_rotz_turns_x_into_y() -> None: assert np.allclose(rotz(np.pi / 2) @ [1, 0, 0], [0, 1, 0]) assert np.allclose(rotx(np.pi / 2) @ [0, 1, 0], [0, 0, 1]) assert np.allclose(roty(np.pi / 2) @ [0, 0, 1], [1, 0, 0]) def test_composition_is_a_rotation_and_not_commutative() -> None: A, B = rotx(0.7), rotz(1.1) assert is_rotation(A @ B) assert not np.allclose(A @ B, B @ A) def test_skew_matches_cross_product() -> None: w, v = RNG.normal(size=3), RNG.normal(size=3) assert np.allclose(skew(w) @ v, np.cross(w, v)) def test_rpy_matches_scipy_extrinsic_xyz() -> None: for r, p, y in ANGLES: expected = Rotation.from_euler("xyz", [r, p, y]).as_matrix() assert np.allclose(rpy_to_R(r, p, y), expected) def test_rpy_round_trip_away_from_gimbal_lock() -> None: for r, p, y in ANGLES: p = np.clip(p, -1.4, 1.4) r2, p2, y2 = R_to_rpy(rpy_to_R(r, p, y)) assert np.allclose([r2, p2, y2], [r, p, y]) def test_gimbal_lock_loses_one_degree_of_freedom() -> None: # At pitch = +90 deg only (roll - yaw) matters. d = np.pi / 180 R1 = rpy_to_R(10 * d, 90 * d, 30 * d) R2 = rpy_to_R(40 * d, 90 * d, 60 * d) assert np.allclose(R1, R2) # R_to_rpy still returns angles that rebuild the same matrix. assert np.allclose(rpy_to_R(*R_to_rpy(R1)), R1) def test_rodrigues_matches_elementary_rotations() -> None: for a in np.linspace(-3.0, 3.0, 13): assert np.allclose(axis_angle_to_R([0, 0, 1], a), rotz(a)) assert np.allclose(axis_angle_to_R([1, 0, 0], a), rotx(a)) def test_rodrigues_matches_scipy_rotvec() -> None: for _ in range(50): axis = RNG.normal(size=3) axis /= np.linalg.norm(axis) angle = RNG.uniform(0, np.pi) expected = Rotation.from_rotvec(axis * angle).as_matrix() assert np.allclose(axis_angle_to_R(axis, angle), expected) def test_axis_angle_round_trip() -> None: axis = np.array([1.0, 2.0, -2.0]) / 3.0 k, a = R_to_axis_angle(axis_angle_to_R(axis, 2.0)) assert a == pytest.approx(2.0) assert np.allclose(k, axis) def test_quat_to_R_is_rotation() -> None: for q in QUATS: assert is_rotation(quat_to_R(q)) def test_quat_round_trip_up_to_sign() -> None: for q in QUATS: q2 = R_to_quat(quat_to_R(q)) assert np.allclose(q2, q) or np.allclose(q2, -q) assert q2[0] >= 0.0 def test_R_to_quat_matches_scipy() -> None: for q in QUATS: R = quat_to_R(q) xyzw = Rotation.from_matrix(R).as_quat() # SciPy: scalar last wxyz = np.roll(xyzw, 1) assert np.allclose(R_to_quat(R), wxyz) or np.allclose(R_to_quat(R), -wxyz) def test_R_to_quat_near_180_degrees() -> None: for axis in ([1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0]): R = axis_angle_to_R(axis, np.pi - 1e-9) assert np.allclose(quat_to_R(R_to_quat(R)), R, atol=1e-8) def test_q_and_minus_q_are_the_same_rotation() -> None: for q in QUATS[:20]: assert np.allclose(quat_to_R(q), quat_to_R(-q)) def test_quat_mul_matches_matrix_product() -> None: for p, q in zip(QUATS[:100], QUATS[100:]): assert np.allclose(quat_to_R(quat_mul(p, q)), quat_to_R(p) @ quat_to_R(q)) def test_quat_mul_identity_and_inverse() -> None: e = np.array([1.0, 0, 0, 0]) for q in QUATS[:20]: assert np.allclose(quat_mul(e, q), q) assert np.allclose(quat_mul(q, quat_conj(q)), e) def test_quat_rotate_matches_matrix() -> None: for q in QUATS[:50]: v = RNG.normal(size=3) assert np.allclose(quat_rotate(q, v), quat_to_R(q) @ v) def test_slerp_endpoints() -> None: for q0, q1 in zip(QUATS[:50], QUATS[50:100]): a, b = slerp(q0, q1, 0.0), slerp(q0, q1, 1.0) assert np.allclose(quat_to_R(a), quat_to_R(q0)) assert np.allclose(quat_to_R(b), quat_to_R(q1)) def test_slerp_stays_on_unit_sphere_and_has_constant_speed() -> None: for q0, q1 in zip(QUATS[:30], QUATS[30:60]): total = quat_angle(q0, q1) for t in np.linspace(0, 1, 11): q = slerp(q0, q1, t) assert np.linalg.norm(q) == pytest.approx(1.0) assert quat_angle(q0, q) == pytest.approx(t * total, abs=1e-7) def test_slerp_takes_short_path() -> None: q0 = np.array([1.0, 0, 0, 0]) q1 = -quat_from_axis_angle([0, 0, 1], 0.5) # same rotation as +0.5 rad, opposite sign mid = slerp(q0, q1, 0.5) assert quat_angle(q0, mid) == pytest.approx(0.25) def test_nlerp_speed_is_not_constant() -> None: q0 = np.array([1.0, 0, 0, 0]) q1 = quat_from_axis_angle([1, 0, 0], np.deg2rad(150)) assert abs(quat_angle(q0, nlerp(q0, q1, 0.25)) - 0.25 * np.deg2rad(150)) > np.deg2rad(2) assert quat_angle(q0, nlerp(q0, q1, 0.5)) == pytest.approx(0.5 * np.deg2rad(150)) def test_transform_inverse() -> None: for q in QUATS[:30]: T = make_T(quat_to_R(q), RNG.normal(size=3)) assert np.allclose(T @ transform_inverse(T), np.eye(4)) assert np.allclose(transform_inverse(T), np.linalg.inv(T)) def test_transform_chain_world_base_camera() -> None: d = np.pi / 180 T_WB = make_T(rotz(90 * d), [0.5, 0.2, 0.0]) T_BC = make_T(rpy_to_R(-90 * d, 0.0, -90 * d), [0.1, 0.0, 0.4]) # Camera optical axis (z) points along base x; camera x along -base y; camera y along -base z. assert np.allclose(T_BC[:3, :3], [[0, 0, 1], [-1, 0, 0], [0, -1, 0]]) p_W = transform_point(T_WB @ T_BC, [0.0, 0.0, 0.6]) assert np.allclose(p_W, [0.5, 0.9, 0.4]) def test_6d_round_trip() -> None: for q in QUATS[:50]: R = quat_to_R(q) assert np.allclose(sixd_to_R(R_to_6d(R)), R) def test_6d_maps_any_vector_to_a_rotation() -> None: for _ in range(100): assert is_rotation(sixd_to_R(RNG.normal(size=6))) def test_6d_is_continuous_where_euler_jumps() -> None: # Yaw passing through 180 deg: the angle jumps by about 2 pi, the 6D vector does not. eps = 1e-4 Ra, Rb = rotz(np.pi - eps), rotz(-np.pi + eps) assert abs(R_to_rpy(Ra)[2] - R_to_rpy(Rb)[2]) > 6.0 assert np.linalg.norm(R_to_6d(Ra) - R_to_6d(Rb)) < 1e-3