From afbfd55bca6df8d3be2e5161d2536be18fb3fa16 Mon Sep 17 00:00:00 2001 From: Adriano Dal Pastro Date: Wed, 20 May 2026 12:27:38 +0200 Subject: [PATCH] feat(right_arm): /move/toward con sign flips, max-step e z-floor --- src/presa_mano/api/right_arm_routes.py | 87 +++++++++++++++++++++ tests/test_right_arm_routes.py | 100 +++++++++++++++++++++++++ 2 files changed, 187 insertions(+) diff --git a/src/presa_mano/api/right_arm_routes.py b/src/presa_mano/api/right_arm_routes.py index a59ec50..4b869cc 100644 --- a/src/presa_mano/api/right_arm_routes.py +++ b/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(), + } diff --git a/tests/test_right_arm_routes.py b/tests/test_right_arm_routes.py index d23c71b..b7bec1c 100644 --- a/tests/test_right_arm_routes.py +++ b/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