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(),
+    }