refactor: rimuove feature Auto-Calibrazione (tab + endpoint + job runner)
Elimina il flusso di acquisizione griglia automatica: tab GUI, modulo autocal3d.js, AutoRunner/auto_job, e gli endpoint /auto/* + /auto_grid. La calibrazione resta operabile via sample manuali + /solve. 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
4981306062
commit
0c211630cc
8 files changed
+3
-1241
No files matched your search
@@ -1,340 +0,0 @@
|
||||
"""Automated hand-eye calibration procedure.
|
||||
|
||||
Operator defines a 3D grid of wrist waypoints around the current pose plus a
|
||||
wrist rotation search delta. The job moves to each waypoint, tries to detect
|
||||
the ArUco marker, and if needed sweeps a 3x3 grid of wrist-frame rotations
|
||||
within the delta. Designed for a single in-flight job (global singleton) and
|
||||
polled status — the frontend renders the waypoints as spheres over the 3D
|
||||
model and updates their colours as the job progresses.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from typing import Any, Callable
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from ..clients import G1StatusError, G1Unavailable
|
||||
from ..geometry import R_to_quat_xyzw, quat_xyzw_to_R
|
||||
from .sampler import CalibrationSampler, Sample
|
||||
from .store import SamplesStore
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Waypoint:
|
||||
idx: int
|
||||
position: list[float] # base-frame xyz (m)
|
||||
quaternion: list[float] # xyzw
|
||||
reachable: bool = True
|
||||
reason: str | None = None # why not reachable, if applicable
|
||||
state: str = "pending" # pending | current | done | failed | skipped
|
||||
attempts: int = 0
|
||||
error: str | None = None
|
||||
sample_total: int | None = None # store length at the moment a sample was acquired
|
||||
|
||||
|
||||
@dataclass
|
||||
class JobState:
|
||||
status: str = "idle" # idle | running | done | stopped | error
|
||||
waypoints: list[Waypoint] = field(default_factory=list)
|
||||
current_index: int = -1
|
||||
started_at: float | None = None
|
||||
finished_at: float | None = None
|
||||
error: str | None = None
|
||||
calibration: dict[str, Any] | None = None
|
||||
stats: dict[str, int] = field(default_factory=lambda: {
|
||||
"done": 0, "failed": 0, "skipped": 0,
|
||||
})
|
||||
# Echoed params so the UI can regenerate visuals from status alone.
|
||||
params: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
def _frac(i: int, n: int) -> float:
|
||||
"""Return a [-0.5, +0.5] grid fraction for index i out of n divisions,
|
||||
or 0.0 when n == 1 (single slice in that axis)."""
|
||||
if n <= 1:
|
||||
return 0.0
|
||||
return (i / (n - 1)) - 0.5
|
||||
|
||||
|
||||
def build_grid(start_position: np.ndarray, start_quaternion: np.ndarray,
|
||||
delta_m: tuple[float, float, float],
|
||||
divisions: tuple[int, int, int],
|
||||
min_z_m: float,
|
||||
order: str = "serpentine") -> list[Waypoint]:
|
||||
"""Axis-aligned grid centred on `start_position`. delta_m is the full span
|
||||
per axis (grid covers start ± delta/2). divisions >= 1 per axis; 1 means
|
||||
"just the starting value on this axis".
|
||||
|
||||
order: "nested" = simple ix/iy/iz lexicographic; "serpentine" = boustrophedon,
|
||||
alternates x-direction per y-row and y-direction per z-layer to minimise
|
||||
total wrist travel between adjacent waypoints.
|
||||
"""
|
||||
waypoints: list[Waypoint] = []
|
||||
idx = 0
|
||||
nx, ny, nz = (max(1, d) for d in divisions)
|
||||
for iz in range(nz):
|
||||
iys = range(ny) if (order != "serpentine" or iz % 2 == 0) else range(ny - 1, -1, -1)
|
||||
for iy in iys:
|
||||
ixs = range(nx) if (order != "serpentine" or (iy + iz) % 2 == 0) else range(nx - 1, -1, -1)
|
||||
for ix in ixs:
|
||||
fx = _frac(ix, nx)
|
||||
fy = _frac(iy, ny)
|
||||
fz = _frac(iz, nz)
|
||||
x = float(start_position[0] + fx * delta_m[0])
|
||||
y = float(start_position[1] + fy * delta_m[1])
|
||||
z = float(start_position[2] + fz * delta_m[2])
|
||||
# Keep the original (unclamped) position so the operator can
|
||||
# see *where* the failing points actually fall — stacking them
|
||||
# all on the floor hides the problem (N z-layers collapsing
|
||||
# to 1 → apparent 9 points instead of 27).
|
||||
reason = None
|
||||
reachable = True
|
||||
if z < min_z_m:
|
||||
reason = f"sotto z_min ({z*100:.1f} < {min_z_m*100:.1f} cm)"
|
||||
reachable = False
|
||||
w = Waypoint(
|
||||
idx=idx,
|
||||
position=[x, y, z],
|
||||
quaternion=list(map(float, start_quaternion)),
|
||||
reachable=reachable,
|
||||
reason=reason,
|
||||
)
|
||||
waypoints.append(w)
|
||||
idx += 1
|
||||
return waypoints
|
||||
|
||||
|
||||
def perturb_quat(q_xyzw: list[float], drx: float, dry: float) -> list[float]:
|
||||
"""Apply a small rotation (drx around local X, dry around local Y) to the
|
||||
wrist-frame orientation described by q_xyzw, returning a new xyzw quat."""
|
||||
R_base = quat_xyzw_to_R(np.asarray(q_xyzw, dtype=np.float64))
|
||||
Rx, _ = cv2.Rodrigues(np.array([drx, 0.0, 0.0]))
|
||||
Ry, _ = cv2.Rodrigues(np.array([0.0, dry, 0.0]))
|
||||
R_new = R_base @ Ry @ Rx
|
||||
return R_to_quat_xyzw(R_new).tolist()
|
||||
|
||||
|
||||
def _rotation_search_order(delta_rad: float) -> list[tuple[float, float]]:
|
||||
"""Return (drx, dry) pairs to try in order of increasing |rotation|.
|
||||
Excludes (0, 0) since that's the nominal pose already attempted."""
|
||||
d = delta_rad
|
||||
if d <= 0:
|
||||
return []
|
||||
offsets = [(drx, dry) for drx in (-d, 0.0, d) for dry in (-d, 0.0, d)
|
||||
if (drx, dry) != (0.0, 0.0)]
|
||||
offsets.sort(key=lambda v: math.hypot(v[0], v[1]))
|
||||
return offsets
|
||||
|
||||
|
||||
class AutoRunner:
|
||||
"""Per-waypoint executor. One instance per job run, stateless between
|
||||
waypoints. Captures dependencies (G1/Nuwa clients, sampler, store)."""
|
||||
|
||||
def __init__(self, *, side: str, marker_id: int, size_mm: float,
|
||||
wrist_rot_delta_rad: float, settle_s: float,
|
||||
move_timeout_s: float,
|
||||
g1_move, sampler: CalibrationSampler, store: SamplesStore,
|
||||
feasibility_only: bool = False):
|
||||
self.side = side
|
||||
self.marker_id = marker_id
|
||||
self.size_mm = size_mm
|
||||
self.wrist_rot_delta_rad = wrist_rot_delta_rad
|
||||
self.settle_s = settle_s
|
||||
self.move_timeout_s = move_timeout_s
|
||||
self.g1_move = g1_move # bound method: wrist_absolute(...)
|
||||
self.sampler = sampler
|
||||
self.store = store
|
||||
self.feasibility_only = feasibility_only
|
||||
|
||||
def _try_sample(self, note: str) -> tuple[bool, str | None]:
|
||||
try:
|
||||
sample = self.sampler.acquire(
|
||||
marker_id=self.marker_id,
|
||||
size_mm=self.size_mm,
|
||||
side=self.side,
|
||||
note=note,
|
||||
)
|
||||
self.store.append(sample)
|
||||
return True, None
|
||||
except Exception as e:
|
||||
return False, str(e)
|
||||
|
||||
def _move(self, position: list[float], quaternion: list[float],
|
||||
timeout_s: float | None = None) -> tuple[bool, str | None]:
|
||||
t = timeout_s if timeout_s is not None else self.move_timeout_s
|
||||
try:
|
||||
r = self.g1_move(
|
||||
side=self.side, position=position, quaternion=quaternion,
|
||||
wait=True, timeout_s=t,
|
||||
)
|
||||
if isinstance(r, dict) and r.get("reached_target") is False:
|
||||
return False, f"reached_target=false (pos_err_mm={r.get('position_error_mm')})"
|
||||
return True, None
|
||||
except (G1Unavailable, G1StatusError) as e:
|
||||
return False, str(e)
|
||||
except Exception as e:
|
||||
return False, str(e)
|
||||
|
||||
def run(self, wp: Waypoint) -> dict[str, Any]:
|
||||
attempts = 1
|
||||
ok, err = self._move(wp.position, wp.quaternion)
|
||||
# Retry once with doubled timeout: reached_target=false with pos_err a
|
||||
# few cm away is usually solved by giving the controller more time to
|
||||
# settle, especially after big jumps between waypoints.
|
||||
if not ok:
|
||||
attempts = 2
|
||||
ok, err = self._move(wp.position, wp.quaternion,
|
||||
timeout_s=self.move_timeout_s * 2)
|
||||
if not ok:
|
||||
return {"ok": False, "attempts": attempts, "error": f"move failed: {err}"}
|
||||
# In feasibility-only mode we skip the settle+detect — reachability has
|
||||
# already been answered by the move itself (reached_target flag).
|
||||
if self.feasibility_only:
|
||||
return {"ok": True, "attempts": attempts, "sample_total": None}
|
||||
|
||||
time.sleep(self.settle_s)
|
||||
acquired, err = self._try_sample(note=f"auto[{wp.idx}]")
|
||||
if acquired:
|
||||
return {"ok": True, "attempts": attempts, "sample_total": len(self.store.load())}
|
||||
|
||||
# Marker not found → rotation search within ±delta.
|
||||
offsets = _rotation_search_order(self.wrist_rot_delta_rad)
|
||||
for k, (drx, dry) in enumerate(offsets, start=1):
|
||||
att = attempts + k
|
||||
q = perturb_quat(wp.quaternion, drx, dry)
|
||||
ok, move_err = self._move(wp.position, q)
|
||||
if not ok:
|
||||
continue
|
||||
time.sleep(self.settle_s)
|
||||
acquired, err = self._try_sample(
|
||||
note=f"auto[{wp.idx}]+rx{math.degrees(drx):+.0f}ry{math.degrees(dry):+.0f}")
|
||||
if acquired:
|
||||
return {"ok": True, "attempts": att, "sample_total": len(self.store.load())}
|
||||
|
||||
return {"ok": False, "attempts": attempts + len(offsets),
|
||||
"error": err or "marker non trovato dopo ricerca rotazione"}
|
||||
|
||||
|
||||
class AutoCalibrationJob:
|
||||
def __init__(self) -> None:
|
||||
self._lock = threading.Lock()
|
||||
self._thread: threading.Thread | None = None
|
||||
self._stop_event = threading.Event()
|
||||
self.state = JobState()
|
||||
|
||||
def is_running(self) -> bool:
|
||||
return self._thread is not None and self._thread.is_alive()
|
||||
|
||||
def snapshot(self) -> dict[str, Any]:
|
||||
with self._lock:
|
||||
return {
|
||||
"status": self.state.status,
|
||||
"current_index": self.state.current_index,
|
||||
"waypoints": [asdict(w) for w in self.state.waypoints],
|
||||
"started_at": self.state.started_at,
|
||||
"finished_at": self.state.finished_at,
|
||||
"error": self.state.error,
|
||||
"calibration": self.state.calibration,
|
||||
"stats": dict(self.state.stats),
|
||||
"params": dict(self.state.params),
|
||||
}
|
||||
|
||||
def stop(self) -> bool:
|
||||
if not self.is_running():
|
||||
return False
|
||||
self._stop_event.set()
|
||||
return True
|
||||
|
||||
def start(self, *, waypoints: list[Waypoint], runner: AutoRunner,
|
||||
on_finish: Callable[[JobState], None] | None = None,
|
||||
params: dict[str, Any] | None = None,
|
||||
home_pose: tuple[list[float], list[float]] | None = None) -> bool:
|
||||
if self.is_running():
|
||||
return False
|
||||
with self._lock:
|
||||
self.state = JobState(
|
||||
status="running", waypoints=waypoints,
|
||||
started_at=time.time(),
|
||||
params=params or {},
|
||||
)
|
||||
self._stop_event.clear()
|
||||
|
||||
def worker():
|
||||
try:
|
||||
stopped = False
|
||||
for wp in waypoints:
|
||||
if self._stop_event.is_set():
|
||||
with self._lock:
|
||||
self.state.status = "stopped"
|
||||
stopped = True
|
||||
break
|
||||
if not wp.reachable:
|
||||
with self._lock:
|
||||
wp.state = "skipped"
|
||||
self.state.stats["skipped"] += 1
|
||||
continue
|
||||
with self._lock:
|
||||
wp.state = "current"
|
||||
self.state.current_index = wp.idx
|
||||
result = runner.run(wp)
|
||||
with self._lock:
|
||||
wp.attempts = int(result.get("attempts", 1))
|
||||
wp.sample_total = result.get("sample_total")
|
||||
if result.get("ok"):
|
||||
wp.state = "done"
|
||||
self.state.stats["done"] += 1
|
||||
else:
|
||||
wp.state = "failed"
|
||||
wp.error = result.get("error")
|
||||
self.state.stats["failed"] += 1
|
||||
# On stop, try to return to the home pose so the robot isn't
|
||||
# stranded at an arbitrary waypoint. Best-effort: errors here
|
||||
# are logged but don't change the job status.
|
||||
if stopped and home_pose is not None:
|
||||
try:
|
||||
with self._lock:
|
||||
self.state.error = "returning to home…"
|
||||
runner._move(home_pose[0], home_pose[1])
|
||||
with self._lock:
|
||||
self.state.error = None
|
||||
except Exception as e:
|
||||
log.warning("return-to-home failed: %s", e)
|
||||
with self._lock:
|
||||
self.state.error = f"return-to-home failed: {e}"
|
||||
with self._lock:
|
||||
if self.state.status == "running":
|
||||
self.state.status = "done"
|
||||
self.state.finished_at = time.time()
|
||||
self.state.current_index = -1
|
||||
if on_finish:
|
||||
try:
|
||||
on_finish(self.state)
|
||||
except Exception:
|
||||
log.exception("on_finish callback failed")
|
||||
except Exception as e:
|
||||
log.exception("auto-calibration worker crashed")
|
||||
with self._lock:
|
||||
self.state.status = "error"
|
||||
self.state.error = str(e)
|
||||
self.state.finished_at = time.time()
|
||||
|
||||
self._thread = threading.Thread(target=worker, daemon=True, name="auto-calib")
|
||||
self._thread.start()
|
||||
return True
|
||||
|
||||
|
||||
# Module-level singleton — FastAPI routes access it via get_job().
|
||||
_JOB = AutoCalibrationJob()
|
||||
|
||||
|
||||
def get_job() -> AutoCalibrationJob:
|
||||
return _JOB
|
||||
Reference in new issue
Block a user