From 40c3fff7555ecfb2a12df368bfccf6bf392eb6e6 Mon Sep 17 00:00:00 2001 From: Adriano Dal Pastro Date: Thu, 21 May 2026 15:33:47 +0200 Subject: [PATCH] feat(pose_3d): POST /apply + quaternion normalization --- src/presa_mano/api/pose_3d_routes.py | 46 +++++++++++++++++ tests/test_pose_3d_quaternion.py | 32 ++++++++++++ tests/test_pose_3d_routes.py | 76 ++++++++++++++++++++++++++++ 3 files changed, 154 insertions(+) create mode 100644 tests/test_pose_3d_quaternion.py diff --git a/src/presa_mano/api/pose_3d_routes.py b/src/presa_mano/api/pose_3d_routes.py index 63317bb..c3ee8e0 100644 --- a/src/presa_mano/api/pose_3d_routes.py +++ b/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(), + } diff --git a/tests/test_pose_3d_quaternion.py b/tests/test_pose_3d_quaternion.py new file mode 100644 index 0000000..c6112a1 --- /dev/null +++ b/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]) diff --git a/tests/test_pose_3d_routes.py b/tests/test_pose_3d_routes.py index 09054ce..0f89c34 100644 --- a/tests/test_pose_3d_routes.py +++ b/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