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(),
+    }

tests/test_pose_3d_quaternion.py

@@ -0,0 +1,32 @@
+"""Unit test della normalizzazione quaternion server-side."""
+from __future__ import annotations
+
+import math
+import numpy as np
+import pytest
+
+from presa_mano.api.pose_3d_routes import normalize_quaternion
+
+
+def test_already_normalized_is_unchanged():
+    q = [0.0, 0.0, 0.0, 1.0]
+    out = normalize_quaternion(q)
+    assert np.allclose(out, q)
+
+
+def test_not_normalized_gets_normalized_to_unit_length():
+    # scala 2.0
+    q = [0.0, 0.0, 0.0, 2.0]
+    out = normalize_quaternion(q)
+    norm = math.sqrt(sum(x*x for x in out))
+    assert abs(norm - 1.0) < 1e-9
+
+
+def test_degenerate_zero_quat_raises():
+    with pytest.raises(ValueError, match="degenerate"):
+        normalize_quaternion([0.0, 0.0, 0.0, 0.0])
+
+
+def test_near_zero_quat_raises():
+    with pytest.raises(ValueError, match="degenerate"):
+        normalize_quaternion([1e-9, 0.0, 0.0, 1e-9])

tests/test_pose_3d_routes.py

@@ -41,3 +41,79 @@ def test_state_502_when_g1_unavailable():
         g1_factory.return_value = g1
         r = client.get("/api/pose_3d/state")
         assert r.status_code == 502
+
+
+def test_apply_dry_run_calls_g1_with_dry_run_true():
+    captured = {}
+    def _abs(**kw):
+        captured.update(kw)
+        return {"ok": True, "reached_target": True}
+    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
+        g1 = MagicMock()
+        g1.wrist_absolute.side_effect = _abs
+        g1_factory.return_value = g1
+        r = client.post("/api/pose_3d/apply", json={
+            "side": "right",
+            "position": [0.30, -0.20, 0.30],
+            "quaternion": [0, 0, 0, 1],
+            "dry_run": True,
+        })
+        assert r.status_code == 200
+        body = r.json()
+        assert body["applied"] is False
+        # Verifica che il client G1 sia stato creato con dry_run=True
+        # (passato a _deps.g1(dry_run=...))
+        assert g1_factory.call_args.kwargs.get("dry_run") is True
+        assert captured["side"] == "right"
+        assert captured["position"] == [0.30, -0.20, 0.30]
+
+
+def test_apply_real_call():
+    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
+        g1 = MagicMock()
+        g1.wrist_absolute.return_value = {"ok": True, "reached_target": True}
+        g1_factory.return_value = g1
+        r = client.post("/api/pose_3d/apply", json={
+            "side": "left", "position": [0.1, 0.2, 0.3],
+            "quaternion": [0, 0, 0, 1], "dry_run": False,
+        })
+        assert r.status_code == 200
+        body = r.json()
+        assert body["applied"] is True
+        assert g1_factory.call_args.kwargs.get("dry_run") is False
+
+
+def test_apply_422_bad_side():
+    r = client.post("/api/pose_3d/apply", json={
+        "side": "middle", "position": [0,0,0], "quaternion": [0,0,0,1],
+    })
+    assert r.status_code == 422
+
+
+def test_apply_422_degenerate_quaternion():
+    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
+        g1_factory.return_value = MagicMock()
+        r = client.post("/api/pose_3d/apply", json={
+            "side": "right", "position": [0,0,0], "quaternion": [0,0,0,0],
+        })
+        assert r.status_code == 422
+        assert "degenerate" in r.json()["detail"]
+
+
+def test_apply_normalizes_quaternion_before_forwarding():
+    captured = {}
+    def _abs(**kw):
+        captured.update(kw)
+        return {"ok": True}
+    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
+        g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs
+        g1_factory.return_value = g1
+        # quat con scala 2.0
+        r = client.post("/api/pose_3d/apply", json={
+            "side": "right", "position": [0,0,0], "quaternion": [0, 0, 0, 2.0],
+        })
+        assert r.status_code == 200
+        # Il g1 deve ricevere il quat normalizzato (norma 1)
+        q = captured["quaternion"]
+        norm = sum(x*x for x in q) ** 0.5
+        assert abs(norm - 1.0) < 1e-6