feat(geometry): aggiungi conversioni RPY (deg) ↔ quaternion xyzw

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])