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