Presa-Mano/scripts/extend_calibration.py

"""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())