refactor(pose_3d): move normalize_quaternion to geometry module

src/presa_mano/api/pose_3d_routes.py

@@ -2,7 +2,6 @@
 from __future__ import annotations
 
 import logging
-import math
 from typing import Any, Literal, Optional
 
 from fastapi import APIRouter, HTTPException
@@ -10,6 +9,7 @@ from pydantic import BaseModel, Field
 
 from ..clients import G1Unavailable
 from ..config import get_config
+from ..geometry import normalize_quaternion
 from ..timing import Stopwatch
 from . import _deps
 
@@ -17,16 +17,6 @@ router = APIRouter()
 log = logging.getLogger(__name__)
 
 
-def normalize_quaternion(q: list[float]) -> list[float]:
-    """Normalizza un quaternion a lunghezza unitaria. Solleva ValueError se
-    la norma e' troppo piccola per essere normalizzata in modo sensato
-    (es. quaternion zero, sotto 1e-6)."""
-    n = math.sqrt(sum(float(x)*float(x) for x in q))
-    if n < 1e-6:
-        raise ValueError(f"quaternion is degenerate (norm={n:.3e})")
-    return [float(x) / n for x in q]
-
-
 class WristPose(BaseModel):
     position: list[float]
     quaternion: list[float]

src/presa_mano/geometry.py

@@ -2,6 +2,8 @@
 
 from __future__ import annotations
 
+import math
+
 import numpy as np
 
 
@@ -60,6 +62,16 @@ def R_to_quat_xyzw(R: np.ndarray) -> np.ndarray:
     return np.array([x, y, z, w])
 
 
+def normalize_quaternion(q: list[float]) -> list[float]:
+    """Normalizza un quaternion a lunghezza unitaria. Solleva ValueError se
+    la norma e' troppo piccola per essere normalizzata in modo sensato
+    (es. quaternion zero, sotto 1e-6)."""
+    n = math.sqrt(sum(float(x)*float(x) for x in q))
+    if n < 1e-6:
+        raise ValueError(f"quaternion is degenerate (norm={n:.3e})")
+    return [float(x) / n for x in q]
+
+
 def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
     T = np.eye(4)
     T[:3, :3] = R