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>
This commit is contained in:
1 parent
fcd921ff32
commit
1fa7d12ad7
8 files changed
+260
-20
No files matched your search
@@ -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"
|
||||
|
||||
Reference in new issue
Block a user