diff --git a/CLAUDE.md b/CLAUDE.md index 05b75ea..16571fd 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -18,7 +18,7 @@ uv run uvicorn presa_mano.main:app --host 0.0.0.0 --port 8890 --reload Useful endpoints (all under `http://localhost:8890`): - `GET /apidocs` — Swagger UI; `GET /redoc` — ReDoc. - `GET/PUT /api/config` — read/patch runtime config (deep-merged into `config.yaml` and persisted). -- Calibration: `POST /api/calibration/sample`, `GET/DELETE /api/calibration/samples`, `POST /api/calibration/solve`, `GET /api/calibration/result`, `POST /api/calibration/preview`, `POST /api/calibration/auto_grid`. +- Calibration: `POST /api/calibration/sample`, `GET/DELETE /api/calibration/samples`, `POST /api/calibration/solve`, `GET /api/calibration/result`, `POST /api/calibration/preview`. - Perception: `POST /api/perception/detect`, `POST /api/perception/object_points`, `GET /api/perception/measure`. - Grasp: `POST /api/grasp/plan`, `POST /api/grasp/execute` (set `dry_run` to gate G1 mutations). - Wrist/EE: `GET /api/wrist`, `POST /api/wrist/{absolute,relative,hand,emergency_stop,unlock}`. @@ -84,7 +84,7 @@ Marker-finder caveat: it uses **D435i intrinsics baked into its config** that do ### Conventions to preserve -- **Dry-run semantics:** any G1-mutating endpoint takes a `dry_run` flag; `G1Client._post` short-circuits and logs `[DRY-RUN]` instead of hitting the network. `auto_grid` keeps `dry_run=True` by default (the G1 DDS has been unstable). Never silently strip dry-run from a mutating path. +- **Dry-run semantics:** any G1-mutating endpoint takes a `dry_run` flag; `G1Client._post` short-circuits and logs `[DRY-RUN]` instead of hitting the network. Never silently strip dry-run from a mutating path. - **Per-request client construction** (via `api/_deps.py`) is intentional — runtime `PUT /api/config` updates would otherwise be invisible. - Endpoints return `{ ..., "timings_ms": {step: ms, ..., total: ms} }`. New endpoints should follow this with `Stopwatch().step(name)`. - Calibration paths in config (`calibration.file`, `calibration.samples_file`) are resolved against `PRESA_MANO_DATA` when relative — see `_deps.calibration_file_path()` / `samples_file_path()`. diff --git a/src/presa_mano/api/calibration_routes.py b/src/presa_mano/api/calibration_routes.py index b3598c5..071fdf3 100644 --- a/src/presa_mano/api/calibration_routes.py +++ b/src/presa_mano/api/calibration_routes.py @@ -3,7 +3,6 @@ from __future__ import annotations import logging -import time from dataclasses import asdict from typing import Any, Optional @@ -12,8 +11,6 @@ from fastapi import APIRouter, HTTPException, Query, Response from pydantic import BaseModel, Field from ..calibration import CalibrationSampler, CalibrationStore, HandEyeSolver, SamplesStore -from ..calibration.auto_job import AutoRunner, build_grid, get_job as _auto_job -from ..clients import G1Unavailable from ..config import get_config from ..timing import Stopwatch from . import _deps @@ -181,294 +178,3 @@ def preview_marker(req: PreviewReq) -> dict[str, Any]: }, "timings_ms": sw.as_dict(), } - - -# --- Future: scripted grid acquisition -------------------------------------- - -class AutoGridReq(BaseModel): - """Automated calibration by visiting a 3D grid of wrist poses. - - Default: 3x3 (X,Y) at 3 heights = 27 waypoints. - Z is clamped so it never drops below `min_z_m` to avoid hitting the table. - """ - start_position: list[float] = Field(..., min_length=3, max_length=3) - orientation_quaternion_xyzw: list[float] = Field(..., min_length=4, max_length=4) - step_x_m: float = 0.05 - step_y_m: float = 0.05 - step_z_m: float = 0.05 - n_x: int = 3 - n_y: int = 3 - n_z: int = 3 - min_z_m: float = Field(..., description="floor/table safety — z is clamped to >= this value") - settle_s: float = 1.0 - side: Optional[str] = None - dry_run: bool = True - auto_solve: bool = True - - -@router.post("/auto_grid") -def auto_grid(req: AutoGridReq) -> dict[str, Any]: - """Move the wrist through a grid of waypoints, acquiring a sample at each. - - While the G1 DDS is unstable, keep dry_run=True to just preview the waypoints. - """ - cfg = get_config() - side = req.side or cfg["robot"]["wrist_side"] - g1 = _deps.g1(dry_run=req.dry_run) - sampler = CalibrationSampler(_deps.nuwa(), _deps.marker(), _deps.g1(dry_run=False)) - store = _samples_store(side) - - x0, y0, z0 = req.start_position - waypoints: list[list[float]] = [] - for iz in range(req.n_z): - for iy in range(req.n_y): - for ix in range(req.n_x): - x = x0 + (ix - (req.n_x - 1) / 2.0) * req.step_x_m - y = y0 + (iy - (req.n_y - 1) / 2.0) * req.step_y_m - z = max(req.min_z_m, z0 + iz * req.step_z_m) - waypoints.append([x, y, z]) - - results: list[dict[str, Any]] = [] - for i, pos in enumerate(waypoints): - try: - move = g1.wrist_absolute(side=side, position=pos, - quaternion=req.orientation_quaternion_xyzw, - wait=True, timeout_s=req.settle_s * 5) - except G1Unavailable as e: - results.append({"idx": i, "position": pos, "error": f"G1 move failed: {e}"}) - continue - time.sleep(req.settle_s) - if req.dry_run: - results.append({"idx": i, "position": pos, "move": move, "sample": None, "dry_run": True}) - continue - try: - sample = sampler.acquire( - marker_id=cfg["aruco"]["wrist_marker"]["id"], - size_mm=cfg["aruco"]["wrist_marker"]["size_mm"], - side=side, - note=f"auto_grid[{i}]", - ) - store.append(sample) - results.append({"idx": i, "position": pos, "move": move, "sample": asdict(sample)}) - except Exception as e: - log.warning("auto_grid sample %d failed: %s", i, e) - results.append({"idx": i, "position": pos, "move": move, "error": str(e)}) - - out: dict[str, Any] = {"waypoints": waypoints, "results": results, "dry_run": req.dry_run} - - if req.auto_solve and not req.dry_run: - samples = store.load() - if len(samples) >= int(cfg.get("calibration", {}).get("min_samples", 5)): - try: - method = (cfg.get("calibration", {}).get("method") or "TSAI").upper() - solve_result = HandEyeSolver.solve(samples, method=method) - _calib_store(side).save(solve_result) - out["calibration"] = solve_result - except Exception as e: - out["solve_error"] = str(e) - out["side"] = side - return out - - -# --- Automated calibration procedure --------------------------------------- -# Grid + wrist rotation search, background job with polled status. Frontend -# (tab "Auto-Calibrazione") renders the waypoints on top of the 3D URDF. - - -class AutoStartReq(BaseModel): - # Starting wrist pose: taken from G1 live if omitted. - start_position: Optional[list[float]] = Field(None, min_length=3, max_length=3) - start_quaternion_xyzw: Optional[list[float]] = Field(None, min_length=4, max_length=4) - # Grid span (mm) and number of divisions per axis. - delta_x_mm: float = Field(..., ge=0.0, le=500.0) - delta_y_mm: float = Field(..., ge=0.0, le=500.0) - delta_z_mm: float = Field(..., ge=0.0, le=500.0) - # Offset (mm) applied to the live wrist pose before centring the grid. - # Lets the operator displace the grid centre from the current pose - # without physically moving the wrist first. - offset_x_mm: float = Field(0.0, ge=-500.0, le=500.0) - offset_y_mm: float = Field(0.0, ge=-500.0, le=500.0) - offset_z_mm: float = Field(0.0, ge=-500.0, le=500.0) - n_x: int = Field(3, ge=1, le=7) - n_y: int = Field(3, ge=1, le=7) - n_z: int = Field(3, ge=1, le=7) - # Wrist rotation search range (degrees) when marker not found at nominal pose. - wrist_rot_delta_deg: float = Field(10.0, ge=0.0, le=45.0) - min_z_m: float = 0.05 - side: Optional[str] = None - settle_s: float = Field(1.0, ge=0.1, le=5.0) - move_timeout_s: float = Field(8.0, ge=1.0, le=30.0) - auto_solve: bool = True - dry_run: bool = False - order: str = Field("serpentine", pattern="^(nested|serpentine)$") - - -class AutoFeasibilityReq(AutoStartReq): - """Same shape as start, used only to preview the grid.""" - pass - - -class AutoVerifyReq(AutoStartReq): - """Real reachability verification — same grid, robot moves to each point - but does NOT acquire samples. Faster than full procedure, definitive.""" - pass - - -def _resolve_start_pose(req: AutoStartReq, side: str) -> tuple[list[float], list[float]]: - """If caller did not supply a start pose, read the current wrist pose from G1.""" - if req.start_position and req.start_quaternion_xyzw: - return list(req.start_position), list(req.start_quaternion_xyzw) - try: - w = _deps.g1().wrist() - except Exception as e: - raise HTTPException(status_code=502, detail=f"cannot read wrist pose: {e}") - body = w.get("data") if isinstance(w.get("data"), dict) else w - 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 HTTPException(status_code=422, detail=f"G1 /wrist missing '{side}' pose") - return list(pose["position"]), list(pose["quaternion"]) - - -def _grid_from_req(req: AutoStartReq, side: str) -> tuple[list, list[float], list[float], list[float]]: - """Returns (grid, home_position, home_quaternion, center_position). - - home_pose is the actual live wrist pose — used for return-to-home on stop. - center_position is home_position + (offset_x, offset_y, offset_z), which - is what the grid is actually centred on.""" - home_p, home_q = _resolve_start_pose(req, side) - center_p = [ - home_p[0] + req.offset_x_mm / 1000.0, - home_p[1] + req.offset_y_mm / 1000.0, - home_p[2] + req.offset_z_mm / 1000.0, - ] - grid = build_grid( - start_position=np.asarray(center_p), - start_quaternion=np.asarray(home_q), - delta_m=(req.delta_x_mm / 1000.0, req.delta_y_mm / 1000.0, req.delta_z_mm / 1000.0), - divisions=(req.n_x, req.n_y, req.n_z), - min_z_m=req.min_z_m, - order=req.order, - ) - return grid, home_p, home_q, center_p - - -@router.post("/auto/feasibility") -def auto_feasibility(req: AutoFeasibilityReq) -> dict[str, Any]: - """Build the grid and return per-point reachability — no motion.""" - from dataclasses import asdict as _asdict - cfg = get_config() - side = req.side or cfg["robot"]["wrist_side"] - grid, home_p, home_q, center_p = _grid_from_req(req, side) - return { - "side": side, - "start_position": home_p, - "start_quaternion_xyzw": home_q, - "center_position": center_p, - "offset_mm": [req.offset_x_mm, req.offset_y_mm, req.offset_z_mm], - "wrist_rot_delta_deg": req.wrist_rot_delta_deg, - "waypoints": [_asdict(w) for w in grid], - "n_reachable": sum(1 for w in grid if w.reachable), - "n_unreachable": sum(1 for w in grid if not w.reachable), - } - - -def _make_runner(req: AutoStartReq, side: str, *, feasibility_only: bool): - cfg = get_config() - return AutoRunner( - side=side, - marker_id=int(cfg["aruco"]["wrist_marker"]["id"]), - size_mm=float(cfg["aruco"]["wrist_marker"]["size_mm"]), - wrist_rot_delta_rad=float(np.deg2rad(req.wrist_rot_delta_deg)), - settle_s=req.settle_s, - move_timeout_s=req.move_timeout_s, - g1_move=_deps.g1(dry_run=req.dry_run).wrist_absolute, - sampler=CalibrationSampler(_deps.nuwa(), _deps.marker(), _deps.g1(dry_run=False)), - store=_samples_store(side), - feasibility_only=feasibility_only, - ) - - -@router.post("/auto/start") -def auto_start(req: AutoStartReq) -> dict[str, Any]: - job = _auto_job() - if job.is_running(): - raise HTTPException(status_code=409, detail="another auto job is already running") - cfg = get_config() - side = req.side or cfg["robot"]["wrist_side"] - grid, home_p, home_q, center_p = _grid_from_req(req, side) - runner = _make_runner(req, side, feasibility_only=False) - - def on_finish(state) -> None: - if not req.auto_solve: - return - samples = _samples_store(side).load() - min_n = int(cfg.get("calibration", {}).get("min_samples", 5)) - if len(samples) < min_n: - return - try: - method = (cfg.get("calibration", {}).get("method") or "TSAI").upper() - result = HandEyeSolver.solve(samples, method=method) - _calib_store(side).save(result) - state.calibration = result - except Exception as e: - log.warning("auto_solve after auto-calibration failed: %s", e) - - params = { - "side": side, - "start_position": home_p, - "start_quaternion_xyzw": home_q, - "center_position": center_p, - "offset_mm": [req.offset_x_mm, req.offset_y_mm, req.offset_z_mm], - "delta_mm": [req.delta_x_mm, req.delta_y_mm, req.delta_z_mm], - "divisions": [req.n_x, req.n_y, req.n_z], - "wrist_rot_delta_deg": req.wrist_rot_delta_deg, - "min_z_m": req.min_z_m, - "dry_run": req.dry_run, - "auto_solve": req.auto_solve, - "order": req.order, - "mode": "calibration", - } - job.start(waypoints=grid, runner=runner, on_finish=on_finish, params=params, - home_pose=(home_p, home_q)) - return {"ok": True, "status": "running", "n_waypoints": len(grid), "params": params} - - -@router.post("/auto/verify_reachability") -def auto_verify_reachability(req: AutoVerifyReq) -> dict[str, Any]: - """Run the robot through the grid WITHOUT acquiring samples. The reached_target - flag from each move is the ground truth for reachability — slow but definitive.""" - job = _auto_job() - if job.is_running(): - raise HTTPException(status_code=409, detail="another auto job is already running") - cfg = get_config() - side = req.side or cfg["robot"]["wrist_side"] - grid, home_p, home_q, center_p = _grid_from_req(req, side) - runner = _make_runner(req, side, feasibility_only=True) - params = { - "side": side, - "start_position": home_p, - "start_quaternion_xyzw": home_q, - "center_position": center_p, - "offset_mm": [req.offset_x_mm, req.offset_y_mm, req.offset_z_mm], - "delta_mm": [req.delta_x_mm, req.delta_y_mm, req.delta_z_mm], - "divisions": [req.n_x, req.n_y, req.n_z], - "order": req.order, - "dry_run": req.dry_run, - "mode": "verify_reachability", - } - job.start(waypoints=grid, runner=runner, params=params, - home_pose=(home_p, home_q)) - return {"ok": True, "status": "running", "n_waypoints": len(grid), "params": params} - - -@router.get("/auto/status") -def auto_status() -> dict[str, Any]: - return _auto_job().snapshot() - - -@router.post("/auto/stop") -def auto_stop() -> dict[str, Any]: - """Stop the current job. If the job was started with a home_pose, the - worker returns the wrist there before finishing.""" - stopped = _auto_job().stop() - return {"ok": True, "was_running": stopped} diff --git a/src/presa_mano/calibration/auto_job.py b/src/presa_mano/calibration/auto_job.py deleted file mode 100644 index 56693c7..0000000 --- a/src/presa_mano/calibration/auto_job.py +++ /dev/null @@ -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 diff --git a/src/presa_mano/main.py b/src/presa_mano/main.py index 15c888e..fe77461 100644 --- a/src/presa_mano/main.py +++ b/src/presa_mano/main.py @@ -101,7 +101,7 @@ def index() -> HTMLResponse: idx = WEB_DIR / "index.html" html = idx.read_text(encoding="utf-8") # Cache-bust app.js and style.css with their mtime so a rebuild forces refetch. - for name in ("app.js", "autocal3d.js", "servo.js", "style.css"): + for name in ("app.js", "servo.js", "style.css"): f = WEB_DIR / name if f.exists(): v = int(f.stat().st_mtime) diff --git a/web/autocal3d.js b/web/autocal3d.js deleted file mode 100644 index b22d206..0000000 --- a/web/autocal3d.js +++ /dev/null @@ -1,506 +0,0 @@ -/* Auto-calibration tab: reuses G1Viewer for the 3D URDF, overlays the - * calibration waypoints as colored spheres + triads (target orientation), - * orchestrates Calcola / Verifica reale / Start / Stop flow, polls - * /api/calibration/auto/status during execution, shows a per-waypoint attempt - * bar chart, and persists form settings in localStorage so the user can - * re-run the same calibration without re-typing. - */ - -import * as THREE from 'three'; -import { G1Viewer } from './diagnostic3d.js'; - -const $ = (id) => document.getElementById(id); - -const STATE_COLORS = { - pending: 0xaaaaaa, - unreach: 0xe74c3c, - skipped: 0x8a3a33, - current: 0x4da3ff, - done: 0x3ecf8e, - failed: 0xf0ad4e, -}; - -const LS_KEY = 'presaMano.autocalParams.v1'; - -let viewer = null; -let booted = false; -let pollTimer = null; -let waypointGroup = null; -let waypointItems = new Map(); // idx -> {sphere, triad} -let hoverTooltip = null; -let lastSnapshot = null; -let showTriads = true; - -async function boot() { - if (booted) return; - booted = true; - const canvas = $('autocalCanvas'); - if (!canvas) return; - $('autocalStatus').textContent = 'caricamento URDF…'; - try { - viewer = new G1Viewer(canvas); - await viewer.load(); - // Hide the per-motor spheres here — user doesn't want them in this tab. - viewer.setShowSpheres(false); - $('autocalStatus').textContent = 'URDF caricato · imposta parametri e clicca Calcola griglia'; - waypointGroup = new THREE.Group(); - waypointGroup.name = 'autocal:waypoints'; - viewer.scene.add(waypointGroup); - installHoverTooltip(); - // Expose hook so app.js powerPollOnce can push /diagnostics (live robot motion). - window.g1AutocalViewer = viewer; - await previewGrid().catch(() => {}); - } catch (e) { - $('autocalStatus').textContent = 'errore: ' + e.message; - console.error(e); - } -} - -function installHoverTooltip() { - hoverTooltip = document.createElement('div'); - hoverTooltip.className = 'diag3d-tooltip hidden'; - hoverTooltip.id = 'autocalTooltip'; - viewer.canvas.parentElement.appendChild(hoverTooltip); - const raycaster = new THREE.Raycaster(); - const mouse = new THREE.Vector2(); - viewer.canvas.addEventListener('pointermove', (ev) => { - if (!waypointItems.size) { hoverTooltip.classList.add('hidden'); return; } - const rect = viewer.canvas.getBoundingClientRect(); - mouse.x = ((ev.clientX - rect.left) / rect.width) * 2 - 1; - mouse.y = -((ev.clientY - rect.top) / rect.height) * 2 + 1; - raycaster.setFromCamera(mouse, viewer.camera); - const spheres = [...waypointItems.values()].map(it => it.sphere); - const hits = raycaster.intersectObjects(spheres, false); - if (!hits.length) { hoverTooltip.classList.add('hidden'); return; } - const w = hits[0].object.userData.waypoint; - hoverTooltip.className = 'diag3d-tooltip ' + - (w.state === 'failed' || !w.reachable ? 'err' : ''); - const rows = [ - ['idx', '#' + w.idx], - ['stato', w.state + (!w.reachable ? ' (unreach)' : '')], - ['x (m)', Number(w.position[0]).toFixed(3)], - ['y (m)', Number(w.position[1]).toFixed(3)], - ['z (m)', Number(w.position[2]).toFixed(3)], - ]; - if (w.attempts) rows.push(['attempts', w.attempts]); - if (w.error) rows.push(['error', w.error.slice(0, 60)]); - if (w.reason) rows.push(['motivo', w.reason]); - hoverTooltip.innerHTML = `
| totale | ${total} |
| raggiungibili | ${reach} |
| non raggiungibili | ${unreach} |
| campioni ok | ${done} |
| falliti | ${failed} |
| saltati | ${skipped} |
| tentativi medi | ${avgAtt} |
| tentativi max | ${maxAtt} |
| corrente | ${current != null ? '#' + current : '—'} |
| stato job | ${snapshot ? snapshot.status : '—'} |
—-