From df949c6989174598c907d1718fe84dc18fd36340 Mon Sep 17 00:00:00 2001
From: Adriano Dal Pastro <adrianodalpastro@tielogic.com>
Date: Thu, 21 May 2026 15:37:14 +0200
Subject: [PATCH] refactor(pose_3d): move normalize_quaternion to geometry
 module

---
 src/presa_mano/api/pose_3d_routes.py | 12 +-----------
 src/presa_mano/geometry.py           | 12 ++++++++++++
 tests/test_pose_3d_quaternion.py     |  2 +-
 3 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/src/presa_mano/api/pose_3d_routes.py b/src/presa_mano/api/pose_3d_routes.py
index c3ee8e0..f8a7651 100644
--- a/src/presa_mano/api/pose_3d_routes.py
+++ b/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]
diff --git a/src/presa_mano/geometry.py b/src/presa_mano/geometry.py
index b675725..b58eeeb 100644
--- a/src/presa_mano/geometry.py
+++ b/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
diff --git a/tests/test_pose_3d_quaternion.py b/tests/test_pose_3d_quaternion.py
index c6112a1..419f7cd 100644
--- a/tests/test_pose_3d_quaternion.py
+++ b/tests/test_pose_3d_quaternion.py
@@ -5,7 +5,7 @@ import math
 import numpy as np
 import pytest
 
-from presa_mano.api.pose_3d_routes import normalize_quaternion
+from presa_mano.geometry import normalize_quaternion
 
 
 def test_already_normalized_is_unchanged():