feat(right_arm): force, camera↔polso offset, cluster filter sul centroide

Tre miglioramenti al move-toward dopo il primo uso live:

1. checkbox "force" accanto a "Muovi": bypassa max_step_m quando
   l'utente sa che il delta richiesto e' grande ma corretto (es. primo
   avvicinamento all'oggetto).

2. camera_offset_wrist_mm: il modello assumeva camera e TCP coincidenti
   (sole sign flips). La camera Nuwa e' invece montata con traslazione
   costante rispetto al polso. La formula diventa:
       delta_wrist = sign * (centroid_cam - offset_cam) + t_wrist_cam
   Esposto in config (default 0,0,0), GUI (3 input "Camera rispetto a
   polso (mm)") e in /state. Il termine compensa il fatto che muovere
   il polso porta la camera, non il TCP, sul punto di presa.

3. filter_nearest_cluster: il centroide era media di tutti i punti
   SAM21 della mask. In presenza di sfondo lontano visibile attraverso
   buchi della depth del primo piano la media veniva trascinata a
   metri di distanza. Ora si tiene il cluster piu' vicino:
       z_near = p5(Z)
       keep   = punti con Z <= z_near + cluster_depth_window_m
   Default window 0.20 m, configurabile. La risposta di /detect/wrist
   espone n_sam21_points, n_centroid_points e z_near/z_cutoff per
   diagnosi.

Aggiornata anche la persistenza: /offset accetta camera_offset_wrist_mm.
GUI: i campi Offset/Sign/CamOff rispettano "user touched" (refresh non
li cancella mentre l'utente digita).

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

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

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(

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"