feat(pose_3d): POST /apply + quaternion normalization

src/presa_mano/api/pose_3d_routes.py

@@ -2,6 +2,7 @@
 from __future__ import annotations
 
 import logging
+import math
 from typing import Any, Literal, Optional
 
 from fastapi import APIRouter, HTTPException
@@ -16,6 +17,16 @@ 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]
@@ -28,6 +39,14 @@ class NamedPose(BaseModel):
     note: Optional[str] = None
 
 
+class ApplyReq(BaseModel):
+    side: Literal["left", "right"]
+    position: list[float] = Field(min_length=3, max_length=3)
+    quaternion: list[float] = Field(min_length=4, max_length=4)
+    dry_run: bool = True
+    precise: bool = True
+
+
 @router.get("/state")
 def get_state() -> dict[str, Any]:
     sw = Stopwatch()
@@ -53,3 +72,30 @@ def get_state() -> dict[str, Any]:
         "live_max_step_m": float(cfg.get("live_max_step_m", 0.15)),
         "timings_ms": sw.as_dict(),
     }
+
+
+@router.post("/apply")
+def post_apply(req: ApplyReq) -> dict[str, Any]:
+    sw = Stopwatch()
+    try:
+        quat_norm = normalize_quaternion(req.quaternion)
+    except ValueError as e:
+        raise HTTPException(status_code=422, detail=str(e))
+    try:
+        with sw.step("wrist_absolute"):
+            res = _deps.g1(dry_run=req.dry_run).wrist_absolute(
+                side=req.side,
+                position=list(req.position),
+                quaternion=quat_norm,
+                precise=req.precise,
+            )
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
+    return {
+        "result": res,
+        "applied": not req.dry_run,
+        "side": req.side,
+        "position": list(req.position),
+        "quaternion": quat_norm,
+        "timings_ms": sw.as_dict(),
+    }