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

src/presa_mano/api/right_arm_routes.py

@@ -14,6 +14,7 @@ from ..clients import G1Unavailable
 from ..config import update_config
 from ..geometry import quaternion_xyzw_to_rpy_deg
 from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21
+from ..right_arm.move import DeltaOversize, apply_z_floor, compute_delta_wrist
 from ..timing import Stopwatch
 from . import _deps
 from .perception_routes import _bbox_to_roi
@@ -187,3 +188,89 @@ def detect_wrist(req: DetectWristReq) -> dict[str, Any]:
     body = _detect_wrist_core(req.class_name, req.conf, sw)
     body["timings_ms"] = sw.as_dict()
     return body
+
+
+class MoveTowardReq(BaseModel):
+    class_name: Optional[str] = None
+    conf: Optional[float] = None
+    dry_run: bool = True
+    force: bool = False
+
+
+def _guard_g1_right() -> None:
+    if _deps.active_robot_key() != "g1":
+        raise HTTPException(status_code=412,
+                            detail=f"activate G1 right first (active={_deps.active_robot_key()})")
+    if _deps.default_side() != "right":
+        raise HTTPException(status_code=412,
+                            detail=f"activate G1 right first (side={_deps.default_side()})")
+
+
+@router.post("/move/toward")
+def move_toward(req: MoveTowardReq) -> dict[str, Any]:
+    sw = Stopwatch()
+    _guard_g1_right()
+    cfg = _deps.tab_right_arm_cfg()
+    off_mm = cfg.get("move_offset_camera_mm", {"x": 0, "y": 0, "z": 0})
+    sign = cfg.get("axis_sign_cam_to_wrist", {"x": 1, "y": 1, "z": 1})
+    max_step_m = float(cfg.get("max_step_m", 0.15))
+    z_floor = cfg.get("z_floor_m")
+    z_floor_m = float(z_floor) if z_floor is not None else None
+
+    det = _detect_wrist_core(req.class_name, req.conf, sw)
+    centroid = np.asarray(det["centroid_camera_m"], dtype=np.float64)
+    offset_m = np.array([off_mm["x"], off_mm["y"], off_mm["z"]], dtype=np.float64) / 1000.0
+    signs = (int(sign["x"]), int(sign["y"]), int(sign["z"]))
+
+    try:
+        delta = compute_delta_wrist(centroid, offset_m, signs,
+                                    max_step_m=max_step_m, force=req.force)
+    except DeltaOversize as e:
+        raise HTTPException(status_code=422, detail=str(e))
+
+    # Leggi posa polso PRIMA del move per applicare z-floor
+    try:
+        with sw.step("g1_wrist_before"):
+            wrist_before = _deps.g1().wrist()["right_wrist"]
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
+
+    delta, z_clipped = apply_z_floor(float(wrist_before["position"][2]), delta, z_floor_m)
+
+    try:
+        with sw.step("wrist_relative"):
+            _deps.g1(dry_run=req.dry_run).wrist_relative(
+                side="right", delta_position=delta.tolist(),
+            )
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
+
+    wrist_after: dict[str, Any] | None = None
+    if not req.dry_run:
+        try:
+            with sw.step("g1_wrist_after"):
+                wrist_after = _deps.g1().wrist()["right_wrist"]
+        except G1Unavailable:
+            wrist_after = None  # best-effort; il move è stato già inviato
+
+    log.info(
+        "move_toward class=%s conf=%s centroid=%s offset=%s sign=%s delta=%s dry_run=%s applied=%s",
+        req.class_name, req.conf, centroid.tolist(),
+        offset_m.tolist(), signs, delta.tolist(), req.dry_run, not req.dry_run,
+    )
+
+    return {
+        "detection": det["detection"],
+        "centroid_camera_m": det["centroid_camera_m"],
+        "distance_m": det["distance_m"],
+        "offset_camera_m": offset_m.tolist(),
+        "axis_signs": list(signs),
+        "delta_wrist_m": delta.tolist(),
+        "delta_wrist_mm": (delta * 1000.0).tolist(),
+        "z_clipped": bool(z_clipped),
+        "wrist_before": wrist_before,
+        "wrist_after": wrist_after,
+        "applied": not req.dry_run,
+        "n_sam21_points": det["n_sam21_points"],
+        "timings_ms": sw.as_dict(),
+    }

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