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
@@ -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}
|
||||
@@ -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
|
||||
@@ -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)
|
||||
|
||||
Reference in new issue
Block a user