diff --git a/src/presa_mano/geometry.py b/src/presa_mano/geometry.py index 17dd8e3..b675725 100644 --- a/src/presa_mano/geometry.py +++ b/src/presa_mano/geometry.py @@ -109,3 +109,24 @@ def transform_points(T: np.ndarray, pts: np.ndarray) -> np.ndarray: R = T[:3, :3] t = T[:3, 3] return pts @ R.T + t + + +def rpy_deg_to_quaternion_xyzw(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray: + """Extrinsic XYZ Euler (deg) -> quaternion xyzw. Inverse of `quaternion_xyzw_to_rpy_deg`. + Convenzione coerente con `R_to_euler_xyz` (XYZ extrinsic, in radianti).""" + cr, sr = np.cos(np.radians(roll_deg) * 0.5), np.sin(np.radians(roll_deg) * 0.5) + cp, sp = np.cos(np.radians(pitch_deg) * 0.5), np.sin(np.radians(pitch_deg) * 0.5) + cy, sy = np.cos(np.radians(yaw_deg) * 0.5), np.sin(np.radians(yaw_deg) * 0.5) + qw = cr * cp * cy + sr * sp * sy + qx = sr * cp * cy - cr * sp * sy + qy = cr * sp * cy + sr * cp * sy + qz = cr * cp * sy - sr * sp * cy + return np.array([qx, qy, qz, qw], dtype=np.float64) + + +def quaternion_xyzw_to_rpy_deg(quat_xyzw) -> tuple[float, float, float]: + """Quaternion xyzw -> extrinsic XYZ Euler (deg). Convention matches `R_to_euler_xyz`.""" + R = quat_xyzw_to_R(np.asarray(quat_xyzw, dtype=np.float64)) + rpy_rad = R_to_euler_xyz(R) + rpy_deg = np.degrees(rpy_rad) + return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2]) diff --git a/tests/test_geometry_rpy.py b/tests/test_geometry_rpy.py new file mode 100644 index 0000000..6cb7887 --- /dev/null +++ b/tests/test_geometry_rpy.py @@ -0,0 +1,40 @@ +"""Roundtrip RPY (deg) ↔ quaternion xyzw.""" +from __future__ import annotations + +import numpy as np +import pytest + +from presa_mano.geometry import ( + quaternion_xyzw_to_rpy_deg, + rpy_deg_to_quaternion_xyzw, +) + + +def test_zero_rpy_is_identity_quat(): + q = rpy_deg_to_quaternion_xyzw(0.0, 0.0, 0.0) + assert np.allclose(q, [0.0, 0.0, 0.0, 1.0], atol=1e-9) + + +def test_quat_identity_is_zero_rpy(): + rpy = quaternion_xyzw_to_rpy_deg([0.0, 0.0, 0.0, 1.0]) + assert np.allclose(rpy, [0.0, 0.0, 0.0], atol=1e-6) + + +@pytest.mark.parametrize("roll,pitch,yaw", [ + (10.0, 20.0, 30.0), + (-45.0, 0.0, 90.0), + (0.0, 60.0, -30.0), + (15.0, -25.0, 80.0), +]) +def test_roundtrip_rpy_quat_rpy(roll, pitch, yaw): + q = rpy_deg_to_quaternion_xyzw(roll, pitch, yaw) + r2, p2, y2 = quaternion_xyzw_to_rpy_deg(q) + assert abs(r2 - roll) < 1e-4 + assert abs(p2 - pitch) < 1e-4 + assert abs(y2 - yaw) < 1e-4 + + +def test_quat_unit_norm(): + q = rpy_deg_to_quaternion_xyzw(33.0, -17.0, 91.5) + n = float(np.linalg.norm(q)) + assert abs(n - 1.0) < 1e-9