feat: initial commit — orchestratore presa-mano con AI assistant + dual-robot
Pipeline completa per calibrazione hand-eye e grasp planning su robot Unitree G1/H1 con camera depth Nuwa-HP60C. Esposta come FastAPI + GUI vanilla JS, con assistente AI (OpenRouter + tool calling curato) per pilotare il sistema in linguaggio naturale. Architettura: - FastAPI orchestrator (porta da .env, default 8890) che coordina 4 microservizi upstream: Nuwa-HP60C, marker-finder, YOLO, robot bridge. - Dual-robot: services.g1 e services.h1 in config.yaml; switchabili a runtime via PUT /api/home/robot. - Calibrazione hand-eye eye-to-hand split per-side (right/left). - Marker pose: marker-finder riceve gli intrinseci RGB della Nuwa al boot, PnP server-side con quei dati. Nessun re-PnP locale richiesto. - Depth-to-RGB: registrazione iterativa IR↔RGB per back-projection 3D corretta (Nuwa /api/measure usa coords IR; il nostro /api/perception/ measure_rgb prende pixel RGB e ritorna 3D nel frame RGB OpenCV). - Correzione affine sul depth bias (calibrabile via marker come ground-truth) — riduce errore Z da ~6% a ~1.5% nei test. - AI assistant: 16 tool curati (move_wrist_*, hand_action, get_marker[_3d], detect_object, calibration_*, set_active_robot…), system prompt dinamico che riflette robot attivo + marker config + addendum customizzabile da GUI. Temperature di default 0.1 per evitare invenzioni. GUI (7 tab): - Home: stato comunicazione device + selettore robot attivo G1/H1 - Assistente AI: chat con OpenRouter, history persistita lato browser, task riutilizzabili (prompt salvati), trace tool calls - Impostazioni AI: modello da combo curata, temperature, max iterations, prompt addendum, anteprima system prompt - Calibrazione, Auto-Calibrazione, TrovaUtensile, Servo Visuale, Vista 3D, Movimento EE — invariate dalle versioni precedenti Setup: - Variabili in presa-mano/.env (template in .env.example) - OPENROUTER_API_KEY richiesta solo per il tab Assistente AI - PRESA_MANO_PORT configurabile Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
commit
ac573505b7
121 files changed
+67199
No files matched your search
@@ -0,0 +1,5 @@
|
||||
from .handeye import HandEyeSolver
|
||||
from .sampler import CalibrationSampler, Sample
|
||||
from .store import CalibrationStore, SamplesStore
|
||||
|
||||
__all__ = ["HandEyeSolver", "CalibrationSampler", "Sample", "CalibrationStore", "SamplesStore"]
|
||||
@@ -0,0 +1,340 @@
|
||||
"""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
|
||||
@@ -0,0 +1,299 @@
|
||||
"""Eye-to-hand calibration: camera fixed, ArUco mounted on the wrist.
|
||||
|
||||
Given N samples (T_base_wrist_i, T_cam_marker_i), find T_base_camera (4x4).
|
||||
|
||||
OpenCV's `cv2.calibrateHandEye` is documented for the eye-in-hand case.
|
||||
For eye-to-hand (our case) we feed the *inverse* of the gripper->base transform,
|
||||
which turns the solver output into T_camera->base (camera-to-base).
|
||||
|
||||
See: https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from ..geometry import invert_T
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
_METHODS = {
|
||||
"TSAI": cv2.CALIB_HAND_EYE_TSAI,
|
||||
"PARK": cv2.CALIB_HAND_EYE_PARK,
|
||||
"HORAUD": cv2.CALIB_HAND_EYE_HORAUD,
|
||||
"ANDREFF": cv2.CALIB_HAND_EYE_ANDREFF,
|
||||
"DANIILIDIS": cv2.CALIB_HAND_EYE_DANIILIDIS,
|
||||
}
|
||||
|
||||
|
||||
def _rot_angle(R: np.ndarray) -> float:
|
||||
"""Rotation angle (radians) of a 3x3 rotation matrix."""
|
||||
cos = (np.trace(R) - 1.0) / 2.0
|
||||
return float(np.arccos(np.clip(cos, -1.0, 1.0)))
|
||||
|
||||
|
||||
class HandEyeSolver:
|
||||
@staticmethod
|
||||
def solve(samples: list[dict[str, Any]], method: str = "TSAI",
|
||||
sanity_check: bool = True,
|
||||
pair_angle_tol_deg: float = 8.0,
|
||||
wm_translation_mad_factor: float = 3.5,
|
||||
max_reprojection_px: float = 3.0,
|
||||
max_ambiguity_ratio: float = 0.90,
|
||||
max_drop_frac: float = 0.5) -> dict[str, Any]:
|
||||
"""Run cv2.calibrateHandEye on `samples`.
|
||||
|
||||
When sanity_check=True (default), apply two filters:
|
||||
* pair-angle check: pre-solver, independent of the unknown extrinsic.
|
||||
For any two samples i,j, the rotation angles |R_wrist_ij| and
|
||||
|R_cam_ij| must match (consequence of AX=XB). Samples whose median
|
||||
per-pair angle discrepancy exceeds `pair_angle_tol_deg` are dropped.
|
||||
Catches the ArUco planar-pose 180° flip ambiguity, which is the
|
||||
typical cause of residuals in the hundreds of mm / tens of degrees.
|
||||
* post-solver T_wrist_marker cluster check: the marker is rigid on the
|
||||
wrist, so T_wrist_marker should be constant across samples. Solve
|
||||
once, then drop samples whose translation component is > MAD*k from
|
||||
the median, then re-solve on the survivors.
|
||||
"""
|
||||
if len(samples) < 3:
|
||||
raise ValueError(f"need at least 3 samples, got {len(samples)}")
|
||||
|
||||
flag = _METHODS.get(method.upper(), cv2.CALIB_HAND_EYE_TSAI)
|
||||
|
||||
filter_report: dict[str, Any] = {"applied": bool(sanity_check)}
|
||||
|
||||
working = list(enumerate(samples)) # (original_idx, sample)
|
||||
|
||||
if sanity_check:
|
||||
kept, dropped_pose = _filter_pose_quality(
|
||||
working, max_reprojection_px=max_reprojection_px,
|
||||
max_ambiguity_ratio=max_ambiguity_ratio,
|
||||
max_drop_frac=max_drop_frac,
|
||||
)
|
||||
filter_report["max_reprojection_px"] = max_reprojection_px
|
||||
filter_report["max_ambiguity_ratio"] = max_ambiguity_ratio
|
||||
filter_report["dropped_by_pose_quality"] = dropped_pose
|
||||
working = kept
|
||||
|
||||
kept, dropped_pair = _filter_pair_angle(
|
||||
working, tol_rad=np.deg2rad(pair_angle_tol_deg),
|
||||
max_drop_frac=max_drop_frac - len(dropped_pose) / max(1, len(samples)),
|
||||
)
|
||||
filter_report["pair_angle_tol_deg"] = pair_angle_tol_deg
|
||||
filter_report["dropped_by_pair_angle"] = dropped_pair
|
||||
working = kept
|
||||
|
||||
if len(working) < 3:
|
||||
raise ValueError(
|
||||
f"after pre-filter only {len(working)} samples remain (need ≥3). "
|
||||
f"Dropped by pair-angle: {filter_report.get('dropped_by_pair_angle')}")
|
||||
|
||||
T_cam2base = _solve_once([s for _, s in working], flag)
|
||||
|
||||
if sanity_check:
|
||||
kept, dropped_wm = _filter_wm_cluster(
|
||||
working, T_cam2base, mad_factor=wm_translation_mad_factor,
|
||||
max_drop_frac=max_drop_frac - len(filter_report.get("dropped_by_pair_angle", [])) / max(1, len(samples)),
|
||||
)
|
||||
filter_report["wm_mad_factor"] = wm_translation_mad_factor
|
||||
filter_report["dropped_by_wm_cluster"] = dropped_wm
|
||||
if kept != working and len(kept) >= 3:
|
||||
working = kept
|
||||
T_cam2base = _solve_once([s for _, s in working], flag)
|
||||
|
||||
residual_mm, residual_rad, per_sample = _residuals(
|
||||
[s for _, s in working], T_cam2base
|
||||
)
|
||||
|
||||
if sanity_check:
|
||||
filter_report["n_input"] = len(samples)
|
||||
filter_report["n_used"] = len(working)
|
||||
filter_report["used_indices"] = [i for i, _ in working]
|
||||
|
||||
return {
|
||||
"T_base_camera": T_cam2base.tolist(),
|
||||
"method": method.upper(),
|
||||
"n_samples": len(working),
|
||||
"residual_translation_mm": residual_mm,
|
||||
"residual_rotation_deg": float(np.degrees(residual_rad)),
|
||||
"per_sample_residuals": per_sample,
|
||||
"sanity_check": filter_report,
|
||||
}
|
||||
|
||||
|
||||
def _solve_once(samples: list[dict[str, Any]], flag: int) -> np.ndarray:
|
||||
R_b2g, t_b2g, R_t2c, t_t2c = [], [], [], []
|
||||
for s in samples:
|
||||
T_bw = np.asarray(s["T_base_wrist"], dtype=np.float64)
|
||||
T_cm = np.asarray(s["T_cam_marker"], dtype=np.float64)
|
||||
# eye-to-hand trick: feed the inverse of base->wrist.
|
||||
T_wb = invert_T(T_bw)
|
||||
R_b2g.append(T_wb[:3, :3])
|
||||
t_b2g.append(T_wb[:3, 3])
|
||||
R_t2c.append(T_cm[:3, :3])
|
||||
t_t2c.append(T_cm[:3, 3])
|
||||
R_cam2base, t_cam2base = cv2.calibrateHandEye(
|
||||
R_b2g, t_b2g, R_t2c, t_t2c, method=flag
|
||||
)
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R_cam2base
|
||||
T[:3, 3] = t_cam2base.flatten()
|
||||
return T
|
||||
|
||||
|
||||
def _filter_pose_quality(working: list[tuple[int, dict[str, Any]]],
|
||||
max_reprojection_px: float,
|
||||
max_ambiguity_ratio: float,
|
||||
max_drop_frac: float) -> tuple[list[tuple[int, dict[str, Any]]],
|
||||
list[dict[str, Any]]]:
|
||||
"""Drop samples whose local PnP was low-confidence: high reprojection
|
||||
error (bad corner detection) or high ambiguity ratio (both planar
|
||||
solutions nearly equivalent, pose underdetermined)."""
|
||||
n = len(working)
|
||||
if n < 3:
|
||||
return working, []
|
||||
max_drops = max(0, int(np.floor(n * max_drop_frac)))
|
||||
scored: list[tuple[int, dict[str, Any], str, float]] = []
|
||||
for local_i, (orig_i, s) in enumerate(working):
|
||||
re = s.get("reprojection_error_px")
|
||||
amb = s.get("ambiguity_ratio")
|
||||
if re is not None and re > max_reprojection_px:
|
||||
scored.append((local_i, s, "reprojection_px", float(re)))
|
||||
elif amb is not None and amb > max_ambiguity_ratio:
|
||||
scored.append((local_i, s, "ambiguity_ratio", float(amb)))
|
||||
# Drop worst first, capped by max_drops.
|
||||
scored.sort(key=lambda x: -x[3])
|
||||
keep_mask = np.ones(n, dtype=bool)
|
||||
dropped: list[dict[str, Any]] = []
|
||||
for local_i, s, reason, val in scored:
|
||||
if len(dropped) >= max_drops:
|
||||
break
|
||||
keep_mask[local_i] = False
|
||||
orig_i = working[local_i][0]
|
||||
dropped.append({"sample_index": orig_i, reason: val})
|
||||
kept = [working[i] for i in range(n) if keep_mask[i]]
|
||||
return kept, dropped
|
||||
|
||||
|
||||
def _filter_pair_angle(working: list[tuple[int, dict[str, Any]]],
|
||||
tol_rad: float,
|
||||
max_drop_frac: float) -> tuple[list[tuple[int, dict[str, Any]]],
|
||||
list[dict[str, Any]]]:
|
||||
"""AX=XB implies the rotation *angle* of the motion (wrist->wrist) equals
|
||||
the rotation angle of the corresponding camera motion. Any sample that
|
||||
consistently breaks this across its pairings is bad (e.g. ArUco flipped).
|
||||
"""
|
||||
n = len(working)
|
||||
if n < 3:
|
||||
return working, []
|
||||
|
||||
R_w = [np.asarray(s["T_base_wrist"], dtype=np.float64)[:3, :3] for _, s in working]
|
||||
R_c = [np.asarray(s["T_cam_marker"], dtype=np.float64)[:3, :3] for _, s in working]
|
||||
|
||||
scores = np.zeros(n)
|
||||
for i in range(n):
|
||||
diffs = []
|
||||
for j in range(n):
|
||||
if i == j:
|
||||
continue
|
||||
dR_w = R_w[j] @ R_w[i].T
|
||||
dR_c = R_c[j] @ R_c[i].T
|
||||
diffs.append(abs(_rot_angle(dR_w) - _rot_angle(dR_c)))
|
||||
scores[i] = float(np.median(diffs)) if diffs else 0.0
|
||||
|
||||
dropped: list[dict[str, Any]] = []
|
||||
max_drops = max(0, int(np.floor(n * max_drop_frac)))
|
||||
# Drop in descending order of score while score > tol and under the cap.
|
||||
order = np.argsort(-scores)
|
||||
keep_mask = np.ones(n, dtype=bool)
|
||||
for idx in order:
|
||||
if len(dropped) >= max_drops:
|
||||
break
|
||||
if scores[idx] <= tol_rad:
|
||||
break
|
||||
keep_mask[idx] = False
|
||||
orig_i = working[idx][0]
|
||||
dropped.append({
|
||||
"sample_index": orig_i,
|
||||
"pair_angle_err_deg": float(np.degrees(scores[idx])),
|
||||
})
|
||||
kept = [working[i] for i in range(n) if keep_mask[i]]
|
||||
return kept, dropped
|
||||
|
||||
|
||||
def _filter_wm_cluster(working: list[tuple[int, dict[str, Any]]],
|
||||
T_base_cam: np.ndarray,
|
||||
mad_factor: float,
|
||||
max_drop_frac: float) -> tuple[list[tuple[int, dict[str, Any]]],
|
||||
list[dict[str, Any]]]:
|
||||
"""T_wrist_marker is rigid (marker bolted to the wrist). Its translation
|
||||
must be the same for every sample up to measurement noise. Drop outliers."""
|
||||
n = len(working)
|
||||
if n < 4:
|
||||
return working, []
|
||||
|
||||
ts = np.zeros((n, 3))
|
||||
for k, (_, s) in enumerate(working):
|
||||
T_bw = np.asarray(s["T_base_wrist"], dtype=np.float64)
|
||||
T_cm = np.asarray(s["T_cam_marker"], dtype=np.float64)
|
||||
T_wm = invert_T(T_bw) @ T_base_cam @ T_cm
|
||||
ts[k] = T_wm[:3, 3]
|
||||
|
||||
med = np.median(ts, axis=0)
|
||||
dist = np.linalg.norm(ts - med, axis=1)
|
||||
mad = float(np.median(np.abs(dist - np.median(dist))))
|
||||
# If MAD is tiny (all samples extremely consistent), pick an absolute floor
|
||||
# so we still drop gross outliers >5 cm without chasing numerical noise.
|
||||
thr = max(mad * mad_factor, 0.02) # metres
|
||||
|
||||
dropped: list[dict[str, Any]] = []
|
||||
max_drops = max(0, int(np.floor(n * max_drop_frac)))
|
||||
order = np.argsort(-dist)
|
||||
keep_mask = np.ones(n, dtype=bool)
|
||||
for idx in order:
|
||||
if len(dropped) >= max_drops:
|
||||
break
|
||||
if dist[idx] <= thr:
|
||||
break
|
||||
keep_mask[idx] = False
|
||||
orig_i = working[idx][0]
|
||||
dropped.append({
|
||||
"sample_index": orig_i,
|
||||
"wm_translation_dev_mm": float(dist[idx] * 1000.0),
|
||||
})
|
||||
kept = [working[i] for i in range(n) if keep_mask[i]]
|
||||
return kept, dropped
|
||||
|
||||
|
||||
def _residuals(samples: list[dict[str, Any]], T_base_cam: np.ndarray
|
||||
) -> tuple[float, float, list[dict[str, float]]]:
|
||||
"""Mean translation (mm) and rotation (rad) deviation of T_wrist_marker
|
||||
from sample 0 across all samples. Also returns per-sample deviations so
|
||||
the UI can highlight the worst offenders."""
|
||||
T_wm_list: list[np.ndarray] = []
|
||||
for s in samples:
|
||||
T_bw = np.asarray(s["T_base_wrist"], dtype=np.float64)
|
||||
T_cm = np.asarray(s["T_cam_marker"], dtype=np.float64)
|
||||
T_wm_list.append(invert_T(T_bw) @ T_base_cam @ T_cm)
|
||||
|
||||
T_ref = T_wm_list[0]
|
||||
per_sample: list[dict[str, float]] = []
|
||||
t_err, r_err = [], []
|
||||
for T in T_wm_list:
|
||||
dt_mm = float(np.linalg.norm(T[:3, 3] - T_ref[:3, 3]) * 1000.0)
|
||||
R = T[:3, :3] @ T_ref[:3, :3].T
|
||||
dr_rad = _rot_angle(R)
|
||||
per_sample.append({
|
||||
"translation_err_mm": dt_mm,
|
||||
"rotation_err_deg": float(np.degrees(dr_rad)),
|
||||
})
|
||||
if dt_mm > 0 or dr_rad > 0: # skip the reference (always 0)
|
||||
t_err.append(dt_mm)
|
||||
r_err.append(dr_rad)
|
||||
if not t_err:
|
||||
return 0.0, 0.0, per_sample
|
||||
return float(np.mean(t_err)), float(np.mean(r_err)), per_sample
|
||||
@@ -0,0 +1,136 @@
|
||||
"""Acquire a calibration sample.
|
||||
|
||||
One sample = (T_base_wrist from G1) + (T_cam_marker from ArUco detection).
|
||||
|
||||
Pose computation: we call marker-finder only for the 2D `corners` (unaffected
|
||||
by its baked D435i intrinsics) and rerun solvePnP locally with the real Nuwa
|
||||
RGB intrinsics. This sidesteps what `CLAUDE.md` flags as the dominant error
|
||||
source in hand-eye calibration on this stack.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..clients import G1Client, MarkerClient, NuwaClient
|
||||
from ..geometry import T_from_pos_quat, make_T
|
||||
from ..perception.aruco_pose import nuwa_intrinsics_to_matrix, solve_square_marker
|
||||
from ..timing import Stopwatch
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Sample:
|
||||
created_at: float
|
||||
side: str
|
||||
marker_id: int
|
||||
T_base_wrist: list[list[float]] # 4x4
|
||||
T_cam_marker: list[list[float]] # 4x4
|
||||
corners: list[list[float]]
|
||||
note: str = ""
|
||||
timings_ms: dict[str, float] | None = None
|
||||
# Diagnostics from the local pose solve (absent when we fell back to
|
||||
# marker-finder's pose).
|
||||
pose_source: str = "marker_finder" # "nuwa_intrinsics" | "marker_finder"
|
||||
reprojection_error_px: float | None = None
|
||||
ambiguity_ratio: float | None = None
|
||||
|
||||
@classmethod
|
||||
def make(cls, side: str, marker_id: int, T_base_wrist: np.ndarray,
|
||||
T_cam_marker: np.ndarray, corners: np.ndarray, note: str = "",
|
||||
timings_ms: dict[str, float] | None = None,
|
||||
pose_source: str = "marker_finder",
|
||||
reprojection_error_px: float | None = None,
|
||||
ambiguity_ratio: float | None = None) -> "Sample":
|
||||
return cls(
|
||||
created_at=time.time(),
|
||||
side=side,
|
||||
marker_id=int(marker_id),
|
||||
T_base_wrist=T_base_wrist.tolist(),
|
||||
T_cam_marker=T_cam_marker.tolist(),
|
||||
corners=np.asarray(corners).tolist(),
|
||||
note=note,
|
||||
timings_ms=timings_ms,
|
||||
pose_source=pose_source,
|
||||
reprojection_error_px=reprojection_error_px,
|
||||
ambiguity_ratio=ambiguity_ratio,
|
||||
)
|
||||
|
||||
|
||||
class SamplerError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
class CalibrationSampler:
|
||||
def __init__(self, nuwa: NuwaClient, marker: MarkerClient, g1: G1Client):
|
||||
self.nuwa = nuwa
|
||||
self.marker = marker
|
||||
self.g1 = g1
|
||||
|
||||
def acquire(self, marker_id: int, size_mm: float, side: str, note: str = "") -> Sample:
|
||||
sw = Stopwatch()
|
||||
# 1) wrist pose from G1
|
||||
with sw.step("g1_wrist"):
|
||||
try:
|
||||
wrist = self.g1.wrist()
|
||||
except Exception as e:
|
||||
raise SamplerError(f"cannot read wrist pose from G1: {e}") from e
|
||||
# G1 returns {'left_wrist': {...}, 'right_wrist': {...}}; tolerate {'left'/'right'} and {'data': ...}.
|
||||
body = wrist.get("data") if isinstance(wrist.get("data"), dict) else wrist
|
||||
pose = body.get(f"{side}_wrist") or body.get(side)
|
||||
if pose is None or "position" not in pose or "quaternion" not in pose:
|
||||
raise SamplerError(f"G1 /wrist response missing '{side}' pose: {wrist}")
|
||||
T_base_wrist = T_from_pos_quat(np.asarray(pose["position"]), np.asarray(pose["quaternion"]))
|
||||
|
||||
# 2) ArUco corners from marker-finder (2D corners are intrinsics-free).
|
||||
with sw.step("marker_detect"):
|
||||
result = self.marker.detect_url(
|
||||
markers=[{"id": int(marker_id), "size_mm": float(size_mm)}],
|
||||
image_url=self.nuwa.rgb_url(),
|
||||
)
|
||||
found = result.get("found", [])
|
||||
if not found:
|
||||
raise SamplerError(f"marker id={marker_id} not detected (not_found={result.get('not_found')})")
|
||||
entry = next((m for m in found if int(m["id"]) == int(marker_id)), None)
|
||||
if entry is None:
|
||||
raise SamplerError(f"marker id={marker_id} missing from 'found': {found}")
|
||||
corners = np.asarray(entry["corners"], dtype=np.float64)
|
||||
|
||||
# 3) Pose: solve locally with Nuwa RGB intrinsics (primary path).
|
||||
pose_source = "marker_finder"
|
||||
reproj_err = None
|
||||
ambiguity = None
|
||||
T_cam_marker = None
|
||||
try:
|
||||
with sw.step("nuwa_intrinsics"):
|
||||
intr = self.nuwa.intrinsics()
|
||||
K, dist = nuwa_intrinsics_to_matrix(intr)
|
||||
with sw.step("local_pnp"):
|
||||
sol = solve_square_marker(corners, float(size_mm) / 1000.0, K, dist)
|
||||
T_cam_marker = make_T(sol["R"], sol["t"])
|
||||
pose_source = "nuwa_intrinsics"
|
||||
reproj_err = sol["reprojection_error_px"]
|
||||
ambiguity = sol["ambiguity_ratio"]
|
||||
except Exception as e:
|
||||
log.warning("local pose failed (%s) — falling back to marker-finder pose", e)
|
||||
|
||||
# Fallback: use marker-finder's baked-in pose if local solve failed.
|
||||
if T_cam_marker is None:
|
||||
R = np.asarray(entry["rotation_matrix"], dtype=np.float64).reshape(3, 3)
|
||||
t = np.asarray(entry["position_m"], dtype=np.float64).flatten()
|
||||
T_cam_marker = make_T(R, t)
|
||||
|
||||
return Sample.make(
|
||||
side=side, marker_id=int(marker_id),
|
||||
T_base_wrist=T_base_wrist, T_cam_marker=T_cam_marker,
|
||||
corners=corners, note=note, timings_ms=sw.as_dict(),
|
||||
pose_source=pose_source,
|
||||
reprojection_error_px=reproj_err,
|
||||
ambiguity_ratio=ambiguity,
|
||||
)
|
||||
@@ -0,0 +1,65 @@
|
||||
"""Persistence for calibration samples (JSON) and result (YAML)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import time
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from .sampler import Sample
|
||||
|
||||
|
||||
class SamplesStore:
|
||||
def __init__(self, path: Path):
|
||||
self.path = Path(path)
|
||||
self.path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
def load(self) -> list[dict[str, Any]]:
|
||||
if not self.path.exists():
|
||||
return []
|
||||
with open(self.path, "r") as f:
|
||||
return json.load(f)
|
||||
|
||||
def save(self, samples: list[dict[str, Any]]) -> None:
|
||||
with open(self.path, "w") as f:
|
||||
json.dump(samples, f, indent=2)
|
||||
|
||||
def append(self, sample: Sample) -> list[dict[str, Any]]:
|
||||
items = self.load()
|
||||
items.append(asdict(sample))
|
||||
self.save(items)
|
||||
return items
|
||||
|
||||
def clear(self) -> None:
|
||||
self.save([])
|
||||
|
||||
|
||||
class CalibrationStore:
|
||||
def __init__(self, path: Path):
|
||||
self.path = Path(path)
|
||||
self.path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
def save(self, result: dict[str, Any]) -> None:
|
||||
doc = {
|
||||
"saved_at": time.time(),
|
||||
**result,
|
||||
}
|
||||
with open(self.path, "w") as f:
|
||||
yaml.safe_dump(doc, f, sort_keys=False)
|
||||
|
||||
def load(self) -> dict[str, Any] | None:
|
||||
if not self.path.exists():
|
||||
return None
|
||||
with open(self.path, "r") as f:
|
||||
return yaml.safe_load(f)
|
||||
|
||||
def T_base_camera(self) -> np.ndarray | None:
|
||||
doc = self.load()
|
||||
if not doc:
|
||||
return None
|
||||
return np.asarray(doc["T_base_camera"], dtype=np.float64)
|
||||
Reference in new issue
Block a user