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
@@ -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()`.
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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 = `<div class="tt-title">waypoint #${w.idx}</div>` +
|
||||
rows.map(([k, v]) => `<div class="tt-row"><span class="k">${k}</span><span>${v}</span></div>`).join('');
|
||||
hoverTooltip.style.left = (ev.clientX - rect.left + 14) + 'px';
|
||||
hoverTooltip.style.top = (ev.clientY - rect.top + 14) + 'px';
|
||||
hoverTooltip.classList.remove('hidden');
|
||||
});
|
||||
}
|
||||
|
||||
function colorForWaypoint(w) {
|
||||
if (!w.reachable) return STATE_COLORS.unreach;
|
||||
switch (w.state) {
|
||||
case 'current': return STATE_COLORS.current;
|
||||
case 'done': return STATE_COLORS.done;
|
||||
case 'failed': return STATE_COLORS.failed;
|
||||
case 'skipped': return STATE_COLORS.skipped;
|
||||
default: return STATE_COLORS.pending;
|
||||
}
|
||||
}
|
||||
|
||||
// Quaternion (xyzw) → 3-axis triad: 3 short coloured segments, length = 4 cm.
|
||||
function makeTriad() {
|
||||
const L = 0.04;
|
||||
const geom = new THREE.BufferGeometry();
|
||||
const verts = new Float32Array([
|
||||
0,0,0, L,0,0, // X
|
||||
0,0,0, 0,L,0, // Y
|
||||
0,0,0, 0,0,L, // Z
|
||||
]);
|
||||
const colors = new Float32Array([
|
||||
1,0.2,0.2, 1,0.2,0.2,
|
||||
0.2,1,0.3, 0.2,1,0.3,
|
||||
0.3,0.6,1, 0.3,0.6,1,
|
||||
]);
|
||||
geom.setAttribute('position', new THREE.BufferAttribute(verts, 3));
|
||||
geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
|
||||
const mat = new THREE.LineBasicMaterial({ vertexColors: true, linewidth: 2, depthTest: false, transparent: true, opacity: 0.9 });
|
||||
const lines = new THREE.LineSegments(geom, mat);
|
||||
lines.renderOrder = 998;
|
||||
return lines;
|
||||
}
|
||||
|
||||
function renderWaypoints(waypoints) {
|
||||
if (!waypointGroup) return;
|
||||
const seen = new Set();
|
||||
for (const w of waypoints) {
|
||||
seen.add(w.idx);
|
||||
let item = waypointItems.get(w.idx);
|
||||
if (!item) {
|
||||
const geom = new THREE.SphereGeometry(0.012, 18, 14);
|
||||
const mat = new THREE.MeshStandardMaterial({
|
||||
color: colorForWaypoint(w),
|
||||
emissive: 0x111111,
|
||||
depthTest: false,
|
||||
transparent: true,
|
||||
opacity: 0.85,
|
||||
});
|
||||
const sphere = new THREE.Mesh(geom, mat);
|
||||
sphere.renderOrder = 999;
|
||||
const triad = makeTriad();
|
||||
const group = new THREE.Group();
|
||||
group.add(sphere);
|
||||
group.add(triad);
|
||||
waypointGroup.add(group);
|
||||
item = { group, sphere, triad };
|
||||
waypointItems.set(w.idx, item);
|
||||
}
|
||||
item.group.position.set(w.position[0], w.position[1], w.position[2]);
|
||||
const q = new THREE.Quaternion(w.quaternion[0], w.quaternion[1], w.quaternion[2], w.quaternion[3]);
|
||||
item.triad.quaternion.copy(q);
|
||||
item.triad.visible = showTriads;
|
||||
item.sphere.material.color.setHex(colorForWaypoint(w));
|
||||
item.sphere.userData.waypoint = w;
|
||||
if (w.state === 'current') {
|
||||
item.sphere.material.emissive.setHex(0x1e5a8a);
|
||||
item.sphere.material.emissiveIntensity = 0.7;
|
||||
const s = 1.0 + 0.2 * Math.sin(Date.now() / 120);
|
||||
item.sphere.scale.setScalar(s);
|
||||
} else {
|
||||
item.sphere.material.emissive.setHex(0x000000);
|
||||
item.sphere.material.emissiveIntensity = 0;
|
||||
item.sphere.scale.setScalar(1.0);
|
||||
}
|
||||
}
|
||||
// Remove stale.
|
||||
for (const [idx, item] of waypointItems.entries()) {
|
||||
if (!seen.has(idx)) {
|
||||
waypointGroup.remove(item.group);
|
||||
item.sphere.geometry.dispose();
|
||||
item.sphere.material.dispose();
|
||||
item.triad.geometry.dispose();
|
||||
item.triad.material.dispose();
|
||||
waypointItems.delete(idx);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function readParams() {
|
||||
const side = $('acSide').value || null;
|
||||
return {
|
||||
delta_x_mm: +$('acDx').value,
|
||||
delta_y_mm: +$('acDy').value,
|
||||
delta_z_mm: +$('acDz').value,
|
||||
offset_x_mm: +$('acOx').value,
|
||||
offset_y_mm: +$('acOy').value,
|
||||
offset_z_mm: +$('acOz').value,
|
||||
n_x: +$('acNx').value,
|
||||
n_y: +$('acNy').value,
|
||||
n_z: +$('acNz').value,
|
||||
wrist_rot_delta_deg: +$('acRotDeg').value,
|
||||
min_z_m: +$('acMinZ').value,
|
||||
settle_s: +$('acSettle').value,
|
||||
side: side || undefined,
|
||||
auto_solve: $('acAutoSolve').checked,
|
||||
dry_run: $('acDryRun').checked,
|
||||
order: $('acOrder').value,
|
||||
};
|
||||
}
|
||||
|
||||
function persistParams() {
|
||||
try { localStorage.setItem(LS_KEY, JSON.stringify(readParams())); } catch {}
|
||||
}
|
||||
function restoreParams() {
|
||||
try {
|
||||
const raw = localStorage.getItem(LS_KEY);
|
||||
if (!raw) return;
|
||||
const p = JSON.parse(raw);
|
||||
const setV = (id, v) => { if ($(id) && v != null) $(id).value = v; };
|
||||
const setC = (id, v) => { if ($(id) && v != null) $(id).checked = !!v; };
|
||||
setV('acDx', p.delta_x_mm); setV('acDy', p.delta_y_mm); setV('acDz', p.delta_z_mm);
|
||||
setV('acOx', p.offset_x_mm); setV('acOy', p.offset_y_mm); setV('acOz', p.offset_z_mm);
|
||||
setV('acNx', p.n_x); setV('acNy', p.n_y); setV('acNz', p.n_z);
|
||||
setV('acRotDeg', p.wrist_rot_delta_deg); setV('acMinZ', p.min_z_m); setV('acSettle', p.settle_s);
|
||||
if (p.side) setV('acSide', p.side);
|
||||
setC('acAutoSolve', p.auto_solve);
|
||||
setC('acDryRun', p.dry_run);
|
||||
if (p.order) setV('acOrder', p.order);
|
||||
} catch (e) { console.warn('restoreParams:', e); }
|
||||
}
|
||||
|
||||
async function api(method, path, body = null) {
|
||||
const opts = { method, headers: {} };
|
||||
if (body !== null) {
|
||||
opts.headers['Content-Type'] = 'application/json';
|
||||
opts.body = JSON.stringify(body);
|
||||
}
|
||||
const r = await fetch(path, opts);
|
||||
const text = await r.text();
|
||||
let data; try { data = JSON.parse(text); } catch { data = text; }
|
||||
if (!r.ok) throw new Error(`${r.status} ${(data && data.detail) || text}`);
|
||||
return data;
|
||||
}
|
||||
|
||||
async function previewGrid() {
|
||||
persistParams();
|
||||
const body = readParams();
|
||||
setStatus('Calcolo griglia…');
|
||||
try {
|
||||
const r = await api('POST', '/api/calibration/auto/feasibility', body);
|
||||
renderWaypoints(r.waypoints);
|
||||
drawChart(r.waypoints);
|
||||
updateStats(r.waypoints, null);
|
||||
const reach = r.waypoints.filter(w => w.reachable).length;
|
||||
setStatus(`griglia: ${r.waypoints.length} punti · ${reach} raggiungibili · ${r.waypoints.length - reach} scartati`);
|
||||
$('acStartBtn').disabled = reach === 0;
|
||||
$('acVerifyBtn').disabled = reach === 0;
|
||||
} catch (e) {
|
||||
setStatus('errore: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
async function startProcedure() {
|
||||
persistParams();
|
||||
const body = readParams();
|
||||
const reach = [...waypointItems.values()].filter(it => it.sphere.userData.waypoint.reachable).length;
|
||||
if (!reach) return;
|
||||
if (!body.dry_run && !confirm(`Avvio calibrazione su ${reach} punti. Il robot si muoverà. Confermi?`)) return;
|
||||
try {
|
||||
setStatus('avvio procedura…');
|
||||
await api('POST', '/api/calibration/auto/start', body);
|
||||
$('acStartBtn').disabled = true;
|
||||
$('acVerifyBtn').disabled = true;
|
||||
$('acStopBtn').disabled = false;
|
||||
startPolling();
|
||||
} catch (e) {
|
||||
setStatus('start fallito: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
async function verifyReachability() {
|
||||
persistParams();
|
||||
const body = readParams();
|
||||
const reach = [...waypointItems.values()].filter(it => it.sphere.userData.waypoint.reachable).length;
|
||||
if (!reach) return;
|
||||
if (!body.dry_run && !confirm(`Verifica raggiungibilità reale: il robot si muoverà a ${reach} punti senza acquisire campioni. Confermi?`)) return;
|
||||
try {
|
||||
setStatus('avvio verifica reachability…');
|
||||
await api('POST', '/api/calibration/auto/verify_reachability', body);
|
||||
$('acStartBtn').disabled = true;
|
||||
$('acVerifyBtn').disabled = true;
|
||||
$('acStopBtn').disabled = false;
|
||||
startPolling();
|
||||
} catch (e) {
|
||||
setStatus('verify fallito: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
async function stopProcedure() {
|
||||
try {
|
||||
await api('POST', '/api/calibration/auto/stop');
|
||||
setStatus('stop richiesto… il robot tornerà a home prima di fermarsi');
|
||||
} catch (e) {
|
||||
setStatus('stop fallito: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
function startPolling() {
|
||||
stopPolling();
|
||||
pollTick();
|
||||
pollTimer = setInterval(pollTick, 500);
|
||||
}
|
||||
function stopPolling() {
|
||||
if (pollTimer) { clearInterval(pollTimer); pollTimer = null; }
|
||||
}
|
||||
|
||||
async function pollTick() {
|
||||
try {
|
||||
const s = await api('GET', '/api/calibration/auto/status');
|
||||
lastSnapshot = s;
|
||||
if (s.waypoints && s.waypoints.length) {
|
||||
renderWaypoints(s.waypoints);
|
||||
drawChart(s.waypoints);
|
||||
}
|
||||
updateStats(s.waypoints, s);
|
||||
if (s.status !== 'running') {
|
||||
stopPolling();
|
||||
$('acStartBtn').disabled = false;
|
||||
$('acVerifyBtn').disabled = false;
|
||||
$('acStopBtn').disabled = true;
|
||||
if (s.calibration) {
|
||||
$('autocalResult').textContent = JSON.stringify({
|
||||
method: s.calibration.method,
|
||||
n_samples: s.calibration.n_samples,
|
||||
residual_translation_mm: s.calibration.residual_translation_mm,
|
||||
residual_rotation_deg: s.calibration.residual_rotation_deg,
|
||||
T_base_camera: s.calibration.T_base_camera,
|
||||
sanity_check: s.calibration.sanity_check,
|
||||
}, null, 2);
|
||||
}
|
||||
const mode = (s.params || {}).mode || 'calibration';
|
||||
setStatus(`${mode}: ${s.status} · done=${s.stats.done} failed=${s.stats.failed} skipped=${s.stats.skipped}`,
|
||||
s.status === 'done' ? 'ok' : s.status === 'error' ? 'err' : '');
|
||||
}
|
||||
} catch (e) {
|
||||
console.warn('autocal poll:', e);
|
||||
}
|
||||
}
|
||||
|
||||
function updateStats(waypoints, snapshot) {
|
||||
const host = $('autocalStats');
|
||||
if (!host) return;
|
||||
const total = waypoints.length;
|
||||
const reach = waypoints.filter(w => w.reachable).length;
|
||||
const unreach = total - reach;
|
||||
const done = waypoints.filter(w => w.state === 'done').length;
|
||||
const failed = waypoints.filter(w => w.state === 'failed').length;
|
||||
const skipped = waypoints.filter(w => w.state === 'skipped').length;
|
||||
const attempts = waypoints.filter(w => w.attempts > 0).map(w => w.attempts);
|
||||
const avgAtt = attempts.length ? (attempts.reduce((a, b) => a + b, 0) / attempts.length).toFixed(2) : '—';
|
||||
const maxAtt = attempts.length ? Math.max(...attempts) : '—';
|
||||
const current = snapshot && snapshot.current_index >= 0 ? snapshot.current_index : null;
|
||||
host.innerHTML = `
|
||||
<table><tbody>
|
||||
<tr><td>totale</td><td><strong>${total}</strong></td></tr>
|
||||
<tr><td>raggiungibili</td><td>${reach}</td></tr>
|
||||
<tr><td>non raggiungibili</td><td>${unreach}</td></tr>
|
||||
<tr><td>campioni ok</td><td>${done}</td></tr>
|
||||
<tr><td>falliti</td><td>${failed}</td></tr>
|
||||
<tr><td>saltati</td><td>${skipped}</td></tr>
|
||||
<tr><td>tentativi medi</td><td>${avgAtt}</td></tr>
|
||||
<tr><td>tentativi max</td><td>${maxAtt}</td></tr>
|
||||
<tr><td>corrente</td><td>${current != null ? '#' + current : '—'}</td></tr>
|
||||
<tr><td>stato job</td><td>${snapshot ? snapshot.status : '—'}</td></tr>
|
||||
</tbody></table>`;
|
||||
}
|
||||
|
||||
// Per-waypoint bar chart: bar height = attempts, color by state.
|
||||
function drawChart(waypoints) {
|
||||
try {
|
||||
_drawChartImpl(waypoints);
|
||||
} catch (e) {
|
||||
console.error('drawChart failed:', e);
|
||||
setStatus('drawChart error: ' + (e && e.message || e), 'err');
|
||||
}
|
||||
}
|
||||
|
||||
function _drawChartImpl(waypoints) {
|
||||
const canvas = $('autocalChart');
|
||||
if (!canvas) return;
|
||||
const dpr = window.devicePixelRatio || 1;
|
||||
const w = canvas.clientWidth || canvas.width;
|
||||
const h = canvas.height;
|
||||
canvas.width = w * dpr;
|
||||
canvas.height = h * dpr;
|
||||
const ctx = canvas.getContext('2d');
|
||||
ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
|
||||
ctx.clearRect(0, 0, w, h);
|
||||
ctx.fillStyle = '#0d1014';
|
||||
ctx.fillRect(0, 0, w, h);
|
||||
if (!waypoints || !waypoints.length) {
|
||||
ctx.fillStyle = '#666';
|
||||
ctx.font = '11px ui-monospace, monospace';
|
||||
ctx.fillText('nessun dato', 10, 20);
|
||||
return;
|
||||
}
|
||||
const n = waypoints.length;
|
||||
const maxA = Math.max(1, ...waypoints.map(x => x.attempts || 0));
|
||||
const pad = { l: 26, r: 6, t: 8, b: 18 };
|
||||
const plotW = w - pad.l - pad.r;
|
||||
const plotH = h - pad.t - pad.b;
|
||||
const barW = plotW / n;
|
||||
// grid
|
||||
ctx.strokeStyle = '#2a2f37';
|
||||
ctx.lineWidth = 1;
|
||||
const ticks = [1, 2, 3, 5, maxA].filter((v, i, a) => v <= maxA && a.indexOf(v) === i);
|
||||
ctx.font = '10px ui-monospace, monospace';
|
||||
ctx.fillStyle = '#666';
|
||||
for (const t of ticks) {
|
||||
const y = pad.t + plotH - (t / maxA) * plotH;
|
||||
ctx.beginPath(); ctx.moveTo(pad.l, y); ctx.lineTo(w - pad.r, y); ctx.stroke();
|
||||
ctx.fillText(String(t), 2, y + 3);
|
||||
}
|
||||
// bars
|
||||
const colorFor = (w) => {
|
||||
if (!w.reachable) return '#e74c3c';
|
||||
if (w.state === 'done' && w.attempts === 1) return '#3ecf8e';
|
||||
if (w.state === 'done') return '#ffd54f';
|
||||
if (w.state === 'failed') return '#f0ad4e';
|
||||
if (w.state === 'current') return '#4da3ff';
|
||||
if (w.state === 'skipped') return '#8a3a33';
|
||||
return '#555';
|
||||
};
|
||||
for (let i = 0; i < n; i++) {
|
||||
const wp = waypoints[i];
|
||||
const a = wp.attempts || 0;
|
||||
const bh = (a / maxA) * plotH;
|
||||
const x = pad.l + i * barW + 1;
|
||||
const y = pad.t + plotH - bh;
|
||||
ctx.fillStyle = colorFor(wp);
|
||||
ctx.fillRect(x, y, Math.max(1, barW - 2), bh);
|
||||
}
|
||||
// x label
|
||||
ctx.fillStyle = '#9aa0a6';
|
||||
ctx.fillText(`waypoints (${n})`, pad.l, h - 4);
|
||||
ctx.fillText(`tentativi`, 2, 10);
|
||||
}
|
||||
|
||||
function setStatus(msg, cls = '') {
|
||||
const el = $('autocalStatus');
|
||||
el.textContent = msg;
|
||||
el.style.color = cls === 'err' ? 'var(--danger)' : cls === 'ok' ? 'var(--ok)' : 'var(--dim)';
|
||||
}
|
||||
|
||||
async function solveNow() {
|
||||
try {
|
||||
setStatus('calcolo matrice…');
|
||||
const side = $('acSide').value || undefined;
|
||||
const r = await api('POST', '/api/calibration/solve', { method: 'TSAI', side });
|
||||
$('autocalResult').textContent = JSON.stringify({
|
||||
method: r.method, n_samples: r.n_samples,
|
||||
residual_translation_mm: r.residual_translation_mm,
|
||||
residual_rotation_deg: r.residual_rotation_deg,
|
||||
T_base_camera: r.T_base_camera,
|
||||
sanity_check: r.sanity_check,
|
||||
}, null, 2);
|
||||
setStatus(`matrice calcolata · residuo ${r.residual_translation_mm.toFixed(1)} mm / ${r.residual_rotation_deg.toFixed(2)}°`, 'ok');
|
||||
} catch (e) { setStatus('solve: ' + e.message, 'err'); }
|
||||
}
|
||||
|
||||
async function clearSamples() {
|
||||
const side = $('acSide').value || 'right';
|
||||
if (!confirm(`Rimuovere tutti i sample del braccio ${side}?`)) return;
|
||||
try {
|
||||
await api('DELETE', `/api/calibration/samples?side=${encodeURIComponent(side)}`);
|
||||
setStatus('sample svuotati', 'ok');
|
||||
} catch (e) { setStatus(e.message, 'err'); }
|
||||
}
|
||||
|
||||
// Wire up events AFTER DOM is parsed.
|
||||
document.addEventListener('DOMContentLoaded', () => {
|
||||
if (!$('autocalCanvas')) return;
|
||||
restoreParams();
|
||||
// persist on any input change
|
||||
['acDx','acDy','acDz','acOx','acOy','acOz','acNx','acNy','acNz','acRotDeg','acMinZ','acSettle',
|
||||
'acSide','acAutoSolve','acDryRun','acOrder'].forEach(id => {
|
||||
const el = $(id); if (el) el.addEventListener('change', persistParams);
|
||||
});
|
||||
$('acPreviewBtn').addEventListener('click', previewGrid);
|
||||
$('acVerifyBtn').addEventListener('click', verifyReachability);
|
||||
$('acStartBtn').addEventListener('click', startProcedure);
|
||||
$('acStopBtn').addEventListener('click', stopProcedure);
|
||||
$('acSolveBtn').addEventListener('click', solveNow);
|
||||
$('acClearBtn').addEventListener('click', clearSamples);
|
||||
$('acShowTriads').addEventListener('change', (e) => {
|
||||
showTriads = e.target.checked;
|
||||
for (const it of waypointItems.values()) it.triad.visible = showTriads;
|
||||
});
|
||||
$('acFitBtn').addEventListener('click', () => viewer && viewer.fitView());
|
||||
$('acResetViewBtn').addEventListener('click', () => viewer && viewer.resetView());
|
||||
});
|
||||
|
||||
// Lazy boot when tab first opened.
|
||||
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
|
||||
if (btn.dataset.tab === 'autocal') {
|
||||
boot();
|
||||
setTimeout(() => viewer && viewer.resize(), 50);
|
||||
}
|
||||
}));
|
||||
|
||||
if (document.getElementById('tab-autocal')?.classList.contains('active')) boot();
|
||||
@@ -431,11 +431,6 @@ class G1Viewer {
|
||||
}
|
||||
}
|
||||
|
||||
// Expose the viewer class so other modules (Auto-Calibrazione) can
|
||||
// instantiate a second canvas over the same URDF pipeline.
|
||||
export { G1Viewer };
|
||||
|
||||
|
||||
// ---- Boot and expose global hook. ----
|
||||
|
||||
const canvas = document.getElementById('diag3dCanvas');
|
||||
|
||||
@@ -13,7 +13,6 @@
|
||||
<button class="tab active" data-tab="home">Home</button>
|
||||
<button class="tab" data-tab="agent">Assistente AI</button>
|
||||
<button class="tab" data-tab="ai-settings">Impostazioni AI</button>
|
||||
<button class="tab" data-tab="autocal">Auto-Calibrazione</button>
|
||||
<button class="tab" data-tab="grasp">TrovaUtensile</button>
|
||||
<button class="tab" data-tab="servo">Servo Visuale</button>
|
||||
<button class="tab" data-tab="view3d">Vista 3D</button>
|
||||
@@ -175,88 +174,6 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section class="panel" id="tab-autocal">
|
||||
<div class="diag3d-toolbar">
|
||||
<button id="acFitBtn">Auto-fit</button>
|
||||
<button id="acResetViewBtn">Reset vista</button>
|
||||
<label class="inline"><input type="checkbox" id="acShowTriads" checked /> Triad sui waypoint</label>
|
||||
<span class="post-health" id="autocalStatus">—</span>
|
||||
</div>
|
||||
<div class="grid-2 diag3d-layout">
|
||||
<div>
|
||||
<div class="canvas-wrap">
|
||||
<canvas id="autocalCanvas" width="720" height="560"></canvas>
|
||||
</div>
|
||||
<div class="marker-info">drag = ruota · wheel = zoom · shift+drag = pan</div>
|
||||
<div class="diag3d-legend">
|
||||
<div><span class="legend-sw" style="background:#aaaaaa;border-radius:50%"></span> raggiungibile</div>
|
||||
<div><span class="legend-sw" style="background:var(--danger);border-radius:50%"></span> non raggiungibile (skip)</div>
|
||||
<div><span class="legend-sw" style="background:var(--accent);border-radius:50%"></span> corrente</div>
|
||||
<div><span class="legend-sw" style="background:var(--ok);border-radius:50%"></span> campione acquisito</div>
|
||||
<div><span class="legend-sw" style="background:var(--warn);border-radius:50%"></span> fallito</div>
|
||||
</div>
|
||||
</div>
|
||||
<div>
|
||||
<h2>Parametri griglia</h2>
|
||||
<div class="ctrl-block">
|
||||
<div class="info-box">
|
||||
Parti dalla posizione corrente del polso (letta dal G1). La griglia copre ±delta/2 su ciascun asse. Le divisioni sono i punti per asse: 3 significa [start-Δ/2, start, start+Δ/2].
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>Δx (mm) <input type="number" id="acDx" value="100" step="10" min="0" max="500"></label>
|
||||
<label>Δy (mm) <input type="number" id="acDy" value="100" step="10" min="0" max="500"></label>
|
||||
<label>Δz (mm) <input type="number" id="acDz" value="80" step="10" min="0" max="500"></label>
|
||||
<label title="Spostamento del centro griglia rispetto alla posa polso attuale">offset x (mm) <input type="number" id="acOx" value="0" step="10" min="-500" max="500"></label>
|
||||
<label title="Spostamento del centro griglia rispetto alla posa polso attuale">offset y (mm) <input type="number" id="acOy" value="0" step="10" min="-500" max="500"></label>
|
||||
<label title="Spostamento del centro griglia rispetto alla posa polso attuale">offset z (mm) <input type="number" id="acOz" value="0" step="10" min="-500" max="500"></label>
|
||||
<label>n_x <input type="number" id="acNx" value="3" step="1" min="1" max="7"></label>
|
||||
<label>n_y <input type="number" id="acNy" value="3" step="1" min="1" max="7"></label>
|
||||
<label>n_z <input type="number" id="acNz" value="3" step="1" min="1" max="7"></label>
|
||||
<label>rot polso (°) <input type="number" id="acRotDeg" value="10" step="1" min="0" max="45"></label>
|
||||
<label>min z (m) <input type="number" id="acMinZ" value="0.05" step="0.01"></label>
|
||||
<label>settle (s) <input type="number" id="acSettle" value="1.0" step="0.1" min="0.1" max="5"></label>
|
||||
<label>side
|
||||
<select id="acSide">
|
||||
<option value="">(config)</option>
|
||||
<option value="right">right</option>
|
||||
<option value="left">left</option>
|
||||
</select>
|
||||
</label>
|
||||
<label class="inline"><input type="checkbox" id="acAutoSolve" checked /> auto-solve finale</label>
|
||||
<label class="inline"><input type="checkbox" id="acDryRun" /> dry-run (preview, nessun moto reale)</label>
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="acPreviewBtn" class="primary">Calcola griglia</button>
|
||||
<button id="acVerifyBtn" title="muove a ogni punto senza acquisire — testa raggiungibilità reale">Verifica reale</button>
|
||||
<button id="acStartBtn" disabled>▶ Start calibrazione</button>
|
||||
<button id="acStopBtn" class="danger" disabled>■ Stop + torna a home</button>
|
||||
<button id="acSolveBtn">Calcola matrice</button>
|
||||
<button id="acClearBtn" class="danger">Rimuovi sample</button>
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>Ordine di visita
|
||||
<select id="acOrder">
|
||||
<option value="serpentine" selected>serpentina (distanza min.)</option>
|
||||
<option value="nested">nested (xyz semplice)</option>
|
||||
</select>
|
||||
</label>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<h2>Stato</h2>
|
||||
<div id="autocalStats" class="ctrl-block">
|
||||
<div class="dim">nessuna griglia calcolata</div>
|
||||
</div>
|
||||
|
||||
<h2>Tentativi per punto</h2>
|
||||
<canvas id="autocalChart" width="520" height="140" class="autocal-chart"></canvas>
|
||||
|
||||
<h2>Risultato calibrazione</h2>
|
||||
<pre id="autocalResult" class="log small">—</pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== TROVA UTENSILE ============================== -->
|
||||
<section class="panel" id="tab-grasp">
|
||||
<div class="grid-2">
|
||||
@@ -502,7 +419,6 @@
|
||||
}
|
||||
</script>
|
||||
<script src="/static/app.js"></script>
|
||||
<script type="module" src="/static/autocal3d.js"></script>
|
||||
<script type="module" src="/static/servo.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -516,15 +516,6 @@ pre.log.err {
|
||||
#diag3dPinned tr.warn td { color: var(--warn); }
|
||||
#diag3dPinned tr.err td { color: var(--danger); }
|
||||
|
||||
.autocal-chart {
|
||||
width: 100%;
|
||||
background: #0d1014;
|
||||
border: 1px solid var(--border);
|
||||
border-radius: 4px;
|
||||
display: block;
|
||||
margin-bottom: 10px;
|
||||
}
|
||||
|
||||
/* Button feedback flash */
|
||||
button.flash-ok {
|
||||
animation: flashOk 0.8s ease-out;
|
||||
|
||||
Reference in new issue
Block a user