diff --git a/src/presa_mano/api/right_arm_routes.py b/src/presa_mano/api/right_arm_routes.py index b503dbe..5ccfe0b 100644 --- a/src/presa_mano/api/right_arm_routes.py +++ b/src/presa_mano/api/right_arm_routes.py @@ -15,6 +15,7 @@ from ..config import update_config from ..geometry import quaternion_xyzw_to_rpy_deg, rpy_deg_to_quaternion_xyzw from ..perception.detector import _normalize as _normalize_yolo from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21 +from ..right_arm.cluster import filter_nearest_cluster from ..right_arm.move import DeltaOversize, apply_z_floor, compute_delta_wrist from ..timing import Stopwatch from . import _deps @@ -40,6 +41,8 @@ def _state_payload(sw: Stopwatch) -> dict[str, Any]: "custom_pose": cfg.get("custom_pose"), "offset_camera_mm": cfg.get("move_offset_camera_mm"), "axis_signs": cfg.get("axis_sign_cam_to_wrist"), + "camera_offset_wrist_mm": cfg.get("camera_offset_wrist_mm"), + "cluster_depth_window_m": cfg.get("cluster_depth_window_m"), "max_step_m": cfg.get("max_step_m"), "z_floor_m": cfg.get("z_floor_m"), "timings_ms": sw.as_dict(), @@ -158,9 +161,18 @@ def _detect_wrist_core(class_name: str | None, conf: float | None, sw: Stopwatch seg = segment_with_sam21(nuwa, _deps.sam21(), bbox, sw) except Sam21Fallback as e: raise HTTPException(status_code=422, detail=f"SAM21 fallback: {e}") - pts = np.asarray(seg.points_cv_m, dtype=np.float64) + pts_all = np.asarray(seg.points_cv_m, dtype=np.float64) + if pts_all.shape[0] < 10: + raise HTTPException(status_code=422, detail=f"too few sam21 points: {pts_all.shape[0]}") + cfg = _deps.tab_right_arm_cfg() + window_m = float(cfg.get("cluster_depth_window_m", 0.20)) + with sw.step("cluster_filter"): + pts, cluster_diag = filter_nearest_cluster(pts_all, window_m=window_m) if pts.shape[0] < 10: - raise HTTPException(status_code=422, detail=f"too few sam21 points: {pts.shape[0]}") + raise HTTPException( + status_code=422, + detail=f"cluster troppo piccolo: {pts.shape[0]} punti dopo filtro " + f"(z_near={cluster_diag['z_near_m']:.3f}m window={window_m:.3f}m)") with sw.step("centroid"): centroid = pts.mean(axis=0) distance = float(np.linalg.norm(centroid)) @@ -169,7 +181,11 @@ def _detect_wrist_core(class_name: str | None, conf: float | None, sw: Stopwatch "bbox": list(bbox), "centroid_camera_m": [float(centroid[0]), float(centroid[1]), float(centroid[2])], "distance_m": distance, - "n_sam21_points": int(pts.shape[0]), + "n_sam21_points": int(pts_all.shape[0]), + "n_centroid_points": int(pts.shape[0]), + "cluster_window_m": window_m, + "cluster_z_near_m": cluster_diag["z_near_m"], + "cluster_z_cutoff_m": cluster_diag["z_cutoff_m"], } @@ -194,6 +210,7 @@ def move_toward(req: MoveTowardReq) -> dict[str, Any]: 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}) + cam_off_mm = cfg.get("camera_offset_wrist_mm", {"x": 0, "y": 0, "z": 0}) 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 @@ -201,10 +218,13 @@ def move_toward(req: MoveTowardReq) -> dict[str, Any]: 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 + cam_offset_wrist_m = np.array( + [cam_off_mm["x"], cam_off_mm["y"], cam_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, + camera_offset_wrist_m=cam_offset_wrist_m, max_step_m=max_step_m, force=req.force) except DeltaOversize as e: raise HTTPException(status_code=422, detail=str(e)) @@ -246,6 +266,7 @@ def move_toward(req: MoveTowardReq) -> dict[str, Any]: "distance_m": det["distance_m"], "offset_camera_m": offset_m.tolist(), "axis_signs": list(signs), + "camera_offset_wrist_m": cam_offset_wrist_m.tolist(), "delta_wrist_m": delta.tolist(), "delta_wrist_mm": (delta * 1000.0).tolist(), "z_clipped": bool(z_clipped), @@ -269,6 +290,7 @@ class PoseUpdateReq(BaseModel): class OffsetUpdateReq(BaseModel): move_offset_camera_mm: Optional[dict[str, float]] = None axis_sign_cam_to_wrist: Optional[dict[str, int]] = None + camera_offset_wrist_mm: Optional[dict[str, float]] = None def _exec_named_pose(pose_key: str, dry_run: bool, sw: Stopwatch) -> dict[str, Any]: @@ -332,9 +354,15 @@ def put_offset(req: OffsetUpdateReq) -> dict[str, Any]: if v not in (-1, 1): raise HTTPException(status_code=422, detail=f"axis_sign_cam_to_wrist['{k}'] must be +/-1") patch["axis_sign_cam_to_wrist"] = {k: int(req.axis_sign_cam_to_wrist[k]) for k in ("x", "y", "z")} + if req.camera_offset_wrist_mm is not None: + for k in ("x", "y", "z"): + if k not in req.camera_offset_wrist_mm: + raise HTTPException(status_code=422, detail=f"camera_offset_wrist_mm missing '{k}'") + patch["camera_offset_wrist_mm"] = {k: float(req.camera_offset_wrist_mm[k]) for k in ("x", "y", "z")} if not patch: raise HTTPException(status_code=422, detail="nothing to update") update_config({"tab_right_arm": patch}) cfg = _deps.tab_right_arm_cfg() return {"move_offset_camera_mm": cfg.get("move_offset_camera_mm"), - "axis_sign_cam_to_wrist": cfg.get("axis_sign_cam_to_wrist")} + "axis_sign_cam_to_wrist": cfg.get("axis_sign_cam_to_wrist"), + "camera_offset_wrist_mm": cfg.get("camera_offset_wrist_mm")} diff --git a/src/presa_mano/right_arm/cluster.py b/src/presa_mano/right_arm/cluster.py new file mode 100644 index 0000000..c7f5e86 --- /dev/null +++ b/src/presa_mano/right_arm/cluster.py @@ -0,0 +1,39 @@ +"""Filtri di pulizia della nuvola SAM21 prima del centroide.""" +from __future__ import annotations + +import numpy as np + + +def filter_nearest_cluster( + points_xyz_m: np.ndarray, + window_m: float, + z_near_percentile: float = 5.0, +) -> tuple[np.ndarray, dict]: + """Tieni solo i punti del cluster più vicino alla camera lungo Z. + + Assume frame OpenCV (Z>0 davanti). `z_near` = percentile robusto di Z + (default p5) — proxy per "il valore Z più vicino" resistente a qualche + outlier negativo. Si tengono i punti con `Z <= z_near + window_m`. + + Utile contro nuvole bimodali tipo oggetto-vicino + background-lontano, + dove la media e' dominata dal background. Niente magic-number su Z max + assoluto: la soglia e' relativa al punto piu' vicino. + + Ritorna (filtered, diag) con diag = {z_near_m, z_cutoff_m, + n_input, n_kept}. + """ + pts = np.asarray(points_xyz_m, dtype=np.float64) + n_in = int(pts.shape[0]) + if n_in == 0: + return pts, {"z_near_m": None, "z_cutoff_m": None, + "n_input": 0, "n_kept": 0} + z = pts[:, 2] + z_near = float(np.percentile(z, float(z_near_percentile))) + z_cut = z_near + float(window_m) + mask = z <= z_cut + return pts[mask], { + "z_near_m": z_near, + "z_cutoff_m": z_cut, + "n_input": n_in, + "n_kept": int(mask.sum()), + } diff --git a/src/presa_mano/right_arm/move.py b/src/presa_mano/right_arm/move.py index 4fec373..eb7a062 100644 --- a/src/presa_mano/right_arm/move.py +++ b/src/presa_mano/right_arm/move.py @@ -17,13 +17,22 @@ def compute_delta_wrist( centroid_camera_m: np.ndarray, offset_camera_m: np.ndarray, axis_signs: tuple[int, int, int], + camera_offset_wrist_m: np.ndarray | None = None, max_step_m: float | None = None, force: bool = False, ) -> np.ndarray: - """Delta da applicare al polso (frame polso) per portare la camera sull'offset. + """Delta da applicare al polso (frame polso) per portare il TCP sull'offset + di presa dall'oggetto osservato dalla camera. - Modello: si assume assi camera ≈ assi polso a meno dei segni. - delta_wrist = sign * (centroid_camera - offset_camera) + Modello: assi camera ≈ assi polso a meno dei segni; la camera è montata sul + polso con traslazione costante `camera_offset_wrist_m` (origine polso → + origine camera, espressa nel frame polso). + + delta_wrist = sign * (centroid_camera - offset_camera) + t_wrist_cam + + Il termine `+ t_wrist_cam` compensa il fatto che camera e TCP non coincidono: + spostare il polso di `sign * (centroid - offset)` porterebbe la CAMERA sul + punto desiderato, lasciando il TCP indietro di t_wrist_cam. Se `max_step_m` è settato e ||delta|| > max_step_m e `force` è False, solleva DeltaOversize. @@ -31,7 +40,9 @@ def compute_delta_wrist( centroid = np.asarray(centroid_camera_m, dtype=np.float64).reshape(3) offset = np.asarray(offset_camera_m, dtype=np.float64).reshape(3) sign = np.asarray(axis_signs, dtype=np.float64).reshape(3) - delta = sign * (centroid - offset) + t_wc = (np.zeros(3, dtype=np.float64) if camera_offset_wrist_m is None + else np.asarray(camera_offset_wrist_m, dtype=np.float64).reshape(3)) + delta = sign * (centroid - offset) + t_wc if max_step_m is not None and not force: n = float(np.linalg.norm(delta)) if n > max_step_m: diff --git a/tests/test_right_arm_cluster.py b/tests/test_right_arm_cluster.py new file mode 100644 index 0000000..792b658 --- /dev/null +++ b/tests/test_right_arm_cluster.py @@ -0,0 +1,56 @@ +"""Unit test del filtro cluster sui punti SAM21.""" +from __future__ import annotations + +import numpy as np + +from presa_mano.right_arm.cluster import filter_nearest_cluster + + +def test_empty_input_returns_empty(): + out, diag = filter_nearest_cluster(np.zeros((0, 3)), window_m=0.2) + assert out.shape == (0, 3) + assert diag["n_input"] == 0 and diag["n_kept"] == 0 + assert diag["z_near_m"] is None + + +def test_bimodal_keeps_only_near_cluster(): + # 100 punti vicini (Z ~0.25 m) + 50 background lontano (Z ~4.0 m). + near = np.column_stack([np.zeros(100), np.zeros(100), + np.random.uniform(0.22, 0.28, 100)]) + far = np.column_stack([np.zeros(50), np.zeros(50), + np.random.uniform(3.8, 4.2, 50)]) + pts = np.vstack([near, far]) + out, diag = filter_nearest_cluster(pts, window_m=0.20) + # Tutti i punti vicini, nessun punto lontano + assert diag["n_kept"] == 100 + assert out[:, 2].max() < 0.30 + assert diag["z_near_m"] < 0.25 # p5 ~= 0.22 + + +def test_single_cluster_keeps_all(): + # Unica nuvola compatta: tutto rientra nella window. + pts = np.column_stack([np.zeros(100), np.zeros(100), + np.random.uniform(0.20, 0.30, 100)]) + out, diag = filter_nearest_cluster(pts, window_m=0.20) + assert diag["n_kept"] == 100 + + +def test_window_too_small_keeps_only_nearest(): + pts = np.array([ + [0.0, 0.0, 0.20], + [0.0, 0.0, 0.30], + [0.0, 0.0, 0.40], + [0.0, 0.0, 4.00], + ]) + out, diag = filter_nearest_cluster(pts, window_m=0.05) + # z_near = p5 di [0.20,0.30,0.40,4.00] ≈ 0.215 → cutoff ≈ 0.265 → solo 0.20 + assert out.shape[0] == 1 + assert out[0, 2] == 0.20 + + +def test_diag_reports_inputs_and_kept(): + pts = np.array([[0.0, 0.0, 0.25]] * 20 + [[0.0, 0.0, 4.0]] * 5) + out, diag = filter_nearest_cluster(pts, window_m=0.10) + assert diag["n_input"] == 25 + assert diag["n_kept"] == 20 + assert diag["z_cutoff_m"] == diag["z_near_m"] + 0.10 diff --git a/tests/test_right_arm_move.py b/tests/test_right_arm_move.py index 51cf5a9..936363a 100644 --- a/tests/test_right_arm_move.py +++ b/tests/test_right_arm_move.py @@ -39,6 +39,29 @@ def test_signs_applied_componentwise(): assert np.allclose(delta, [0.05, -0.10, -0.20]) +def test_camera_offset_wrist_added_after_sign_flip(): + # centroid=(0,0,1) offset=(0,0,0) sign=(1,1,1) t_wc=(0.05,-0.02,0.08) + # → sign*(centroid-offset) = (0,0,1) + # → + t_wc = (0.05, -0.02, 1.08) + delta = compute_delta_wrist( + centroid_camera_m=np.array([0.0, 0.0, 1.0]), + offset_camera_m=np.array([0.0, 0.0, 0.0]), + axis_signs=(1, 1, 1), + camera_offset_wrist_m=np.array([0.05, -0.02, 0.08]), + ) + assert np.allclose(delta, [0.05, -0.02, 1.08]) + + +def test_camera_offset_wrist_default_zero_back_compat(): + # Senza camera_offset_wrist_m la formula vecchia resta invariata. + delta = compute_delta_wrist( + centroid_camera_m=np.array([0.10, 0.20, 0.30]), + offset_camera_m=np.array([0.00, 0.00, 0.10]), + axis_signs=(1, -1, -1), + ) + assert np.allclose(delta, [0.10, -0.20, -0.20]) + + def test_oversize_raises_when_not_forced(): with pytest.raises(DeltaOversize): compute_delta_wrist( diff --git a/tests/test_right_arm_routes.py b/tests/test_right_arm_routes.py index 54e2a7b..ed36ce2 100644 --- a/tests/test_right_arm_routes.py +++ b/tests/test_right_arm_routes.py @@ -222,6 +222,35 @@ def test_detect_wrist_returns_centroid_and_distance(): assert abs(c[2] - 0.20) < 0.05 assert body["distance_m"] > 0 assert body["n_sam21_points"] == 15 + assert body["n_centroid_points"] == 15 # cluster compatto: tutti tenuti + + +def test_detect_wrist_filters_far_background_from_centroid(): + """Regressione: nuvola bimodale (oggetto vicino + background lontano) + deve dare centroide dell'oggetto, non della media (che sarebbe lontana).""" + import numpy as np + near = np.column_stack([np.zeros(100), np.zeros(100), + np.full(100, 0.25)]) + far = np.column_stack([np.zeros(50), np.zeros(50), + np.full(50, 4.0)]) + fake_points = np.vstack([near, far]) # media ingenua: ~1.5m; cluster: ~0.25m + + 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") as sam_f: + nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa + yolo = MagicMock(); yolo.detect_url.return_value = _fake_yolo_top(); yolo_f.return_value = yolo + sam_f.return_value = MagicMock() + seg = MagicMock(); seg.points_cv_m = fake_points; seg_f.return_value = seg + + r = client.post("/api/right_arm/detect/wrist", + json={"class_name": "pestello"}) + assert r.status_code == 200 + body = r.json() + assert body["n_sam21_points"] == 150 + assert body["n_centroid_points"] == 100 # solo i vicini + assert body["centroid_camera_m"][2] == 0.25 # non 1.5m della media def test_detect_wrist_422_too_few_points(): @@ -256,6 +285,22 @@ def _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20): return seg_pts +def _stub_tab_cfg(**overrides): + """Config fissa per i test del move (isolata da modifiche utente al config.yaml).""" + base = { + "move_offset_camera_mm": {"x": 0.0, "y": 0.0, "z": 100.0}, + "axis_sign_cam_to_wrist": {"x": 1, "y": -1, "z": -1}, + "camera_offset_wrist_mm": {"x": 0.0, "y": 0.0, "z": 0.0}, + "cluster_depth_window_m": 0.20, + "max_step_m": 0.15, + "z_floor_m": None, + "home_pose": {"position": [0.30, -0.20, 0.30], "rpy_deg": [0, 0, 0]}, + "custom_pose": {"position": [0.30, -0.20, 0.30], "rpy_deg": [0, 0, 0]}, + } + base.update(overrides) + return base + + 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) @@ -266,6 +311,8 @@ def test_move_toward_dry_run_returns_delta_no_call_to_g1(): 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.tab_right_arm_cfg", + return_value=_stub_tab_cfg()), \ 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" @@ -302,6 +349,8 @@ def test_move_toward_real_call_returns_wrist_after(): 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.tab_right_arm_cfg", + return_value=_stub_tab_cfg()), \ 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" @@ -328,6 +377,8 @@ def test_move_toward_422_oversize_delta_without_force(): 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.tab_right_arm_cfg", + return_value=_stub_tab_cfg()), \ 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" diff --git a/web/index.html b/web/index.html index ab5bf3c..3d8df1c 100644 --- a/web/index.html +++ b/web/index.html @@ -400,9 +400,16 @@ +
+ Camera rispetto a polso (mm): + + + +
+
— nessun move
diff --git a/web/right_arm.js b/web/right_arm.js index 9c19611..980df25 100644 --- a/web/right_arm.js +++ b/web/right_arm.js @@ -162,16 +162,9 @@ async function refreshPose() { // Aggiorna anche i campi delle pose (solo se non sono stati toccati) if (s.home_pose) fillPoseFields('raHome', s.home_pose); if (s.custom_pose) fillPoseFields('raCust', s.custom_pose); - if (s.offset_camera_mm) { - $('raOffX').value = s.offset_camera_mm.x; - $('raOffY').value = s.offset_camera_mm.y; - $('raOffZ').value = s.offset_camera_mm.z; - } - if (s.axis_signs) { - $('raSgnX').value = s.axis_signs.x; - $('raSgnY').value = s.axis_signs.y; - $('raSgnZ').value = s.axis_signs.z; - } + if (s.offset_camera_mm) fillIfUntouched('raOff', s.offset_camera_mm); + if (s.axis_signs) fillIfUntouched('raSgn', s.axis_signs); + if (s.camera_offset_wrist_mm) fillIfUntouched('raCamOff', s.camera_offset_wrist_mm); } catch (e) { // silenzioso: il polso può non essere disponibile } @@ -188,6 +181,21 @@ function fillPoseFields(prefix, pose) { $(prefix + 'W').value = pose.rpy_deg[2]; } +function fillIfUntouched(prefix, xyz) { + for (const k of ['X','Y','Z']) { + const el = $(prefix + k); + if (!el || el.dataset.userTouched) continue; + el.value = xyz[k.toLowerCase()]; + } +} + +function clearUserTouched(prefix) { + for (const k of ['X','Y','Z','R','P','W']) { + const el = $(prefix + k); + if (el) delete el.dataset.userTouched; + } +} + function readPoseFields(prefix) { return { position: [ @@ -220,8 +228,16 @@ async function onSaveOffset() { y: parseInt($('raSgnY').value, 10), z: parseInt($('raSgnZ').value, 10), }, + camera_offset_wrist_mm: { + x: parseFloat($('raCamOffX').value), + y: parseFloat($('raCamOffY').value), + z: parseFloat($('raCamOffZ').value), + }, }), }); + clearUserTouched('raOff'); + clearUserTouched('raSgn'); + clearUserTouched('raCamOff'); alert('Offset salvato.'); } catch (e) { alert(`Errore salvataggio: ${e.message}`); @@ -234,11 +250,17 @@ async function onMove() { const className = $('raWristClass').value; const conf = parseFloat($('raWristConf').value) || null; const dryRun = $('raMoveDryRun').checked; - if (!dryRun && !confirm('Dry-run OFF: eseguire move REALE verso il pezzo?')) return; + const force = $('raMoveForce').checked; + if (!dryRun) { + const msg = force + ? 'Dry-run OFF + force ON (max_step bypassato): eseguire move REALE verso il pezzo?' + : 'Dry-run OFF: eseguire move REALE verso il pezzo?'; + if (!confirm(msg)) return; + } try { const res = await jsonFetch('/api/right_arm/move/toward', { method: 'POST', headers: { 'Content-Type': 'application/json' }, - body: JSON.stringify({ class_name: className, conf, dry_run: dryRun }), + body: JSON.stringify({ class_name: className, conf, dry_run: dryRun, force }), }); const d = res.delta_wrist_mm; $('raMoveResult').classList.remove('dim'); @@ -335,6 +357,9 @@ function init() { $('raWristAutoRefresh').addEventListener('change', startWristLoop); markUserTouchedOnInput('raHome'); markUserTouchedOnInput('raCust'); + markUserTouchedOnInput('raOff'); + markUserTouchedOnInput('raSgn'); + markUserTouchedOnInput('raCamOff'); populateClassDropdowns(); startFrontLoop();