feat(right_arm): /move/toward con sign flips, max-step e z-floor

tests/test_right_arm_routes.py

@@ -226,3 +226,103 @@ def test_detect_wrist_404_when_yolo_no_match():
         yolo = MagicMock(); yolo.detect_url.return_value = {"detections": []}; yolo_f.return_value = yolo
         r = client.post("/api/right_arm/detect/wrist", json={"class_name": "pestello"})
         assert r.status_code == 404
+
+
+def _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20):
+    """Helper: prepara un cloud SAM21 sintetico con centroide a `centroid_m`."""
+    import numpy as np
+    seg_pts = np.tile(np.array(centroid_m, dtype=np.float64), (n_points, 1))
+    return seg_pts
+
+
+def test_move_toward_dry_run_returns_delta_no_call_to_g1():
+    import numpy as np
+    seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20)
+    g1_called = []
+
+    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
+         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
+         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
+         patch("presa_mano.api.right_arm_routes._deps.sam21"), \
+         patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
+         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
+         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
+        nuwa_f.return_value.rgb_url.return_value = "http://x"
+        yolo_f.return_value.detect_url.return_value = _fake_yolo_top()
+        seg_f.return_value = MagicMock(points_cv_m=seg_pts)
+        g1 = MagicMock()
+        g1.wrist.return_value = _fake_wrist_response()
+
+        def _track_relative(**kw):
+            g1_called.append(kw)
+            return {"ok": True}
+        g1.wrist_relative.side_effect = _track_relative
+        g1_f.return_value = g1
+
+        r = client.post("/api/right_arm/move/toward",
+                        json={"class_name": "pestello", "dry_run": True})
+        assert r.status_code == 200
+        body = r.json()
+        # offset default (0,0,100mm) → centroid - offset = (0, 0, 0.10) →
+        # con sign (1,-1,-1) → (0, 0, -0.10) m
+        assert abs(body["delta_wrist_m"][2] - (-0.10)) < 1e-6
+        assert body["applied"] is False
+        # g1.wrist_relative viene comunque invocato (G1Client gestisce dry-run
+        # internamente; l'endpoint non lo salta).
+        assert len(g1_called) == 1
+        assert g1_called[0]["side"] == "right"
+
+
+def test_move_toward_real_call_returns_wrist_after():
+    import numpy as np
+    seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20)
+    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
+         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
+         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
+         patch("presa_mano.api.right_arm_routes._deps.sam21"), \
+         patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
+         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
+         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
+        nuwa_f.return_value.rgb_url.return_value = "http://x"
+        yolo_f.return_value.detect_url.return_value = _fake_yolo_top()
+        seg_f.return_value = MagicMock(points_cv_m=seg_pts)
+        g1 = MagicMock()
+        g1.wrist.return_value = _fake_wrist_response()
+        g1.wrist_relative.return_value = {"ok": True}
+        g1_f.return_value = g1
+        r = client.post("/api/right_arm/move/toward",
+                        json={"class_name": "pestello", "dry_run": False})
+        assert r.status_code == 200
+        body = r.json()
+        assert body["applied"] is True
+        assert body["wrist_after"] is not None
+
+
+def test_move_toward_422_oversize_delta_without_force():
+    import numpy as np
+    # centroid=(0,0,0.50), default offset=(0,0,0.10) → delta_z=-0.40 → > max_step 0.15
+    seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.50), n_points=20)
+    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
+         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
+         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
+         patch("presa_mano.api.right_arm_routes._deps.sam21"), \
+         patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
+         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
+         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
+        nuwa_f.return_value.rgb_url.return_value = "http://x"
+        yolo_f.return_value.detect_url.return_value = _fake_yolo_top()
+        seg_f.return_value = MagicMock(points_cv_m=seg_pts)
+        g1 = MagicMock(); g1.wrist.return_value = _fake_wrist_response(); g1_f.return_value = g1
+        r = client.post("/api/right_arm/move/toward",
+                        json={"class_name": "pestello", "dry_run": True, "force": False})
+        assert r.status_code == 422
+        assert "delta" in r.json()["detail"].lower()
+
+
+def test_move_toward_412_when_not_g1_right():
+    """Se config attiva != g1 o side != right, rifiuta con 412."""
+    with patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="h1"), \
+         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="left"):
+        r = client.post("/api/right_arm/move/toward",
+                        json={"class_name": "pestello", "dry_run": True})
+        assert r.status_code == 412