diff --git a/README.md b/README.md index cd7245b..72f0e93 100644 --- a/README.md +++ b/README.md @@ -2,21 +2,24 @@ Microservizio Python (FastAPI) che orchestra: -1. **Calibrazione hand-eye** (eye-to-hand) tra la **Nuwa depth camera** (montata in alto, - a sinistra del robot) e la **base del G1**, usando un **ArUco sul polso destro**. +1. **Calibrazione hand-eye** (eye-to-hand) tra la **Nuwa depth camera** (fissa) e la + **base del robot**, usando un **ArUco sul polso**. Supporta **G1** e **H1**. 2. **Riconoscimento oggetto** via YOLO server + point cloud Nuwa. -3. **Pianificazione presa**: asse dell'oggetto (PCA) + posa polso per il G1. +3. **Pianificazione presa**: asse dell'oggetto (PCA) + posa polso per il robot attivo. ## Microservizi usati -| Servizio | URL (default) | Scopo | -|---------------|---------------------------|-------------------------------------| -| Nuwa-HP60C | http://localhost:8877 | RGB, depth, point cloud, intrinsics | -| Marker-finder | http://localhost:8082 | Detection 2D ArUco (corners) | -| YOLO Server | http://localhost:8879 | Bbox oggetto da prendere | -| G1 API Server | http://localhost:8765 | Controllo polso/mano (dry-run se down) | +| Servizio | URL (default) | Scopo | +|-------------------|-----------------------|----------------------------------------------------| +| Nuwa-HP60C | http://localhost:8877 | RGB, depth, point cloud, intrinsics | +| Marker-finder | http://localhost:8082 | Detection 2D ArUco (corners + pose) | +| YOLO Server | http://localhost:8879 | Bbox oggetto da prendere | +| G1 API Server | http://localhost:8765 | Controllo polso/mano G1 (dry-run se down) | +| H1 API Server | http://localhost:8767 | Controllo polso/mano H1 (network host, 4 DoF) | -Il servizio **presa-mano** espone GUI + API su **http://localhost:8890**. +Il servizio **presa-mano** espone GUI + API su **http://localhost:8890**. Il robot attivo +è scelto da `config.yaml > robot.active` (`g1` | `h1`); l'orchestrator fa routing +automatico verso il bridge corrispondente. ## Quick start @@ -29,13 +32,50 @@ docker compose up -d presa-mano Tre tab: -- **Calibrazione**: muovi il polso in varie pose, premi *Acquisisci sample* per ognuna, - poi *Risolvi*. Salva `data/calibration.yaml`. -- **Grasp**: rileva oggetto, visualizza bbox + asse, esegui (con toggle dry-run). +- **Calibrazione**: muovi il polso in varie pose, *Acquisisci sample*, poi *Risolvi*. + Salva `data/calibration_.yaml`. +- **Grasp**: rileva oggetto, visualizza bbox + asse, esegui (toggle dry-run). - **Movimento EE**: lettura pose correnti, invio pose assolute/relative, apri/chiudi mano, emergency stop. +## Calibrazione automatica via script + +In `scripts/`: + +| Script | Cosa fa | +|----------------------------|--------------------------------------------------------------------------| +| `explore_and_calibrate.py` | Probe assiale dell'envelope di visibilità tag → workspace JSON + PNG → N sample random nel workspace → solve. | +| `extend_calibration.py` | Estende la calibrazione esistente fino a `n_used >= TARGET_GOOD` (default 30). Riusa il workspace, NON cancella i sample. | +| `repeatability.py` | N detection consecutive a polso fermo per misurare σ del marker-finder (diagnostica bias vs noise). | + +Tutti producono file in `data/`: `workspace_.json`, `workspace_.png`, +`samples_.json`, `calibration_.yaml`. + ## Configurazione -`config.yaml` contiene i default; l'API espone `GET/PUT /api/config` per override runtime -(es. cambiare ID/size ArUco dalla GUI). +`config.yaml` contiene i default; l'API espone `GET/PUT /api/config` per override +runtime (es. cambiare ID/size ArUco, robot attivo, side di default). + +Default rilevanti: + +- `calibration.method: DANIILIDIS` — sui dati H1+Nuwa con bias intrinsics di + marker-finder, DANIILIDIS dà ~4× meno residuo di TSAI (default OpenCV). +- `robot.active: h1` / `robot.wrist_side: right`. + +## Caveat noti + +- **Wrist passthrough**: i campi `speed_mm_s` e `traj_step_mm` non sono nello schema + di `/api/wrist/{absolute,relative}`. Per move "precisi" (closed-loop, slow, + waypoint fitti) bisogna chiamare direttamente il bridge robot, es. + `POST http://localhost:8767/wrist/absolute` su H1. +- **Marker-finder intrinsics**: il sibling service usa intrinsics D435i bakeate che + non corrispondono alla Nuwa → bias sistematico in `T_cam_marker`. È il floor del + residuo hand-eye (~14 mm su H1+Nuwa). Per scendere serve ricalibrare quel servizio. +- **H1 4 DoF**: nessun polso articolato. Si comanda solo posizione; l'orientamento + del polso è output dell'IK e drifta con la posa (fino a ~30° su Y ai bordi + dell'envelope). I solver hand-eye risentono della scarsa diversità rotazionale. +- **`/api/arms/go_neutral`**: il proxy nell'orchestrator non è montato (404). Per + riportare le braccia H1 a home: `POST http://localhost:8767/arms/go_neutral` diretto. + +Vedi `CLAUDE.md` per i dettagli di architettura e per le frame conventions +(Nuwa native vs OpenCV vs Nuwa ROI). diff --git a/config.yaml b/config.yaml index 31820c0..8b21fd7 100644 --- a/config.yaml +++ b/config.yaml @@ -40,7 +40,7 @@ grasp: - 0 calibration: min_samples: 5 - method: TSAI + method: DANIILIDIS # sweep su 49 sample H1 sx: TSAI=62.5mm, DANIILIDIS=14.2mm trans residual file: data/calibration.yaml samples_file: data/samples.json chat: diff --git a/scripts/explore_and_calibrate.py b/scripts/explore_and_calibrate.py new file mode 100644 index 0000000..acaf50c --- /dev/null +++ b/scripts/explore_and_calibrate.py @@ -0,0 +1,419 @@ +"""Esplora l'envelope di visibilità del tag ArUco sul polso H1 (sx) usando +la posizione reale (actual), non il target. Poi calibra con sample random +nell'envelope, scartando duplicati e arm stuck. Salva workspace JSON, PNG +e calibration. + +Uso: python3 scripts/explore_and_calibrate.py + +Note H1: 4 DoF, niente polso articolato → la diversità di orientamento per la +calibrazione viene dall'IK sui (X,Y,Z) variati, non da delta_quat. Si comanda +sempre quat=identità e si lascia l'IK scegliere la branca. +""" + +from __future__ import annotations + +import json +import math +import random +import time +from pathlib import Path +from typing import Any + +import requests + +ORC = "http://localhost:8890" +H1 = "http://localhost:8767" +SIDE = "left" +QUAT_IDENT = [0.0, 0.0, 0.0, 1.0] + +# Geometria di sondaggio +PROBE_STEP_M = 0.030 +PROBE_MAX_M = 0.250 +REFINE_HALVINGS = 2 +Z_RANGE_HALF_M = 0.050 # ±50 mm da home (richiesta utente) +Z_PROBE_OFFSETS_M = [0.0, 0.025, -0.025, 0.050, -0.050] +ENVELOPE_MARGIN_M = 0.020 # restringi i bounds di 20 mm dai punti reachable + +# Movimento — alta precisione: precise closed-loop, velocità bassa, waypoint fitti +PRECISE = True +SPEED_MMS = 40 +TRAJ_STEP_MM = 10 +SETTLE_S = 1.2 +MOVE_TIMEOUT_S = 25.0 +STUCK_DELTA_M = 0.030 # se actual cambia meno di 30 mm e il target era lontano → stuck +STUCK_TARGET_DIST_M = 0.100 + +# Calibrazione +N_SAMPLES = 24 +RANDOM_SEED = 17 +MIN_SAMPLE_SEP_M = 0.030 # distanza minima tra actual di sample accettati +REUSE_WORKSPACE_IF_FRESH_S = 3600 # se workspace_left.json è < 1h, riusalo (skip probes) + +DATA_DIR = Path("/opt/docker/robotserver/presa-mano/data") + + +def post(url: str, body: dict[str, Any] | None = None, timeout: float = 15.0) -> dict[str, Any]: + r = requests.post(url, json=body or {}, timeout=timeout) + try: + return r.json() + except Exception: + return {"status_code": r.status_code, "text": r.text[:500]} + + +def get(url: str, timeout: float = 5.0) -> dict[str, Any]: + r = requests.get(url, timeout=timeout) + return r.json() + + +def go_home() -> dict[str, Any]: + return post(f"{H1}/arms/go_neutral", {}) + + +def read_left_pos() -> list[float]: + return get(f"{ORC}/api/wrist")["left_wrist"]["position"] + + +def read_left_quat() -> list[float]: + return get(f"{ORC}/api/wrist")["left_wrist"]["quaternion"] + + +def move_abs(pos: list[float], quat: list[float] = QUAT_IDENT) -> dict[str, Any]: + """Bypassa l'orchestratore (la sua AbsoluteReq scarta speed_mm_s/traj_step_mm). + Parla direttamente al bridge H1 per ottenere precise + slow + waypoint fitti.""" + body = { + "side": SIDE, + "position": list(pos), + "quaternion": list(quat), + "wait": True, + "timeout_s": MOVE_TIMEOUT_S, + "precise": PRECISE, + "speed_mm_s": SPEED_MMS, + "traj_step_mm": TRAJ_STEP_MM, + } + return post(f"{H1}/wrist/absolute", body, timeout=MOVE_TIMEOUT_S + 5) + + +def detect_marker() -> dict[str, Any]: + return post(f"{ORC}/api/calibration/preview", {}, timeout=10) + + +def take_calib_sample() -> dict[str, Any]: + return post(f"{ORC}/api/calibration/sample", {"side": SIDE}, timeout=15) + + +def solve_calib() -> dict[str, Any]: + return post(f"{ORC}/api/calibration/solve", {"side": SIDE}, timeout=30) + + +def clear_samples() -> dict[str, Any]: + r = requests.delete(f"{ORC}/api/calibration/samples", params={"side": SIDE}, timeout=10) + return r.json() if r.headers.get("content-type", "").startswith("application/json") else {"text": r.text} + + +def dist3(a: list[float], b: list[float]) -> float: + return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b))) + + +def move_and_observe(target: list[float], prev_actual: list[float] | None = None) -> dict[str, Any]: + """Esegue il move_abs, restituisce dict con fields: + target, actual, found, marker_id, center_px, stuck (bool), move_resp. + """ + move_resp = move_abs(target) + time.sleep(SETTLE_S) + actual = read_left_pos() + det = detect_marker() + found = bool(det.get("found")) + stuck = False + if prev_actual is not None: + target_dist = dist3(target, prev_actual) + actual_change = dist3(actual, prev_actual) + if target_dist > STUCK_TARGET_DIST_M and actual_change < STUCK_DELTA_M: + stuck = True + return { + "target": target, + "actual": actual, + "found": found, + "center_px": det.get("center_px"), + "marker_id": det.get("marker_id"), + "stuck": stuck, + "move_stalled": bool(move_resp.get("stalled")), + "move_reached": bool(move_resp.get("reached_target")), + } + + +def probe_axis(home: list[float], direction: str) -> list[dict]: + """Cammina dall'home finché il tag scompare oppure il move fallisce o l'arm si blocca. + Poi raffina binario tra l'ultimo visibile e il primo invisibile.""" + axis = 0 if direction[0] == "x" else 1 + sign = 1 if direction[1] == "+" else -1 + points: list[dict] = [] + last_visible = list(home) + first_invisible: list[float] | None = None + prev_actual = list(home) + + for k in range(1, int(PROBE_MAX_M / PROBE_STEP_M) + 1): + target = list(home) + target[axis] = home[axis] + sign * k * PROBE_STEP_M + print(f" step {k} {direction}: target={[round(v,3) for v in target]}", flush=True) + rec = move_and_observe(target, prev_actual=prev_actual) + rec.update({"phase": "axial", "direction": direction}) + points.append(rec) + print( + f" actual={[round(v,3) for v in rec['actual']]} " + f"found={rec['found']} stuck={rec['stuck']} stalled={rec['move_stalled']}", + flush=True, + ) + prev_actual = rec["actual"] + if rec["stuck"]: + print(f" → arm stuck su {direction}, fermo qui", flush=True) + break + if rec["found"]: + last_visible = list(rec["actual"]) # uso ACTUAL come edge + else: + first_invisible = list(rec["actual"]) + break + + if first_invisible is not None and not rec.get("stuck", False): + for i in range(REFINE_HALVINGS): + mid = [(a + b) / 2.0 for a, b in zip(last_visible, first_invisible)] + print(f" refine #{i+1} {direction}: target={[round(v,3) for v in mid]}", flush=True) + rec = move_and_observe(mid, prev_actual=prev_actual) + rec.update({"phase": "refine", "direction": direction}) + points.append(rec) + print( + f" actual={[round(v,3) for v in rec['actual']]} " + f"found={rec['found']} stuck={rec['stuck']}", + flush=True, + ) + prev_actual = rec["actual"] + if rec["stuck"]: + break + if rec["found"]: + last_visible = list(rec["actual"]) + else: + first_invisible = list(rec["actual"]) + + return points + + +def probe_z(home: list[float]) -> list[dict]: + """Probe Z a home XY per misurare il range Z effettivamente reachable+visibile.""" + points: list[dict] = [] + prev_actual = list(home) + for dz in Z_PROBE_OFFSETS_M: + target = [home[0], home[1], home[2] + dz] + print(f" z probe dz={dz*1000:+.0f}mm: target={[round(v,3) for v in target]}", flush=True) + rec = move_and_observe(target, prev_actual=prev_actual) + rec.update({"phase": "z_axial", "direction": f"z{'+' if dz>=0 else '-'}", "dz_m": dz}) + points.append(rec) + print( + f" actual={[round(v,3) for v in rec['actual']]} found={rec['found']}", + flush=True, + ) + prev_actual = rec["actual"] + return points + + +def visualize(workspace: dict[str, Any]) -> Path: + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + from matplotlib.patches import Rectangle + + fig, ax = plt.subplots(figsize=(9, 9)) + for p in workspace["probes"]: + x, y = p["actual"][0], p["actual"][1] + c = "tab:green" if p["found"] else "tab:red" + marker = "o" if p["phase"] == "axial" else ("s" if p["phase"] == "refine" else "D") + ax.scatter(x, y, c=c, s=80, alpha=0.7, marker=marker, edgecolors="black", linewidths=0.4) + for s in workspace.get("calib_samples_xy", []): + ax.scatter(s[0], s[1], c="tab:blue", s=60, marker="x", linewidths=2.0) + xr = workspace["x_range_m"] + yr = workspace["y_range_m"] + rect = Rectangle((xr[0], yr[0]), xr[1] - xr[0], yr[1] - yr[0], + fill=False, edgecolor="tab:blue", linewidth=2, linestyle="--", label="envelope") + ax.add_patch(rect) + raw_xr = workspace["x_range_raw_m"] + raw_yr = workspace["y_range_raw_m"] + rect_raw = Rectangle((raw_xr[0], raw_yr[0]), raw_xr[1] - raw_xr[0], raw_yr[1] - raw_yr[0], + fill=False, edgecolor="gray", linewidth=1, linestyle=":", label="reachable raw") + ax.add_patch(rect_raw) + h = workspace["home"] + ax.scatter(h[0], h[1], c="black", s=250, marker="*", zorder=5, label="home") + ax.set_xlabel("X (m, base frame)") + ax.set_ylabel("Y (m, base frame)") + title = ( + f"Tag visibility envelope — left wrist (H1)\n" + f"X: [{xr[0]:.3f}, {xr[1]:.3f}] m Y: [{yr[0]:.3f}, {yr[1]:.3f}] m " + f"Z: [{workspace['z_range_m'][0]:.3f}, {workspace['z_range_m'][1]:.3f}] m\n" + f"verde=visibile, rosso=non visibile, blu x=calib samples, *=home" + ) + ax.set_title(title) + ax.set_aspect("equal") + ax.grid(True, alpha=0.3) + ax.legend(loc="best") + out_path = DATA_DIR / "workspace_left.png" + plt.savefig(out_path, dpi=120, bbox_inches="tight") + plt.close() + return out_path + + +def shrink(lo: float, hi: float, margin: float) -> tuple[float, float]: + if hi - lo <= 2 * margin: + # envelope troppo stretto → niente shrink + return lo, hi + return lo + margin, hi - margin + + +def main() -> int: + print("=== presa-mano: explore + calibrate (left, H1) — v2 (actual-based) ===", flush=True) + + print("\n[0] go_neutral", flush=True) + print(go_home(), flush=True) + time.sleep(2.0) + home = read_left_pos() + home_quat = read_left_quat() + print(f"home pos = {[round(v,4) for v in home]}", flush=True) + print(f"home quat = {[round(v,4) for v in home_quat]}", flush=True) + + ws_path = DATA_DIR / "workspace_left.json" + workspace: dict[str, Any] | None = None + if ws_path.exists(): + age = time.time() - ws_path.stat().st_mtime + if age < REUSE_WORKSPACE_IF_FRESH_S: + try: + workspace = json.loads(ws_path.read_text()) + print(f"\n[1*] riuso workspace già mappato (age={age:.0f}s) — skip probes", flush=True) + print(f" X={workspace['x_range_m']} Y={workspace['y_range_m']} Z={workspace['z_range_m']}", flush=True) + except Exception: + workspace = None + + if workspace is None: + print("\n[1] axial probes per envelope visibilità tag (X,Y)", flush=True) + all_probes: list[dict] = [] + for direction in ["x+", "x-", "y+", "y-"]: + print(f"\n--- {direction} ---", flush=True) + go_home() + time.sleep(1.5) + all_probes.extend(probe_axis(home, direction)) + + print("\n[1.5] Z probe a home XY", flush=True) + go_home() + time.sleep(1.5) + all_probes.extend(probe_z(home)) + + visible_actuals = [p["actual"] for p in all_probes if p["found"]] + if not visible_actuals: + print("Nessun probe visibile — abort.", flush=True) + return 1 + xs = [a[0] for a in visible_actuals] + ys = [a[1] for a in visible_actuals] + zs = [a[2] for a in visible_actuals] + raw_x = (min(xs), max(xs)) + raw_y = (min(ys), max(ys)) + raw_z = (min(zs), max(zs)) + + x_lo, x_hi = shrink(*raw_x, ENVELOPE_MARGIN_M) + y_lo, y_hi = shrink(*raw_y, ENVELOPE_MARGIN_M) + z_lo_user = home[2] - Z_RANGE_HALF_M + z_hi_user = home[2] + Z_RANGE_HALF_M + z_lo = max(z_lo_user, raw_z[0] + 0.005) + z_hi = min(z_hi_user, raw_z[1] - 0.005) + if z_lo >= z_hi: + z_lo = z_hi = home[2] + + workspace = { + "side": SIDE, + "robot": "h1", + "home": home, + "home_quat_xyzw": home_quat, + "x_range_raw_m": list(raw_x), + "y_range_raw_m": list(raw_y), + "z_range_raw_m": list(raw_z), + "x_range_m": [x_lo, x_hi], + "y_range_m": [y_lo, y_hi], + "z_range_m": [z_lo, z_hi], + "probes": all_probes, + "probe_step_m": PROBE_STEP_M, + "envelope_margin_m": ENVELOPE_MARGIN_M, + "refine_halvings": REFINE_HALVINGS, + "timestamp": time.strftime("%Y-%m-%dT%H:%M:%S"), + } + ws_path.write_text(json.dumps(workspace, indent=2)) + print(f"\n[2] workspace salvato → {ws_path}", flush=True) + print(f" raw reachable+visibile (actual): X={raw_x} Y={raw_y} Z={raw_z}", flush=True) + print(f" envelope (con margin {ENVELOPE_MARGIN_M*1000:.0f}mm): X=[{x_lo:.3f},{x_hi:.3f}] Y=[{y_lo:.3f},{y_hi:.3f}] Z=[{z_lo:.3f},{z_hi:.3f}]", flush=True) + else: + x_lo, x_hi = workspace["x_range_m"] + y_lo, y_hi = workspace["y_range_m"] + z_lo, z_hi = workspace["z_range_m"] + + print(f"\n[3] random calibration: target {N_SAMPLES} sample, dedup ≥{MIN_SAMPLE_SEP_M*1000:.0f}mm", flush=True) + go_home() + time.sleep(2.0) + print(f"clear samples: {clear_samples()}", flush=True) + + rng = random.Random(RANDOM_SEED) + accepted_actuals: list[list[float]] = [] + accepted_records: list[dict] = [] + attempts = 0 + max_attempts = N_SAMPLES * 4 + prev_actual = read_left_pos() + + while len(accepted_actuals) < N_SAMPLES and attempts < max_attempts: + attempts += 1 + x = rng.uniform(x_lo, x_hi) + y = rng.uniform(y_lo, y_hi) + z = rng.uniform(z_lo, z_hi) if z_hi > z_lo else home[2] + target = [x, y, z] + print(f"\n[sample {len(accepted_actuals)+1}/{N_SAMPLES}, attempt {attempts}] target={[round(v,3) for v in target]}", flush=True) + rec = move_and_observe(target, prev_actual=prev_actual) + actual = rec["actual"] + if rec["stuck"]: + print(" → stuck, skip (no sample API call)", flush=True) + continue + if not rec["found"]: + print(" → marker non visibile, skip", flush=True) + prev_actual = actual + continue + # dedup + too_close = [a for a in accepted_actuals if dist3(a, actual) < MIN_SAMPLE_SEP_M] + if too_close: + min_d = min(dist3(a, actual) for a in too_close) * 1000 + print(f" → duplicato (dist {min_d:.1f}mm da sample esistente), skip", flush=True) + prev_actual = actual + continue + sres = take_calib_sample() + if not sres.get("ok", True): + print(f" → sample API fallito: {sres}", flush=True) + prev_actual = actual + continue + accepted_actuals.append(actual) + accepted_records.append({"target": target, "actual": actual, "result": sres}) + prev_actual = actual + print(f" ✓ accept actual={[round(v,3) for v in actual]} count={sres.get('count','?')}", flush=True) + + print(f"\n→ {len(accepted_actuals)}/{N_SAMPLES} samples raccolti in {attempts} tentativi", flush=True) + workspace["calib_samples_xy"] = [[a[0], a[1]] for a in accepted_actuals] + workspace["calib_attempts"] = attempts + workspace["calib_accepted"] = len(accepted_actuals) + ws_path.write_text(json.dumps(workspace, indent=2)) + + print("\n[4] solve calibration", flush=True) + sol = solve_calib() + summary = {k: v for k, v in sol.items() if k not in ("samples",)} + print(json.dumps(summary, indent=2)[:2500], flush=True) + + print("\n[5] visualize", flush=True) + png_path = visualize(workspace) + print(f"PNG → {png_path}", flush=True) + + print("\n[6] go home final", flush=True) + go_home() + + print("\n=== DONE ===", flush=True) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/extend_calibration.py b/scripts/extend_calibration.py new file mode 100644 index 0000000..2ab9eea --- /dev/null +++ b/scripts/extend_calibration.py @@ -0,0 +1,211 @@ +"""Estende la calibrazione hand-eye accumulando nuovi sample random nell'envelope +finché il sanity_check del solver accetta almeno TARGET_GOOD sample. + +Riusa workspace_left.json e i sample già su disco (NON cancella). +Loop: pick random target → move (precise, slow) → marker check → dedup → sample API → solve → +controlla n_used. Se ≥ TARGET_GOOD si ferma. +""" + +from __future__ import annotations + +import json +import math +import random +import time +from pathlib import Path +from typing import Any + +import requests + +ORC = "http://localhost:8890" +H1 = "http://localhost:8767" +SIDE = "left" +QUAT_IDENT = [0.0, 0.0, 0.0, 1.0] + +TARGET_GOOD = 30 +MAX_TOTAL_SAMPLES = 80 +MIN_SAMPLE_SEP_M = 0.025 # un filo più stretto: con 24 già accumulati serve più packing + +# Movimento (uguale a explore_and_calibrate.py — H1 diretto) +PRECISE = True +SPEED_MMS = 40 +TRAJ_STEP_MM = 10 +SETTLE_S = 1.2 +MOVE_TIMEOUT_S = 25.0 +STUCK_DELTA_M = 0.030 +STUCK_TARGET_DIST_M = 0.100 + +RANDOM_SEED = 51 # seed diverso per non rigenerare gli stessi target + +DATA_DIR = Path("/opt/docker/robotserver/presa-mano/data") + + +def post(url: str, body: dict[str, Any] | None = None, timeout: float = 15.0) -> dict[str, Any]: + r = requests.post(url, json=body or {}, timeout=timeout) + try: + return r.json() + except Exception: + return {"status_code": r.status_code, "text": r.text[:500]} + + +def get(url: str, timeout: float = 5.0) -> dict[str, Any]: + r = requests.get(url, timeout=timeout) + return r.json() + + +def go_home() -> dict[str, Any]: + return post(f"{H1}/arms/go_neutral", {}) + + +def read_left_pos() -> list[float]: + return get(f"{ORC}/api/wrist")["left_wrist"]["position"] + + +def move_abs(pos: list[float]) -> dict[str, Any]: + body = { + "side": SIDE, + "position": list(pos), + "quaternion": list(QUAT_IDENT), + "wait": True, + "timeout_s": MOVE_TIMEOUT_S, + "precise": PRECISE, + "speed_mm_s": SPEED_MMS, + "traj_step_mm": TRAJ_STEP_MM, + } + return post(f"{H1}/wrist/absolute", body, timeout=MOVE_TIMEOUT_S + 5) + + +def detect_marker() -> dict[str, Any]: + return post(f"{ORC}/api/calibration/preview", {}, timeout=10) + + +def take_calib_sample() -> dict[str, Any]: + return post(f"{ORC}/api/calibration/sample", {"side": SIDE}, timeout=15) + + +def solve_calib() -> dict[str, Any]: + return post(f"{ORC}/api/calibration/solve", {"side": SIDE}, timeout=30) + + +def list_samples() -> list[dict]: + r = get(f"{ORC}/api/calibration/samples?side={SIDE}") + return r.get("samples", []) + + +def dist3(a, b) -> float: + return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b))) + + +def wrist_xyz_from_T(T: list[list[float]]) -> list[float]: + return [T[0][3], T[1][3], T[2][3]] + + +def main() -> int: + # 1) workspace + ws_path = DATA_DIR / "workspace_left.json" + if not ws_path.exists(): + print("workspace_left.json non trovato — esegui prima explore_and_calibrate.py", flush=True) + return 1 + ws = json.loads(ws_path.read_text()) + x_lo, x_hi = ws["x_range_m"] + y_lo, y_hi = ws["y_range_m"] + z_lo, z_hi = ws["z_range_m"] + home = ws["home"] + print(f"=== extend_calibration → target good ≥ {TARGET_GOOD} ===", flush=True) + print(f"envelope: X=[{x_lo:.3f},{x_hi:.3f}] Y=[{y_lo:.3f},{y_hi:.3f}] Z=[{z_lo:.3f},{z_hi:.3f}]", flush=True) + + # 2) seed dedup da sample esistenti + existing = list_samples() + accepted_actuals: list[list[float]] = [wrist_xyz_from_T(s["T_base_wrist"]) for s in existing] + print(f"sample esistenti su disco: {len(accepted_actuals)}", flush=True) + + # solve iniziale per baseline + sol = solve_calib() + n_used = sol.get("sanity_check", {}).get("n_used", 0) + n_input = sol.get("sanity_check", {}).get("n_input", len(existing)) + print(f"baseline solve: n_input={n_input}, n_used={n_used}, residual_trans_mm={sol.get('residual_translation_mm'):.1f}, residual_rot_deg={sol.get('residual_rotation_deg'):.2f}", flush=True) + + if n_used >= TARGET_GOOD: + print(f"già ≥ {TARGET_GOOD} good — niente da fare.", flush=True) + return 0 + + # 3) home + loop + go_home() + time.sleep(2.0) + rng = random.Random(RANDOM_SEED) + prev_actual = read_left_pos() + + while len(accepted_actuals) < MAX_TOTAL_SAMPLES: + target = [ + rng.uniform(x_lo, x_hi), + rng.uniform(y_lo, y_hi), + rng.uniform(z_lo, z_hi) if z_hi > z_lo else home[2], + ] + print(f"\n[total {len(accepted_actuals)+1} | good {n_used}/{TARGET_GOOD}] target={[round(v,3) for v in target]}", flush=True) + move_resp = move_abs(target) + time.sleep(SETTLE_S) + actual = read_left_pos() + + target_dist = dist3(target, prev_actual) + actual_change = dist3(actual, prev_actual) + if target_dist > STUCK_TARGET_DIST_M and actual_change < STUCK_DELTA_M: + print(" → stuck, skip", flush=True) + continue + + det = detect_marker() + if not det.get("found"): + print(" → marker non visibile, skip", flush=True) + prev_actual = actual + continue + + too_close = [a for a in accepted_actuals if dist3(a, actual) < MIN_SAMPLE_SEP_M] + if too_close: + md = min(dist3(a, actual) for a in too_close) * 1000 + print(f" → duplicato (dist {md:.1f}mm), skip", flush=True) + prev_actual = actual + continue + + sres = take_calib_sample() + if not sres.get("ok", True): + print(f" → sample API fallito: {sres}", flush=True) + prev_actual = actual + continue + + accepted_actuals.append(actual) + prev_actual = actual + print(f" ✓ accept actual={[round(v,3) for v in actual]} total={len(accepted_actuals)}", flush=True) + + # solve incrementale (rapido, ~5ms) + sol = solve_calib() + sc = sol.get("sanity_check", {}) + n_used = sc.get("n_used", 0) + print(f" solve: n_input={sc.get('n_input')}, n_used={n_used}, " + f"trans={sol.get('residual_translation_mm'):.1f}mm, rot={sol.get('residual_rotation_deg'):.2f}deg", flush=True) + if n_used >= TARGET_GOOD: + print(f"\n=== GOAL: n_used={n_used} ≥ {TARGET_GOOD} ===", flush=True) + break + else: + print(f"\n=== STOP: raggiunto MAX_TOTAL_SAMPLES={MAX_TOTAL_SAMPLES} senza arrivare a {TARGET_GOOD} good ===", flush=True) + + # 4) final summary + print("\n[final solve]", flush=True) + sol = solve_calib() + sc = sol.get("sanity_check", {}) + print(json.dumps({ + "n_input": sc.get("n_input"), + "n_used": sc.get("n_used"), + "used_indices": sc.get("used_indices"), + "dropped_by_pose_quality": [d.get("sample_index") for d in sc.get("dropped_by_pose_quality", [])], + "dropped_by_pair_angle": [d.get("sample_index") for d in sc.get("dropped_by_pair_angle", [])], + "dropped_by_wm_cluster": [d.get("sample_index") for d in sc.get("dropped_by_wm_cluster", [])], + "residual_translation_mm": sol.get("residual_translation_mm"), + "residual_rotation_deg": sol.get("residual_rotation_deg"), + }, indent=2), flush=True) + + print("\ngo home final", flush=True) + go_home() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/repeatability.py b/scripts/repeatability.py new file mode 100644 index 0000000..dc19de9 --- /dev/null +++ b/scripts/repeatability.py @@ -0,0 +1,104 @@ +"""Test ripetibilità marker-finder: polso fermo, N detection consecutive, +calcola deviazione standard di tvec e rotazione (rvec). Dirige se bias o noise.""" + +from __future__ import annotations + +import math +import statistics as stats +import time +from typing import Any + +import requests + +ORC = "http://localhost:8890" +H1 = "http://localhost:8767" +N_FRAMES = 30 +SETTLE_S = 0.5 # tra detection + + +def post(url, body=None, t=10): + r = requests.post(url, json=body or {}, timeout=t) + return r.json() + + +def get(url, t=5): + return requests.get(url, timeout=t).json() + + +def go_home(): + return post(f"{H1}/arms/go_neutral", {}) + + +def detect(): + return post(f"{ORC}/api/calibration/preview", {}) + + +def main() -> int: + print("=== repeatability test (left, H1) — N=", N_FRAMES, flush=True) + print("[0] go_neutral + settle", flush=True) + print(go_home(), flush=True) + time.sleep(3.0) + pos = get(f"{ORC}/api/wrist")["left_wrist"]["position"] + print(f"polso fermo a {[round(v,4) for v in pos]}", flush=True) + + tvecs: list[list[float]] = [] + rvecs: list[list[float]] = [] + quats: list[list[float]] = [] + centers: list[list[float]] = [] + timings: list[float] = [] + found = 0 + for i in range(N_FRAMES): + d = detect() + if not d.get("found"): + print(f" frame {i}: NOT FOUND", flush=True) + continue + found += 1 + pose = d["pose_camera"] + tvecs.append(pose["tvec"]) + rvecs.append(pose["rvec"]) + quats.append(pose["quaternion_xyzw"]) + centers.append(d["center_px"]) + timings.append(d.get("timings_ms", {}).get("marker_detect", 0)) + if i < 3 or i == N_FRAMES - 1: + print(f" frame {i}: tvec={[round(v,4) for v in pose['tvec']]}", flush=True) + time.sleep(SETTLE_S) + + print(f"\nfound: {found}/{N_FRAMES}", flush=True) + if found < 5: + return 1 + + def std(xs): + return stats.pstdev(xs) if len(xs) > 1 else 0.0 + + def fmt(label, vals): + return f" {label}: mean=[{vals[0]:+.4f},{vals[1]:+.4f},{vals[2]:+.4f}] std=[{std([t[0] for t in tvecs])*1000:.2f},{std([t[1] for t in tvecs])*1000:.2f},{std([t[2] for t in tvecs])*1000:.2f}]mm" + + mean_t = [sum(t[i] for t in tvecs) / len(tvecs) for i in range(3)] + std_t_mm = [std([t[i] for t in tvecs]) * 1000 for i in range(3)] + mean_r = [sum(r[i] for r in rvecs) / len(rvecs) for i in range(3)] + std_r_deg = [std([r[i] for r in rvecs]) * 180 / math.pi for i in range(3)] + mean_c = [sum(c[i] for c in centers) / len(centers) for i in range(2)] + std_c_px = [std([c[i] for c in centers]) for i in range(2)] + + radial_t = math.sqrt(sum(s ** 2 for s in std_t_mm)) + radial_r = math.sqrt(sum(s ** 2 for s in std_r_deg)) + + print("\n--- statistics su T_cam_marker (camera frame) ---", flush=True) + print(f"mean tvec : [{mean_t[0]:+.4f}, {mean_t[1]:+.4f}, {mean_t[2]:+.4f}] m", flush=True) + print(f"std tvec : [{std_t_mm[0]:.2f}, {std_t_mm[1]:.2f}, {std_t_mm[2]:.2f}] mm (radial {radial_t:.2f} mm)", flush=True) + print(f"std rvec : [{std_r_deg[0]:.3f}, {std_r_deg[1]:.3f}, {std_r_deg[2]:.3f}] deg (radial {radial_r:.3f} deg)", flush=True) + print(f"mean center px: [{mean_c[0]:.1f}, {mean_c[1]:.1f}] std: [{std_c_px[0]:.2f}, {std_c_px[1]:.2f}] px", flush=True) + print(f"detect latency mean: {sum(timings)/len(timings):.1f} ms", flush=True) + + print("\n--- diagnosi ---", flush=True) + if radial_t < 5.0: + print(f"σ_trans={radial_t:.1f}mm < 5mm → noise basso, residuo dominato da BIAS (verosimilmente intrinsics).", flush=True) + elif radial_t > 20.0: + print(f"σ_trans={radial_t:.1f}mm > 20mm → noise alto, marker o lighting.", flush=True) + else: + print(f"σ_trans={radial_t:.1f}mm intermedio: noise + bias entrambi presenti.", flush=True) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())