feat(pose_3d): POST /apply + quaternion normalization

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