feat: calibration helper scripts + DANIILIDIS solver default + dual-robot README
- 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>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
1 parent
f6197488f9
commit
4981306062
5 files changed
+790
-16
No files matched your search
@@ -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())
|
||||
Reference in new issue
Block a user