Adriano Dal PastroandClaude Opus 4.7
4981306062
- scripts/: explore_and_calibrate.py (probe envelope from actual + random sampling), extend_calibration.py (loop fino a n_used >= TARGET_GOOD), repeatability.py (diagnostica bias vs noise sul marker-finder) - config.yaml: calibration.method TSAI → DANIILIDIS (sweep su 49 sample H1 sx mostra 4× meno residuo di TSAI per via del bias intrinsics-dipendente di marker-finder) - README: dual-robot G1/H1, scripts dir, caveat noti (wrist passthrough, marker-finder intrinsics floor, H1 4 DoF, /api/arms/go_neutral non proxato) Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
105 lines
3.8 KiB
Python
105 lines
3.8 KiB
Python
"""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())
|