diff --git a/src/presa_mano/api/__init__.py b/src/presa_mano/api/__init__.py index 4a3a3f1..830b132 100644 --- a/src/presa_mano/api/__init__.py +++ b/src/presa_mano/api/__init__.py @@ -11,6 +11,7 @@ from . import ( perception_routes, posture_routes, robot_routes, + work_plane_routes, wrist_routes, ) @@ -23,5 +24,6 @@ __all__ = [ "perception_routes", "posture_routes", "robot_routes", + "work_plane_routes", "wrist_routes", ] diff --git a/src/presa_mano/api/grasp_routes.py b/src/presa_mano/api/grasp_routes.py index 8792914..2cb541c 100644 --- a/src/presa_mano/api/grasp_routes.py +++ b/src/presa_mano/api/grasp_routes.py @@ -5,26 +5,17 @@ from __future__ import annotations from typing import Any, Optional import numpy as np -import time -from dataclasses import asdict from fastapi import APIRouter, HTTPException -from pydantic import BaseModel, Field +from pydantic import BaseModel from ..calibration import CalibrationStore from ..clients import G1Unavailable from ..config import get_config from ..geometry import transform_points from ..grasp import GraspPlanner, compute_axes -from ..grasp.visual_servo import ( - ServoError, - WorkPlane, - fit_plane_svd, - get_work_plane, - plan as servo_plan, - set_work_plane, -) from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame from ..perception.sam21_bridge import Sam21Fallback, points_via_sam21 +from ..perception.work_plane import apply_below_plane_filter, get_work_plane from .perception_routes import _bbox_to_roi from ..timing import Stopwatch from . import _deps @@ -71,27 +62,32 @@ def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopw if cfg.get("grasp", {}).get("use_sam21", True): try: - pts_cv = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) + pts_cv, plane_info = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) with sw.step("to_base_frame"): pts_base = transform_points(T_base_cam, pts_cv) meta["source"] = "sam21" meta["n_points"] = int(len(pts_base)) + meta["plane_applied"] = plane_info["plane_applied"] + meta["n_points_before_plane"] = plane_info["n_points_before_plane"] return pts_base, meta except Sam21Fallback as e: # Logging happens inside points_via_sam21 with the right level. meta["sam21_fallback_reason"] = str(e) - pts_base = _points_via_bbox(nuwa, T_base_cam, bbox, sw) + pts_base, plane_info = _points_via_bbox(nuwa, T_base_cam, bbox, sw) meta["source"] = "bbox_roi" meta["n_points"] = int(len(pts_base)) + meta["plane_applied"] = plane_info["plane_applied"] + meta["n_points_before_plane"] = plane_info["n_points_before_plane"] return pts_base, meta def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, int], - sw: Stopwatch) -> np.ndarray: - """Legacy path: bbox → Nuwa ROI cloud → filter outliers → base frame. + sw: Stopwatch) -> tuple[np.ndarray, dict]: + """Legacy path: bbox → Nuwa ROI cloud → outlier filter → plane filter → base frame. - Raises HTTPException(422) on <10 valid depth points (existing behaviour). + Raises HTTPException(422) on <10 valid depth points (after both outlier + filter and below-plane filter). Returns (pts_base, plane_info). """ cfg = get_config() u_min, v_min, u_max, v_max = bbox @@ -106,12 +102,17 @@ def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, in raise HTTPException(status_code=502, detail=f"Nuwa ROI pointcloud failed: {e}") with sw.step("filter_outliers"): cropped = filter_outliers(pc) - if len(cropped) < 10: - raise HTTPException(status_code=422, detail=f"too few valid depth points in bbox: {len(cropped)}") + pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64) + pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane()) + if len(pts_cv) < 10: + raise HTTPException( + status_code=422, + detail=f"too few valid depth points after plane filter: {len(pts_cv)}" + f" (before plane: {plane_info['n_points_before_plane']})" + ) with sw.step("to_base_frame"): - pts_cv = nuwa_to_cv_frame(cropped[:, :3]) pts_base = transform_points(T_base_cam, pts_cv) - return pts_base + return pts_base, plane_info @router.post("/plan") @@ -171,175 +172,3 @@ def execute(req: ExecuteReq) -> dict[str, Any]: raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}") return {"plan": g.to_dict(), "steps": steps, "timings_ms": sw.as_dict(), **meta} - -# --------------------------------------------------------------------------- -# Visual servo (marker-relative, no static hand-eye calibration). -# --------------------------------------------------------------------------- - -class VisualServoReq(BaseModel): - conf: Optional[float] = None - class_name: Optional[str] = None - side: Optional[str] = None - min_distance_m: Optional[float] = None # default from config (0.1 m) - step_fraction: Optional[float] = None # default from config (0.5) - # Override for grasp.visual_servo.wrist_marker_rvec (radians). Useful for - # live tuning from the UI without persisting to config on every try. - wrist_marker_rvec: Optional[list[float]] = None - - -class VisualServoStepReq(VisualServoReq): - dry_run: bool = True - timeout_s: float = 5.0 - - -def _servo_plan_from_req(req: VisualServoReq, sw: Stopwatch): - cfg = get_config() - vs_cfg = (cfg.get("grasp", {}) or {}).get("visual_servo", {}) or {} - side = req.side or cfg["robot"]["wrist_side"] - min_dist = float(req.min_distance_m - if req.min_distance_m is not None - else vs_cfg.get("min_distance_m", 0.10)) - step_frac = float(req.step_fraction - if req.step_fraction is not None - else vs_cfg.get("step_fraction", 0.5)) - # Request-level rvec override: thread it into a shallow-copied cfg so the - # core plan() function keeps reading from a single place. - if req.wrist_marker_rvec is not None: - cfg = {**cfg, "grasp": {**(cfg.get("grasp") or {}), - "visual_servo": {**vs_cfg, - "wrist_marker_rvec": list(req.wrist_marker_rvec)}}} - marker_id = int(cfg["aruco"]["wrist_marker"]["id"]) - size_mm = float(cfg["aruco"]["wrist_marker"]["size_mm"]) - pc_step = int(cfg.get("grasp", {}).get("pointcloud_step", 1)) - pc_max_depth = int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000)) - try: - with sw.step("servo_plan"): - result = servo_plan( - side=side, marker_id=marker_id, size_mm=size_mm, - conf=req.conf, class_name=req.class_name, - min_distance_m=min_dist, step_fraction=step_frac, - pointcloud_step=pc_step, pointcloud_max_depth_mm=pc_max_depth, - cfg=cfg, - nuwa=_deps.nuwa(), marker=_deps.marker(), - yolo=_deps.yolo(), g1=_deps.g1(dry_run=False), - ) - except ServoError as e: - raise HTTPException(status_code=422, detail=str(e)) - except Exception as e: - raise HTTPException(status_code=502, detail=f"servo plan failed: {e}") - return side, min_dist, step_frac, result - - -@router.post("/visual_servo/plan") -def visual_servo_plan(req: VisualServoReq) -> dict[str, Any]: - """Perception + next-step computation, no motion. Useful for UI preview.""" - sw = Stopwatch() - side, min_dist, step_frac, p = _servo_plan_from_req(req, sw) - return { - "side": side, - "min_distance_m": min_dist, - "step_fraction": step_frac, - "plan": p.__dict__, - "timings_ms": sw.as_dict(), - } - - -@router.post("/visual_servo/step") -def visual_servo_step(req: VisualServoStepReq) -> dict[str, Any]: - """Plan + execute one half-step toward the object. Returns the move result - plus the plan (before the move) so the UI can visualise what just happened; - the next click re-plans from the new wrist pose.""" - sw = Stopwatch() - side, min_dist, step_frac, p = _servo_plan_from_req(req, sw) - move_result: dict[str, Any] | None = None - if p.done or p.target_position_base is None: - return { - "side": side, - "min_distance_m": min_dist, - "step_fraction": step_frac, - "plan": p.__dict__, - "moved": False, - "reason": p.reason or "reached", - "timings_ms": sw.as_dict(), - } - g1 = _deps.g1(dry_run=req.dry_run) - try: - with sw.step("wrist_absolute"): - move_result = g1.wrist_absolute( - side=side, - position=list(p.target_position_base), - quaternion=list(p.target_quaternion_base or p.wrist_quaternion_base), - wait=True, - timeout_s=req.timeout_s, - ) - except G1Unavailable as e: - raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}") - return { - "side": side, - "min_distance_m": min_dist, - "step_fraction": step_frac, - "plan": p.__dict__, - "moved": True, - "move_result": move_result, - "timings_ms": sw.as_dict(), - } - - -# --------------------------------------------------------------------------- -# Work plane (for the visual-servo tab): fit once from a user-selected ROI, -# stored in-memory (singleton). Subsequent visual_servo/plan calls enrich the -# response with plane projections of marker and object. -# --------------------------------------------------------------------------- - -class PlaneFitReq(BaseModel): - u_min: int = Field(..., ge=0) - v_min: int = Field(..., ge=0) - u_max: int = Field(..., gt=0) - v_max: int = Field(..., gt=0) - max_depth_mm: Optional[int] = None - - -@router.post("/visual_servo/plane/fit") -def visual_servo_plane_fit(req: PlaneFitReq) -> dict[str, Any]: - cfg = get_config() - nuwa = _deps.nuwa() - pc_max = req.max_depth_mm or int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000)) - try: - pc, _meta = nuwa.pointcloud_roi( - req.u_min, req.v_min, req.u_max, req.v_max, - step=1, max_depth_mm=pc_max, - ) - except Exception as e: - raise HTTPException(status_code=502, detail=f"Nuwa pointcloud_roi failed: {e}") - if len(pc) < 10: - raise HTTPException(status_code=422, detail=f"too few depth points in ROI: {len(pc)}") - pts_cv = nuwa_to_cv_frame(pc[:, :3]) - # Drop gross outliers before SVD (stray far-depth pixels around the edges). - pts_cv = filter_outliers(pts_cv, n_std=2.0) - try: - normal, offset, centroid, rms_mm = fit_plane_svd(pts_cv) - except ServoError as e: - raise HTTPException(status_code=422, detail=str(e)) - wp = WorkPlane( - normal=[float(x) for x in normal], - offset=float(offset), - centroid=[float(x) for x in centroid], - n_inliers=int(len(pts_cv)), - fit_rms_mm=float(rms_mm), - roi=[int(req.u_min), int(req.v_min), int(req.u_max), int(req.v_max)], - created_at=time.time(), - ) - set_work_plane(wp) - return {"ok": True, "plane": asdict(wp)} - - -@router.get("/visual_servo/plane") -def visual_servo_plane_get() -> dict[str, Any]: - wp = get_work_plane() - return {"set": wp is not None, "plane": asdict(wp) if wp is not None else None} - - -@router.delete("/visual_servo/plane") -def visual_servo_plane_clear() -> dict[str, Any]: - set_work_plane(None) - return {"ok": True, "set": False} diff --git a/src/presa_mano/api/perception_routes.py b/src/presa_mano/api/perception_routes.py index 4bf8b62..be91d83 100644 --- a/src/presa_mano/api/perception_routes.py +++ b/src/presa_mano/api/perception_routes.py @@ -12,6 +12,7 @@ from ..config import get_config from ..geometry import transform_points from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame from ..perception.sam21_bridge import Sam21Fallback, points_via_sam21 +from ..perception.work_plane import apply_below_plane_filter, get_work_plane from ..timing import Stopwatch from . import _deps @@ -110,6 +111,9 @@ def scan(req: ScanReq) -> dict[str, Any]: out["source"] = source if extra.get("sam21_fallback_reason"): out["sam21_fallback_reason"] = extra["sam21_fallback_reason"] + if "plane_applied" in extra: + out["plane_applied"] = extra["plane_applied"] + out["n_points_before_plane"] = extra["n_points_before_plane"] out["n_points"] = int(len(pts_cv)) out["points_camera"] = pts_cv.tolist() if len(pts_cv) else [] @@ -153,6 +157,9 @@ def object_points(req: DetectReq) -> dict[str, Any]: } if extra.get("sam21_fallback_reason"): out["sam21_fallback_reason"] = extra["sam21_fallback_reason"] + if "plane_applied" in extra: + out["plane_applied"] = extra["plane_applied"] + out["n_points_before_plane"] = extra["n_points_before_plane"] T_base_cam = _load_calibration() if T_base_cam is not None and len(pts_cv) > 0: @@ -177,7 +184,9 @@ def _object_cloud_cv(nuwa, bbox: tuple[int, int, int, int], max_points: int, extra: dict[str, Any] = {} if cfg.get("grasp", {}).get("use_sam21", True): try: - pts_cv = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) + pts_cv, plane_info = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) + extra["plane_applied"] = plane_info["plane_applied"] + extra["n_points_before_plane"] = plane_info["n_points_before_plane"] if len(pts_cv) > max_points: idx = np.random.choice(len(pts_cv), max_points, replace=False) pts_cv = pts_cv[idx] @@ -198,10 +207,13 @@ def _object_cloud_cv(nuwa, bbox: tuple[int, int, int, int], max_points: int, raise HTTPException(status_code=502, detail=f"Nuwa pointcloud ROI failed: {e}") with sw.step("filter_outliers"): cropped = filter_outliers(pc) - if len(cropped) > max_points: - idx = np.random.choice(len(cropped), max_points, replace=False) - cropped = cropped[idx] pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64) + pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane()) + extra["plane_applied"] = plane_info["plane_applied"] + extra["n_points_before_plane"] = plane_info["n_points_before_plane"] + if len(pts_cv) > max_points: + idx = np.random.choice(len(pts_cv), max_points, replace=False) + pts_cv = pts_cv[idx] return pts_cv, "bbox_roi", extra diff --git a/src/presa_mano/api/work_plane_routes.py b/src/presa_mano/api/work_plane_routes.py new file mode 100644 index 0000000..1c1a3f5 --- /dev/null +++ b/src/presa_mano/api/work_plane_routes.py @@ -0,0 +1,123 @@ +"""Work-plane calibration endpoints: fit (preview), save, get, delete.""" +from __future__ import annotations + +import time +from dataclasses import asdict +from typing import Any + +import numpy as np +from fastapi import APIRouter, HTTPException +from pydantic import BaseModel, Field + +from ..perception import filter_outliers, nuwa_to_cv_frame +from ..perception.work_plane import ( + WorkPlane, + WorkPlaneError, + fit_plane_svd, + get_work_plane, + set_work_plane, +) +from ..timing import Stopwatch +from . import _deps + +router = APIRouter() + + +class FitReq(BaseModel): + u_min: int = Field(..., ge=0) + v_min: int = Field(..., ge=0) + u_max: int = Field(..., gt=0) + v_max: int = Field(..., gt=0) + offset_mm: float = Field(5.0, ge=0.0, le=200.0) + max_preview_points: int = Field(5000, ge=100, le=20000) + + +class SaveReq(BaseModel): + plane: dict[str, Any] # accept a serialised WorkPlane + + +@router.post("/fit") +def fit(req: FitReq) -> dict[str, Any]: + """Compute a candidate work plane from a rectangle on the RGB image. + + Does NOT persist. Returns the plane + a downsampled CV-frame cloud for + the GUI 3D preview. + """ + sw = Stopwatch() + nuwa = _deps.nuwa() + try: + with sw.step("pointcloud_roi"): + pc, _meta = nuwa.pointcloud_roi( + req.u_min, req.v_min, req.u_max, req.v_max, + step=1, max_depth_mm=4000, + ) + except Exception as e: + raise HTTPException(status_code=502, detail=f"Nuwa pointcloud_roi failed: {e}") + if len(pc) < 10: + raise HTTPException( + status_code=422, + detail=f"too few depth points in ROI: {len(pc)}", + ) + pts_cv = nuwa_to_cv_frame(pc[:, :3]) + with sw.step("filter_outliers"): + pts_cv = filter_outliers(pts_cv, n_std=2.0) + if len(pts_cv) < 10: + raise HTTPException( + status_code=422, + detail=f"too few inliers after outlier filter: {len(pts_cv)}", + ) + try: + with sw.step("fit_plane_svd"): + normal, offset, centroid, rms_mm = fit_plane_svd(pts_cv) + except WorkPlaneError as e: + raise HTTPException(status_code=422, detail=str(e)) + + wp = WorkPlane( + normal=[float(x) for x in normal], + offset=float(offset), + centroid=[float(x) for x in centroid], + n_inliers=int(len(pts_cv)), + fit_rms_mm=float(rms_mm), + roi=[int(req.u_min), int(req.v_min), int(req.u_max), int(req.v_max)], + offset_mm=float(req.offset_mm), + created_at=time.time(), + ) + + # Downsample for preview. + if len(pts_cv) > req.max_preview_points: + idx = np.random.choice(len(pts_cv), req.max_preview_points, replace=False) + preview = pts_cv[idx] + else: + preview = pts_cv + + return { + "plane": asdict(wp), + "preview": True, + "points_camera": preview.tolist(), + "timings_ms": sw.as_dict(), + } + + +@router.post("/save") +def save(req: SaveReq) -> dict[str, Any]: + """Persist a previously-fitted work plane to disk.""" + try: + d = dict(req.plane) + d.setdefault("offset_mm", 5.0) + wp = WorkPlane(**d) + except Exception as e: + raise HTTPException(status_code=422, detail=f"invalid plane payload: {e}") + set_work_plane(wp) + return {"ok": True, "plane": asdict(wp), "saved": True} + + +@router.get("") +def get_current() -> dict[str, Any]: + wp = get_work_plane() + return {"plane": asdict(wp) if wp is not None else None} + + +@router.delete("") +def delete_current() -> dict[str, Any]: + set_work_plane(None) + return {"ok": True, "plane": None} diff --git a/src/presa_mano/grasp/visual_servo.py b/src/presa_mano/grasp/visual_servo.py deleted file mode 100644 index 0a376f0..0000000 --- a/src/presa_mano/grasp/visual_servo.py +++ /dev/null @@ -1,504 +0,0 @@ -"""Marker-relative visual servoing (no static hand-eye calibration). - -Idea: the wrist-mounted ArUco marker is our live reference. Each step we read -current wrist pose from G1, detect the marker in the camera frame, detect the -object (YOLO bbox → Nuwa ROI point cloud → centroid), and plan a wrist move -that advances a fraction of the camera-frame marker→object vector. - -Mapping a camera-frame delta to a base-frame wrist delta needs `R_base_camera`, -which is derived live each step: - - R_base_camera = R_base_wrist @ R_wrist_marker @ R_cam_marker^-1 - -Only the *rotation* part of `T_wrist_marker` matters for deltas — translation -cancels. We take `R_wrist_marker` from config (rvec), defaulting to identity -when the marker is roughly aligned with the wrist frame. - -Stop condition: camera-frame marker→object distance ≤ `min_distance_m`. This -is a direct physical proxy for wrist-to-object proximity, independent of the -(unknown) marker-to-wrist translation offset. -""" - -from __future__ import annotations - -import json -import logging -import time -from dataclasses import asdict, dataclass -from pathlib import Path -from typing import Any - -import numpy as np - -from ..clients import G1Client, MarkerClient, NuwaClient, YoloClient -from ..config import data_dir -from ..geometry import ( - T_from_pos_quat, - invert_T, - make_T, - quat_xyzw_to_R, - rvec_to_R, -) -from ..perception import filter_outliers, nuwa_to_cv_frame -from ..perception.aruco_pose import nuwa_intrinsics_to_matrix, solve_square_marker -from ..perception.detector import ObjectDetector - -log = logging.getLogger(__name__) - - -class ServoError(RuntimeError): - pass - - -@dataclass -class WorkPlane: - """Plane in camera frame: normal·X + offset = 0, normal is unit-length and - pointing roughly toward the camera (i.e. has negative Z in OpenCV frame).""" - normal: list[float] - offset: float - centroid: list[float] - n_inliers: int - fit_rms_mm: float - roi: list[int] - created_at: float - - -_WORK_PLANE: WorkPlane | None = None -_WORK_PLANE_LOADED = False - - -def _work_plane_path() -> Path: - return data_dir() / "work_plane.json" - - -def _save_work_plane_to_disk(wp: WorkPlane | None) -> None: - path = _work_plane_path() - try: - if wp is None: - if path.exists(): - path.unlink() - return - path.parent.mkdir(parents=True, exist_ok=True) - with open(path, "w") as f: - json.dump(asdict(wp), f, indent=2) - except Exception as e: - log.warning("failed to persist work plane: %s", e) - - -def _load_work_plane_from_disk() -> WorkPlane | None: - path = _work_plane_path() - try: - with open(path, "r") as f: - d = json.load(f) - return WorkPlane(**d) - except FileNotFoundError: - return None - except Exception as e: - log.warning("failed to load persisted work plane (%s)", e) - return None - - -def get_work_plane() -> WorkPlane | None: - global _WORK_PLANE, _WORK_PLANE_LOADED - if not _WORK_PLANE_LOADED: - _WORK_PLANE = _load_work_plane_from_disk() - _WORK_PLANE_LOADED = True - return _WORK_PLANE - - -def set_work_plane(wp: WorkPlane | None) -> None: - global _WORK_PLANE, _WORK_PLANE_LOADED - _WORK_PLANE = wp - _WORK_PLANE_LOADED = True - _save_work_plane_to_disk(wp) - - -def fit_plane_svd(points_xyz: np.ndarray - ) -> tuple[np.ndarray, float, np.ndarray, float]: - """Least-squares plane via SVD. Returns (normal, offset, centroid, rms_mm). - - Plane equation: normal·X + offset = 0. Normal is oriented to point toward - the camera origin (negative Z in CV frame).""" - pts = np.asarray(points_xyz, dtype=np.float64) - if pts.ndim != 2 or pts.shape[1] < 3: - raise ServoError(f"unexpected points shape for plane fit: {pts.shape}") - xyz = pts[:, :3] - if len(xyz) < 10: - raise ServoError(f"too few points to fit plane: {len(xyz)}") - c = xyz.mean(axis=0) - centered = xyz - c - _, _, Vt = np.linalg.svd(centered, full_matrices=False) - normal = Vt[-1] - n = float(np.linalg.norm(normal)) - if n < 1e-9: - raise ServoError("degenerate plane (zero normal)") - normal = normal / n - # Orient normal to point toward the camera origin: the signed distance of - # the origin from the plane must be positive, i.e. d = -n·c > 0 → n·c < 0. - # Flipping only on normal[2] is wrong for tilted planes (the camera may - # not be above the plane in Z-only sense). - if float(normal @ c) > 0: - normal = -normal - offset = -float(normal @ c) - signed = centered @ normal - rms_mm = float(np.sqrt(np.mean(signed * signed)) * 1000.0) - return normal, offset, c, rms_mm - - -def project_point_on_plane(P: np.ndarray, normal: np.ndarray, offset: float - ) -> tuple[np.ndarray, float]: - """Perpendicular projection of a 3D point onto the plane. - Returns (foot, signed_distance). Signed distance > 0 means P is on the - side of the plane the normal points toward (camera side in our convention). - """ - s = float(normal @ P + offset) - return P - s * normal, s - - -def backproject_pixel_on_plane(uv: np.ndarray, K: np.ndarray, - normal: np.ndarray, offset: float - ) -> np.ndarray: - """Intersect the viewing ray through pixel (u,v) with the work plane.""" - r = np.linalg.inv(K) @ np.array([float(uv[0]), float(uv[1]), 1.0]) - denom = float(normal @ r) - if abs(denom) < 1e-9: - raise ServoError("ray parallel to work plane") - t = -offset / denom - if t <= 0: - raise ServoError("plane intersection is behind the camera") - return t * r - - -@dataclass -class ServoPlan: - # Truth (from G1) - wrist_position_base: list[float] - wrist_quaternion_base: list[float] - - # Camera-frame observations - marker_position_cam: list[float] - object_position_cam: list[float] - - # Pixel-space positions for the 2D overlay (u, v in image coordinates). - marker_pixel: list[float] - object_pixel: list[float] - - # Perpendicular depth to the camera plane (Z component, m). This is what - # drives the pinhole mm/px scale — NOT the slant range. - marker_depth_z_m: float - object_depth_z_m: float - # Euclidean distance camera origin → point (slant range, m). - marker_distance_cam_m: float - object_distance_cam_m: float - - # Pinhole scale at the point's depth: mm-per-pixel = (z_mm / fx), i.e. - # physical size of one image pixel at that depth. Depends on Z only, so - # two points at the same Z but different XY will share mm/px. - marker_mm_per_px: float - object_mm_per_px: float - - # Approximate base-frame positions for visualisation only. Computed with - # T_wrist_marker = identity: relative distances are correct, absolute - # positions are offset by the unknown marker-to-wrist translation. - marker_position_base_est: list[float] - object_position_base_est: list[float] - T_base_camera_est: list[list[float]] - - # Scalar cues - distance_m: float # camera-frame magnitude of marker→object - done: bool # True when distance ≤ min_distance_m - - # Next wrist target in base frame (None when done) - target_position_base: list[float] | None - target_quaternion_base: list[float] | None - planned_step_m: float # how much the wrist is about to move (base frame) - - # Work-plane projection block — present only when a plane has been fitted. - # Everything camera-frame. `in_plane_distance_m` is the Euclidean distance - # between the two foot-of-perpendicular points (lives on the plane). - plane: dict[str, Any] | None = None - - # Diagnostics - detection: dict[str, Any] | None = None - n_object_points: int = 0 - marker_reprojection_error_px: float | None = None - marker_translation_source: str = "solvepnp" # "nuwa_depth" | "solvepnp" - object_point_source: str = "centroid" # "centroid" | "highest_above_plane(...)" - reason: str | None = None - - -def _resolve_wrist_pose(g1: G1Client, side: str) -> tuple[np.ndarray, np.ndarray, list[float]]: - w = g1.wrist() - 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 ServoError(f"G1 /wrist missing '{side}' pose: {w!r}") - pos = np.asarray(pose["position"], dtype=np.float64) - quat = np.asarray(pose["quaternion"], dtype=np.float64) - return pos, quat, list(map(float, pose["quaternion"])) - - -def _detect_marker_cam(nuwa: NuwaClient, marker: MarkerClient, - marker_id: int, size_mm: float - ) -> tuple[np.ndarray, np.ndarray, np.ndarray, float | None, str]: - """Return (R_cam_marker, t_cam_marker, corners_px, reprojection_error_px, source). - - Translation comes from the Nuwa depth at the marker centre pixel whenever - possible — much more stable than solvePnP on a small planar marker, which - suffers from the IPPE_SQUARE planar ambiguity (two solutions, same - reprojection error) at ~1.4 m distance. Rotation still comes from solvePnP - because the servo mapping cam→base needs it. - - `source` reports how `t_cam_marker` was obtained: "nuwa_depth" when the - depth-sensor value at the marker pixel was usable, "solvepnp" otherwise. - """ - result = marker.detect_url( - markers=[{"id": int(marker_id), "size_mm": float(size_mm)}], - image_url=nuwa.rgb_url(), - ) - found = result.get("found", []) - entry = next((m for m in found if int(m["id"]) == int(marker_id)), None) - if entry is None: - raise ServoError(f"marker id={marker_id} not detected (not_found={result.get('not_found')})") - corners = np.asarray(entry["corners"], dtype=np.float64) - - R_cm = None - t_pnp = None - reproj_err: float | None = None - try: - intr = nuwa.intrinsics() - K, dist = nuwa_intrinsics_to_matrix(intr) - sol = solve_square_marker(corners, float(size_mm) / 1000.0, K, dist) - R_cm = sol["R"] - t_pnp = sol["t"].flatten() - reproj_err = float(sol["reprojection_error_px"]) - except Exception as e: - log.warning("local pnp failed (%s) — using marker-finder orientation", e) - R_cm = np.asarray(entry["rotation_matrix"], dtype=np.float64).reshape(3, 3) - t_pnp = np.asarray(entry["position_m"], dtype=np.float64).flatten() - - # Prefer Nuwa depth at the marker centre pixel for the translation. - centre = corners.mean(axis=0) - u = int(round(float(centre[0]))) - v = int(round(float(centre[1]))) - t_depth = None - try: - m = nuwa.measure(u, v, radius=8) - pt = m.get("point_m") if isinstance(m, dict) else None - if not isinstance(pt, dict): - pt = m - x = float(pt.get("x", pt.get("X"))) - y = float(pt.get("y", pt.get("Y"))) - z = float(pt.get("z", pt.get("Z"))) - # /api/measure returns already-OpenCV-frame coords (Z>0 in front of - # the camera). No flip. - t_depth = np.array([x, y, z], dtype=np.float64) - if not np.all(np.isfinite(t_depth)) or abs(t_depth[2]) < 1e-3: - t_depth = None - except Exception as e: - log.warning("nuwa.measure failed at (%d,%d): %s", u, v, e) - t_depth = None - - if t_depth is not None: - return R_cm, t_depth, corners, reproj_err, "nuwa_depth" - return R_cm, t_pnp, corners, reproj_err, "solvepnp" - - -def _detect_object_point_cam(nuwa: NuwaClient, yolo: YoloClient, - conf: float | None, class_name: str | None, - pointcloud_step: int, pointcloud_max_depth_mm: int, - work_plane: WorkPlane | None, - top_fraction: float = 0.05, - ) -> tuple[np.ndarray, dict[str, Any], int, str]: - """Return (point_cam_xyz, top_detection, n_points, source). - - When a work plane is available, returns the mean of the top `top_fraction` - of points (by signed distance from the plane) — this is the highest part - of the object above the work surface, which is the natural grasp target. - Without a plane, falls back to the centroid of all filtered points.""" - detector = ObjectDetector(yolo, nuwa) - dets = detector.detect(conf=conf) - if class_name: - dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()] - if not dets: - raise ServoError("no object detected" + (f" for class '{class_name}'" if class_name else "")) - dets.sort(key=lambda d: d["confidence"], reverse=True) - top = dets[0] - b = top["bbox"] - u_min = max(0, int(np.floor(b["x1"]))) - v_min = max(0, int(np.floor(b["y1"]))) - u_max = int(np.ceil(b["x2"])) - v_max = int(np.ceil(b["y2"])) - pc, _meta = nuwa.pointcloud_roi(u_min, v_min, u_max, v_max, - step=pointcloud_step, - max_depth_mm=pointcloud_max_depth_mm) - # Empirical: Nuwa ROI points come in Nuwa-native frame (Z<0 in front of - # camera) despite the docstring. Flip to true OpenCV frame so comparisons - # with the plane (also CV) are consistent. - cropped = filter_outliers(pc) - if len(cropped) < 10: - raise ServoError(f"too few valid depth points in bbox: {len(cropped)}") - pts_cv = nuwa_to_cv_frame(cropped[:, :3]) - if work_plane is None: - return pts_cv.mean(axis=0), top, int(len(pts_cv)), "centroid" - n_vec = np.asarray(work_plane.normal, dtype=np.float64) - d_off = float(work_plane.offset) - signed = pts_cv @ n_vec + d_off # Nx (positive = above plane) - k = max(1, int(round(float(top_fraction) * len(pts_cv)))) - idx = np.argpartition(signed, -k)[-k:] - highest = pts_cv[idx].mean(axis=0) - return highest, top, int(len(pts_cv)), f"highest_above_plane(top {k}/{len(pts_cv)})" - - -def _r_wrist_marker_from_config(cfg: dict[str, Any]) -> np.ndarray: - """Rotation part of the marker-on-wrist rigid offset. Translation does not - matter for camera-frame delta servo. Read from - `grasp.visual_servo.wrist_marker_rvec` as a 3-vector (Rodrigues), default - identity.""" - vs = cfg.get("grasp", {}).get("visual_servo", {}) or {} - rvec = vs.get("wrist_marker_rvec") or [0.0, 0.0, 0.0] - return rvec_to_R(np.asarray(rvec, dtype=np.float64)) - - -def plan(*, side: str, marker_id: int, size_mm: float, - conf: float | None, class_name: str | None, - min_distance_m: float, step_fraction: float, - pointcloud_step: int, pointcloud_max_depth_mm: int, - cfg: dict[str, Any], - nuwa: NuwaClient, marker: MarkerClient, yolo: YoloClient, g1: G1Client, - ) -> ServoPlan: - # 1) Wrist pose in base - wrist_pos, wrist_quat, wrist_quat_list = _resolve_wrist_pose(g1, side) - R_bw = quat_xyzw_to_R(wrist_quat) - - # 2) Marker pose in camera - R_cm, t_cm, corners_px, reproj_err, marker_t_source = _detect_marker_cam( - nuwa, marker, marker_id, size_mm, - ) - marker_pixel = corners_px.mean(axis=0) # centre of the 4 corners - # Intrinsics for mm/px scale at each point's depth. - try: - intr = nuwa.intrinsics() - K, _ = nuwa_intrinsics_to_matrix(intr) - fx = float(K[0, 0]) - except Exception as e: - log.warning("intrinsics unavailable for mm/px (%s)", e) - fx = float("nan") - - # 3) Object reference point in camera: highest-above-plane if a plane is - # set, otherwise centroid of the bbox pointcloud. - wp_here = get_work_plane() - centroid_cam, detection, n_pts, object_point_source = _detect_object_point_cam( - nuwa, yolo, conf, class_name, - pointcloud_step, pointcloud_max_depth_mm, - work_plane=wp_here, - ) - b = detection["bbox"] - object_pixel = np.array([0.5 * (b["x1"] + b["x2"]), 0.5 * (b["y1"] + b["y2"])]) - - # 4) Rotation mapping cam→base (identity marker-to-wrist rotation by default) - R_wm = _r_wrist_marker_from_config(cfg) - R_base_cam = R_bw @ R_wm @ np.linalg.inv(R_cm) - - # 5) Camera-frame vector & distance (marker → object) - d_cam = centroid_cam - t_cm - distance_m = float(np.linalg.norm(d_cam)) - done = distance_m <= float(min_distance_m) - - # 6) Estimate T_base_camera for visualisation (identity T_wrist_marker): - # translation part is off by the real marker-to-wrist translation, but - # relative positions of marker and object in base frame are preserved. - T_bc_est = np.eye(4) - T_bc_est[:3, :3] = R_base_cam - T_bw_full = T_from_pos_quat(wrist_pos, wrist_quat) - T_wm_identity = np.eye(4) # translation part left at zero - T_wm_identity[:3, :3] = R_wm - T_cm_full = make_T(R_cm, t_cm) - T_bc_est = T_bw_full @ T_wm_identity @ invert_T(T_cm_full) - - marker_base_est = (T_bc_est @ np.array([*t_cm, 1.0]))[:3] - object_base_est = (T_bc_est @ np.array([*centroid_cam, 1.0]))[:3] - - # 7) Plan step - target_pos = None - target_quat = None - planned_step_m = 0.0 - reason = None - if done: - reason = f"distance {distance_m*1000:.1f} mm ≤ {min_distance_m*1000:.0f} mm — reached" - else: - step_cam = d_cam * float(step_fraction) - step_len = float(np.linalg.norm(step_cam)) - # Don't overshoot below min_distance_m - if distance_m - step_len < float(min_distance_m): - step_cam = d_cam * ((distance_m - float(min_distance_m)) / distance_m) - step_len = float(np.linalg.norm(step_cam)) - reason = "clamped to stop at min_distance_m" - step_base = R_base_cam @ step_cam - target_pos = wrist_pos + step_base - target_quat = wrist_quat_list - planned_step_m = float(np.linalg.norm(step_base)) - - # 8) Work-plane projection (optional) - plane_block = None - wp = get_work_plane() - if wp is not None: - try: - K_plane, _ = nuwa_intrinsics_to_matrix(nuwa.intrinsics()) - n_vec = np.asarray(wp.normal, dtype=np.float64) - d_off = float(wp.offset) - # Perpendicular foot of the measured 3D points. - m_foot, m_signed = project_point_on_plane(np.asarray(t_cm), n_vec, d_off) - o_foot, o_signed = project_point_on_plane(np.asarray(centroid_cam), n_vec, d_off) - # Back-projected pixel hits on the plane (trust pixel + depth plane, - # ignoring the measured Z of each point — more stable when depth is - # noisy, at the cost of assuming the point actually lies on the - # plane). - m_hit = backproject_pixel_on_plane(np.asarray(marker_pixel), K_plane, n_vec, d_off) - o_hit = backproject_pixel_on_plane(np.asarray(object_pixel), K_plane, n_vec, d_off) - plane_block = { - "normal": wp.normal, - "offset": wp.offset, - "fit_rms_mm": wp.fit_rms_mm, - "roi": wp.roi, - "marker_foot_cam": [float(x) for x in m_foot], - "object_foot_cam": [float(x) for x in o_foot], - "marker_signed_distance_m": float(m_signed), - "object_signed_distance_m": float(o_signed), - "marker_pixel_hit_cam": [float(x) for x in m_hit], - "object_pixel_hit_cam": [float(x) for x in o_hit], - "in_plane_distance_m": float(np.linalg.norm(o_foot - m_foot)), - "in_plane_pixel_distance_m": float(np.linalg.norm(o_hit - m_hit)), - } - except Exception as e: - log.warning("plane projection failed: %s", e) - - return ServoPlan( - wrist_position_base=[float(x) for x in wrist_pos], - wrist_quaternion_base=wrist_quat_list, - marker_position_cam=[float(x) for x in t_cm], - object_position_cam=[float(x) for x in centroid_cam], - marker_pixel=[float(x) for x in marker_pixel], - object_pixel=[float(x) for x in object_pixel], - marker_depth_z_m=float(abs(t_cm[2])), - object_depth_z_m=float(abs(centroid_cam[2])), - marker_distance_cam_m=float(np.linalg.norm(t_cm)), - object_distance_cam_m=float(np.linalg.norm(centroid_cam)), - marker_mm_per_px=float(abs(t_cm[2]) * 1000.0 / fx) if fx == fx and fx > 0 else 0.0, - object_mm_per_px=float(abs(centroid_cam[2]) * 1000.0 / fx) if fx == fx and fx > 0 else 0.0, - marker_position_base_est=[float(x) for x in marker_base_est], - object_position_base_est=[float(x) for x in object_base_est], - T_base_camera_est=T_bc_est.tolist(), - distance_m=distance_m, - done=done, - target_position_base=[float(x) for x in target_pos] if target_pos is not None else None, - target_quaternion_base=target_quat, - planned_step_m=planned_step_m, - plane=plane_block, - detection=detection, - n_object_points=n_pts, - marker_reprojection_error_px=reproj_err, - marker_translation_source=marker_t_source, - object_point_source=object_point_source, - reason=reason, - ) diff --git a/src/presa_mano/main.py b/src/presa_mano/main.py index fe77461..6be1893 100644 --- a/src/presa_mano/main.py +++ b/src/presa_mano/main.py @@ -19,6 +19,7 @@ from .api import ( posture_routes, robot_routes, wrist_routes, + work_plane_routes, ) from .clients import G1StatusError from .config import load_config @@ -42,6 +43,7 @@ app.include_router(grasp_routes.router, prefix="/api/grasp", tags=["grasp"]) app.include_router(wrist_routes.router, prefix="/api/wrist", tags=["wrist"]) app.include_router(posture_routes.router, prefix="/api/posture", tags=["posture"]) app.include_router(robot_routes.router, prefix="/api/robot", tags=["robot"]) +app.include_router(work_plane_routes.router, prefix="/api/work_plane", tags=["work_plane"]) WEB_DIR = Path(__file__).resolve().parents[2] / "web" if WEB_DIR.exists(): @@ -101,7 +103,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", "servo.js", "style.css"): + for name in ("app.js", "work_plane.js", "style.css"): f = WEB_DIR / name if f.exists(): v = int(f.stat().st_mtime) diff --git a/src/presa_mano/perception/sam21_bridge.py b/src/presa_mano/perception/sam21_bridge.py index c488b60..7efdbfc 100644 --- a/src/presa_mano/perception/sam21_bridge.py +++ b/src/presa_mano/perception/sam21_bridge.py @@ -17,6 +17,7 @@ from ..clients import NuwaError, Sam21Error, Sam21Unavailable from ..timing import Stopwatch from .depth_align import align_depth_to_rgb from .depth_to_rgb import CameraRegistration +from .work_plane import apply_below_plane_filter, get_work_plane log = logging.getLogger(__name__) @@ -31,13 +32,17 @@ SAM21_BUG_CODES = frozenset({"SHAPE_MISMATCH", "BBOX_OUT_OF_BOUNDS", def points_via_sam21(nuwa, sam21, bbox: tuple[int, int, int, int], - sw: Stopwatch) -> np.ndarray: + sw: Stopwatch) -> tuple[np.ndarray, dict]: """SAM21 path: bbox → segmented point cloud in RGB-camera/OpenCV frame. Returns points in **meters**. Raises Sam21Fallback on failures that should trigger the bbox path. Re-raises HTTPException(502) for Nuwa failures (the bbox path would fail too — better to fail fast). All SAM21-specific logging happens here; callers do not log on fallback. + + After the SAM21 segmentation the work-plane filter is applied (no-op if no + plane is calibrated). Returns (points, plane_info) where plane_info has + `plane_applied` and `n_points_before_plane`. """ try: with sw.step("nuwa_grab"): @@ -120,7 +125,12 @@ def points_via_sam21(nuwa, sam21, bbox: tuple[int, int, int, int], log.error("Unexpected SAM21 error: %r", e) raise Sam21Fallback(f"unexpected: {e!r}") from e + pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane()) if pts_cv.shape[0] < 10: - log.warning("SAM21 returned %d points (<10), falling back to bbox", pts_cv.shape[0]) - raise Sam21Fallback(f"too few points: {pts_cv.shape[0]}") - return pts_cv + log.warning("SAM21 cloud has %d points after plane filter (<10), falling back to bbox", + pts_cv.shape[0]) + raise Sam21Fallback( + f"too few points after plane filter: {pts_cv.shape[0]}" + f" (before plane: {plane_info['n_points_before_plane']})" + ) + return pts_cv, plane_info diff --git a/src/presa_mano/perception/work_plane.py b/src/presa_mano/perception/work_plane.py new file mode 100644 index 0000000..af9ca65 --- /dev/null +++ b/src/presa_mano/perception/work_plane.py @@ -0,0 +1,146 @@ +"""Work-plane fit, persistence, and below-plane filter. + +The work plane is fitted from a user-drawn rectangle on the RGB image. The +fit uses SVD on the bbox depth cloud (after `nuwa_to_cv_frame`), so the +plane normal lives in OpenCV RGB-camera frame and is oriented toward the +camera (negative Z component in OpenCV convention). + +The filter `apply_below_plane_filter` removes points more than `offset_mm` +below the plane: keep `(normal · X + offset) >= -offset_mm/1000`. A +positive `offset_mm` is permissive (preserves a buffer below the plane). +""" +from __future__ import annotations + +import json +import logging +from dataclasses import asdict, dataclass +from pathlib import Path + +import numpy as np + +from ..config import data_dir + +log = logging.getLogger(__name__) + + +class WorkPlaneError(RuntimeError): + """Bad input to `fit_plane_svd` (too few points, degenerate cloud).""" + + +@dataclass +class WorkPlane: + """Plane in CV camera frame. `normal · X + offset = 0`; `normal` is + unit-length and oriented toward the camera (negative Z in OpenCV).""" + normal: list[float] + offset: float + centroid: list[float] + n_inliers: int + fit_rms_mm: float + roi: list[int] + offset_mm: float + created_at: float + + +_WORK_PLANE: WorkPlane | None = None +_WORK_PLANE_LOADED = False + + +def _work_plane_path() -> Path: + return data_dir() / "work_plane.json" + + +def _save_work_plane_to_disk(wp: WorkPlane | None) -> None: + path = _work_plane_path() + try: + if wp is None: + if path.exists(): + path.unlink() + return + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(json.dumps(asdict(wp), indent=2)) + except Exception as e: + log.warning("failed to persist work plane: %s", e) + + +def _load_work_plane_from_disk() -> WorkPlane | None: + path = _work_plane_path() + try: + with open(path, "r") as f: + d = json.load(f) + d.setdefault("offset_mm", 5.0) # legacy files + return WorkPlane(**d) + except FileNotFoundError: + return None + except Exception as e: + log.warning("failed to load persisted work plane (%s)", e) + return None + + +def get_work_plane() -> WorkPlane | None: + global _WORK_PLANE, _WORK_PLANE_LOADED + if not _WORK_PLANE_LOADED: + _WORK_PLANE = _load_work_plane_from_disk() + _WORK_PLANE_LOADED = True + return _WORK_PLANE + + +def set_work_plane(wp: WorkPlane | None) -> None: + global _WORK_PLANE, _WORK_PLANE_LOADED + _WORK_PLANE = wp + _WORK_PLANE_LOADED = True + _save_work_plane_to_disk(wp) + + +def fit_plane_svd(points_xyz: np.ndarray + ) -> tuple[np.ndarray, float, np.ndarray, float]: + """Least-squares plane via SVD. Returns (normal, offset, centroid, rms_mm). + + Plane equation: `normal · X + offset = 0`. Normal is oriented toward + the camera origin (so `signed_distance = normal · X + offset` is + positive for points on the camera side of the plane).""" + pts = np.asarray(points_xyz, dtype=np.float64) + if pts.ndim != 2 or pts.shape[1] < 3: + raise WorkPlaneError(f"unexpected points shape for plane fit: {pts.shape}") + xyz = pts[:, :3] + if len(xyz) < 10: + raise WorkPlaneError(f"too few points to fit plane: {len(xyz)}") + c = xyz.mean(axis=0) + centered = xyz - c + _, _, Vt = np.linalg.svd(centered, full_matrices=False) + normal = Vt[-1] + n = float(np.linalg.norm(normal)) + if n < 1e-9: + raise WorkPlaneError("degenerate plane (zero normal)") + normal = normal / n + # Orient toward camera origin: signed distance of origin from plane must + # be positive, i.e. `-n·c > 0`, i.e. `n·c < 0`. + if float(normal @ c) > 0: + normal = -normal + offset = -float(normal @ c) + signed = centered @ normal + rms_mm = float(np.sqrt(np.mean(signed * signed)) * 1000.0) + return normal, offset, c, rms_mm + + +def apply_below_plane_filter( + pts_cv: np.ndarray, + wp: WorkPlane | None, +) -> tuple[np.ndarray, dict]: + """Drop points more than `offset_mm` below the plane. + + Returns `(filtered_pts, info)`. `info` always carries: + plane_applied (bool): true iff a plane was provided AND there were + points to filter + n_points_before_plane (int): len(pts_cv) on entry + """ + info = {"plane_applied": False, + "n_points_before_plane": int(len(pts_cv))} + if wp is None or len(pts_cv) == 0: + return pts_cv, info + n_vec = np.asarray(wp.normal, dtype=np.float64) + d_off = float(wp.offset) + offset_m = float(wp.offset_mm) / 1000.0 + signed = pts_cv @ n_vec + d_off + keep = signed >= -offset_m + info["plane_applied"] = True + return pts_cv[keep], info diff --git a/tests/test_work_plane.py b/tests/test_work_plane.py new file mode 100644 index 0000000..3622000 --- /dev/null +++ b/tests/test_work_plane.py @@ -0,0 +1,142 @@ +"""Unit tests for perception.work_plane.""" +from __future__ import annotations + +import json + +import numpy as np +import pytest + +from presa_mano.perception.work_plane import ( + WorkPlane, + WorkPlaneError, + apply_below_plane_filter, + fit_plane_svd, + _load_work_plane_from_disk, + _save_work_plane_to_disk, + _work_plane_path, +) + + +def _flat_plane_cloud(z: float = 1.0, n: int = 200, noise_m: float = 0.0): + """Cloud sampled on the plane z=z (camera frame, OpenCV: camera looks +Z).""" + rng = np.random.default_rng(0) + xy = rng.uniform(-0.2, 0.2, size=(n, 2)) + zs = np.full((n, 1), z) + rng.normal(0, noise_m, size=(n, 1)) + return np.hstack([xy, zs]) + + +def test_fit_plane_svd_horizontal_plane(): + """Plane at z=1.0, normal must point toward camera (negative Z).""" + pts = _flat_plane_cloud(z=1.0, n=200, noise_m=0.0) + normal, offset, centroid, rms_mm = fit_plane_svd(pts) + assert abs(normal[2]) > 0.99 + assert normal[2] < 0 + assert abs(offset - 1.0) < 1e-3 + assert rms_mm < 1.0 + + +def test_fit_plane_svd_raises_on_too_few_points(): + with pytest.raises(WorkPlaneError): + fit_plane_svd(np.zeros((5, 3))) + + +def test_apply_below_plane_filter_noop_without_plane(): + pts = _flat_plane_cloud(z=1.0) + out, info = apply_below_plane_filter(pts, None) + assert info["plane_applied"] is False + assert info["n_points_before_plane"] == len(pts) + assert out is pts + + +def test_apply_below_plane_filter_keeps_above_drops_below(): + """Plane at z=1.0, normal toward camera (-Z): points with z<1.0 are above.""" + above = _flat_plane_cloud(z=0.95, n=100) + below = _flat_plane_cloud(z=1.10, n=100) + pts = np.vstack([above, below]) + wp = WorkPlane( + normal=[0.0, 0.0, -1.0], offset=1.0, + centroid=[0.0, 0.0, 1.0], + n_inliers=100, fit_rms_mm=0.0, + roi=[0, 0, 10, 10], + offset_mm=0.0, + created_at=0.0, + ) + out, info = apply_below_plane_filter(pts, wp) + assert info["plane_applied"] is True + assert info["n_points_before_plane"] == 200 + assert len(out) == 100 + assert np.all(out[:, 2] <= 1.0 + 1e-9) + + +def test_apply_below_plane_filter_offset_buffer_keeps_below(): + """offset_mm=50 keeps points up to 50mm below the plane.""" + above = _flat_plane_cloud(z=0.95, n=50) + just_below = _flat_plane_cloud(z=1.03, n=50) + far_below = _flat_plane_cloud(z=1.20, n=50) + pts = np.vstack([above, just_below, far_below]) + wp = WorkPlane( + normal=[0.0, 0.0, -1.0], offset=1.0, + centroid=[0.0, 0.0, 1.0], + n_inliers=50, fit_rms_mm=0.0, + roi=[0, 0, 10, 10], + offset_mm=50.0, + created_at=0.0, + ) + out, info = apply_below_plane_filter(pts, wp) + assert info["plane_applied"] is True + assert len(out) == 100 # above (50) + just_below (50); far_below dropped + + +def test_apply_below_plane_filter_empty_input(): + wp = WorkPlane( + normal=[0.0, 0.0, -1.0], offset=1.0, + centroid=[0.0, 0.0, 1.0], + n_inliers=10, fit_rms_mm=0.0, + roi=[0, 0, 10, 10], + offset_mm=5.0, created_at=0.0, + ) + out, info = apply_below_plane_filter(np.empty((0, 3)), wp) + assert info["plane_applied"] is False # short-circuit: empty input never "ran" + assert info["n_points_before_plane"] == 0 + assert len(out) == 0 + + +def test_work_plane_disk_roundtrip(tmp_path, monkeypatch): + """Save then load preserves all fields including offset_mm.""" + monkeypatch.setenv("PRESA_MANO_DATA", str(tmp_path)) + wp = WorkPlane( + normal=[0.0, 0.0, -1.0], offset=1.0, + centroid=[0.1, 0.2, 1.0], + n_inliers=42, fit_rms_mm=1.5, + roi=[10, 20, 100, 200], + offset_mm=7.5, + created_at=1234567890.0, + ) + _save_work_plane_to_disk(wp) + loaded = _load_work_plane_from_disk() + assert loaded is not None + assert loaded.normal == wp.normal + assert loaded.offset == wp.offset + assert loaded.centroid == wp.centroid + assert loaded.n_inliers == wp.n_inliers + assert loaded.fit_rms_mm == wp.fit_rms_mm + assert loaded.roi == wp.roi + assert loaded.offset_mm == 7.5 + assert loaded.created_at == wp.created_at + + +def test_work_plane_load_legacy_file_defaults_offset(tmp_path, monkeypatch): + """A pre-existing work_plane.json without offset_mm gets the 5.0 default.""" + monkeypatch.setenv("PRESA_MANO_DATA", str(tmp_path)) + path = _work_plane_path() + legacy = { + "normal": [0.0, 0.0, -1.0], "offset": 1.0, + "centroid": [0.0, 0.0, 1.0], + "n_inliers": 10, "fit_rms_mm": 1.0, + "roi": [0, 0, 10, 10], + "created_at": 0.0, + } + path.write_text(json.dumps(legacy)) + loaded = _load_work_plane_from_disk() + assert loaded is not None + assert loaded.offset_mm == 5.0 diff --git a/web/index.html b/web/index.html index 20a6fb1..e6e9c98 100644 --- a/web/index.html +++ b/web/index.html @@ -13,8 +13,8 @@ - - + + @@ -174,6 +174,56 @@ + +
+
+ — +
+
+
+
+
+ Nuwa RGB + +
+
+
RGB live · shift+drag = ROI
+

Vista 3D (frame camera)

+
+ +
+
piano viola · verde = inlier · giallo = sopra piano+offset · rosso = sotto piano-offset · grigio = buffer · drag = ruota · wheel = zoom
+
+
+

Calibrazione

+
+
+ Disegna con shift+drag un rettangolo sul tavolo (senza oggetti), poi + clicca Calcola piano. Regola l'offset finché i punti + gialli rappresentano l'oggetto e i rossi spariscono. Poi Salva piano. +
+
+ + +
+
+ + + +
+
+ +

Stato

+
+
nessun piano
+
+ +

Dettagli piano

+ 
—
+
+
+
+
@@ -215,102 +265,17 @@
-

Precisione (ultime 5 detection)

- - - - - + +
+ 5 +
assemedia [m]σ [mm]range [mm]minmax
- -
x—————
y—————
z—————
-
loop: inattivo
- -

Dettagli

- 
(nessuna detection)
- - -
- - -
-
- — -
-
-
-
-
- Nuwa RGB - -
+ +
+ 

       

-      
RGB live · ciano = marker · giallo = oggetto · wheel zoom · drag pan · doppio click reset

-      
Vista 3D (frame camera)

-      

-        
-      

-      
origine = camera · piano viola · marker ciano · oggetto giallo · proiezioni più chiare · drag ruota · wheel zoom

-      

-        
 marker polso

-        
 oggetto (centro bbox)

-      

-    

-    

-      
Parametri

-      

-        

-          Avvicinamento marker-relativo senza hand-eye calibration: la camera vede marker e oggetto nello stesso frame, ogni step sposta il polso della frazione indicata della distanza rimanente. Si ferma a min_distance_m.
-        

-        

-          
-          
-          
-          
-          
-          
-          
-        

-        

-          Orientazione marker rispetto al polso (rvec in gradi). Se non allineato al frame polso il target risulta spostato: tuna finché marker, oggetto e target sono coerenti.
-        

-        

-          
-          
-          
-          
-        

-        

-          
-          
-          
-        

-        

-          Piano di lavoro. Clicca "Seleziona area", poi traccia un rettangolo sull'immagine con shift+drag: il backend calcola il piano dalla depth in quell'area. Marker e oggetto vengono poi proiettati su questo piano.
-        

-        

-          
-          
-          
-        

-        
piano: —

-      

-
-      
Stato

-      

-        
nessun piano calcolato

-      

-
-      
Dettagli piano

-      
—

     

   

 

@@ -419,6 +384,6 @@
 }
 
 
-
+
 
 
diff --git a/web/servo.js b/web/servo.js
deleted file mode 100644
index 7194f1a..0000000
--- a/web/servo.js
+++ /dev/null
@@ -1,669 +0,0 @@
-/* Visual-servo tab: live Nuwa RGB with marker + object-centre overlays.
- * Each click: plan → render overlay + stats → user confirms → step → wait →
- * re-plan → render. No 3D viewer in this tab — it's an image-space tool.
- */
-
-import * as THREE from 'three';
-import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
-
-const $ = (id) => document.getElementById(id);
-const LS_KEY = 'presaMano.servoParams.v1';
-const DEG = Math.PI / 180;
-
-// ----- 3D scene (camera-frame) ----------------------------------------------
-let scene3d = null, cam3d = null, renderer3d = null, controls3d = null;
-let scene3dObjs = {};   // named overlays to clear/update on each plan
-
-function boot3d() {
-  if (scene3d) return;
-  const canvas = $('servo3dCanvas');
-  if (!canvas) return;
-  scene3d = new THREE.Scene();
-  scene3d.background = new THREE.Color(0x0a0d11);
-  // Camera looks from outside the scene; world frame == Nuwa camera frame
-  // (OpenCV: +X right, +Y down, +Z forward). We orbit around the origin.
-  cam3d = new THREE.PerspectiveCamera(45, canvas.width / canvas.height, 0.05, 20);
-  cam3d.position.set(0.8, -0.6, -0.2);
-  cam3d.up.set(0, -1, 0);                // Y-down so CV axes match visually
-  renderer3d = new THREE.WebGLRenderer({ canvas, antialias: true });
-  renderer3d.setPixelRatio(window.devicePixelRatio || 1);
-  renderer3d.setSize(canvas.width, canvas.height, false);
-  controls3d = new OrbitControls(cam3d, canvas);
-  controls3d.target.set(0, 0, 1.2);
-  controls3d.update();
-
-  scene3d.add(new THREE.AmbientLight(0xffffff, 0.6));
-  const dir = new THREE.DirectionalLight(0xffffff, 0.6);
-  dir.position.set(1, -1, 1);
-  scene3d.add(dir);
-
-  // Camera-origin marker (small axes triad)
-  const ax = new THREE.AxesHelper(0.2);
-  scene3d.add(ax);
-  // Small grid at Z=1.5 to anchor the eye when there's no plane yet.
-  const grid = new THREE.GridHelper(2, 10, 0x222a33, 0x1a2029);
-  grid.rotation.x = Math.PI / 2;
-  grid.position.z = 1.5;
-  scene3d.add(grid);
-
-  // Frustum wireframe (approximate FOV lines up to 2 m depth).
-  const fh = 0.7, fw = 1.0, fz = 2.0;
-  const frPts = [
-    0,0,0,  fw,  fh, fz,
-    0,0,0, -fw,  fh, fz,
-    0,0,0,  fw, -fh, fz,
-    0,0,0, -fw, -fh, fz,
-    fw,  fh, fz,  fw, -fh, fz,
-    fw, -fh, fz, -fw, -fh, fz,
-   -fw, -fh, fz, -fw,  fh, fz,
-   -fw,  fh, fz,  fw,  fh, fz,
-  ];
-  const frGeo = new THREE.BufferGeometry();
-  frGeo.setAttribute('position', new THREE.Float32BufferAttribute(frPts, 3));
-  const frMat = new THREE.LineBasicMaterial({ color: 0x2a3340 });
-  scene3d.add(new THREE.LineSegments(frGeo, frMat));
-
-  animate3d();
-}
-
-function animate3d() {
-  requestAnimationFrame(animate3d);
-  if (controls3d) controls3d.update();
-  if (renderer3d && scene3d && cam3d) renderer3d.render(scene3d, cam3d);
-}
-
-function clear3dOverlays() {
-  for (const k of Object.keys(scene3dObjs)) {
-    const o = scene3dObjs[k];
-    if (!o) continue;
-    scene3d.remove(o);
-    if (o.geometry) o.geometry.dispose();
-    if (o.material) (Array.isArray(o.material) ? o.material : [o.material]).forEach(m => m.dispose());
-    scene3dObjs[k] = null;
-  }
-}
-
-function addSphere3d(key, xyz, color, radius = 0.025) {
-  const geom = new THREE.SphereGeometry(radius, 24, 16);
-  const mat = new THREE.MeshStandardMaterial({ color, emissive: color, emissiveIntensity: 0.25 });
-  const m = new THREE.Mesh(geom, mat);
-  m.position.set(xyz[0], xyz[1], xyz[2]);
-  scene3d.add(m);
-  scene3dObjs[key] = m;
-}
-
-function addLine3d(key, a, b, color) {
-  const g = new THREE.BufferGeometry().setFromPoints([
-    new THREE.Vector3(...a), new THREE.Vector3(...b),
-  ]);
-  const m = new THREE.Line(g, new THREE.LineBasicMaterial({ color }));
-  scene3d.add(m);
-  scene3dObjs[key] = m;
-}
-
-function addPlaneMesh3d(key, normal, offset, anchor, size = 1.2, color = 0x9b6dff) {
-  const n = new THREE.Vector3(...normal).normalize();
-  // Build two in-plane orthonormal vectors.
-  const up = Math.abs(n.y) < 0.9 ? new THREE.Vector3(0, 1, 0) : new THREE.Vector3(1, 0, 0);
-  const u = new THREE.Vector3().crossVectors(up, n).normalize();
-  const v = new THREE.Vector3().crossVectors(n, u).normalize();
-  // Centre the mesh on the anchor point (foot of camera onto plane if given).
-  const anchorVec = new THREE.Vector3(...anchor);
-  // Pull anchor onto the plane to be safe.
-  const s = anchorVec.dot(n) + offset;
-  anchorVec.addScaledVector(n, -s);
-  const geom = new THREE.BufferGeometry();
-  const h = size / 2;
-  const corners = [
-    anchorVec.clone().addScaledVector(u, -h).addScaledVector(v, -h),
-    anchorVec.clone().addScaledVector(u,  h).addScaledVector(v, -h),
-    anchorVec.clone().addScaledVector(u,  h).addScaledVector(v,  h),
-    anchorVec.clone().addScaledVector(u, -h).addScaledVector(v,  h),
-  ];
-  const pos = [];
-  for (const c of corners) pos.push(c.x, c.y, c.z);
-  geom.setAttribute('position', new THREE.Float32BufferAttribute(pos, 3));
-  geom.setIndex([0, 1, 2, 0, 2, 3]);
-  geom.computeVertexNormals();
-  const mat = new THREE.MeshStandardMaterial({
-    color, opacity: 0.35, transparent: true, side: THREE.DoubleSide,
-  });
-  const mesh = new THREE.Mesh(geom, mat);
-  scene3d.add(mesh);
-  scene3dObjs[key] = mesh;
-  // Edges
-  const edgeGeom = new THREE.EdgesGeometry(geom);
-  const edgeMat = new THREE.LineBasicMaterial({ color });
-  const edges = new THREE.LineSegments(edgeGeom, edgeMat);
-  scene3d.add(edges);
-  scene3dObjs[key + '_edges'] = edges;
-}
-
-function redraw3d() {
-  if (!scene3d) return;
-  clear3dOverlays();
-  const p = lastPlan && lastPlan.plan;
-  // Plane (from stored planeInfo, independent of current plan)
-  if (planeInfo) {
-    const anchor = planeInfo.centroid || [0, 0, 1.5];
-    addPlaneMesh3d('plane', planeInfo.normal, planeInfo.offset, anchor);
-  }
-  if (!p) return;
-  addSphere3d('marker', p.marker_position_cam, 0x4dd0e1, 0.022);
-  addSphere3d('object', p.object_position_cam, 0xffd54f, 0.028);
-  addLine3d('mo_line', p.marker_position_cam, p.object_position_cam, 0xbbbbbb);
-  if (p.plane) {
-    addSphere3d('marker_foot', p.plane.marker_foot_cam, 0x80ecf7, 0.018);
-    addSphere3d('object_foot', p.plane.object_foot_cam, 0xffe998, 0.020);
-    addLine3d('m_drop', p.marker_position_cam, p.plane.marker_foot_cam, 0x4dd0e1);
-    addLine3d('o_drop', p.object_position_cam, p.plane.object_foot_cam, 0xffd54f);
-    addLine3d('foot_line', p.plane.marker_foot_cam, p.plane.object_foot_cam, 0x9b6dff);
-  }
-}
-
-let lastPlan = null;
-let busy = false;
-let imgRefreshTimer = null;
-// Image native dimensions (from /api/intrinsics or from the first load).
-let imgW = 0, imgH = 0;
-// Pan/zoom state: scale and translate applied to the inner pan-zoom wrapper.
-const view = { scale: 1, tx: 0, ty: 0 };
-let drag = null;
-// Work plane selection state.
-let planeInfo = null;             // last response from /plane/fit or /plane GET
-let roiMode = false;              // true when user clicked "Seleziona area"
-let roi = null;                   // {u0,v0,u1,v1} in image coords (committed)
-let roiDrag = null;               // in-progress drag
-
-function setStatus(msg, cls) {
-  const el = $('servoStatus');
-  if (!el) return;
-  el.textContent = msg;
-  el.style.color = cls === 'err' ? 'var(--danger)' : cls === 'ok' ? 'var(--ok)' : 'var(--dim)';
-}
-
-function readParams() {
-  const side = $('svSide').value || null;
-  const cls = $('svClass').value.trim();
-  const conf = $('svConf').value.trim();
-  const body = {
-    min_distance_m: (+$('svMinDist').value) / 1000,
-    step_fraction: +$('svStepFrac').value,
-  };
-  if (cls) body.class_name = cls;
-  if (conf) body.conf = +conf;
-  if (side) body.side = side;
-  const rx = +$('svRx').value, ry = +$('svRy').value, rz = +$('svRz').value;
-  if (rx || ry || rz) {
-    body.wrist_marker_rvec = [rx * DEG, ry * DEG, rz * DEG];
-  }
-  return body;
-}
-
-function persistParams() {
-  try {
-    localStorage.setItem(LS_KEY, JSON.stringify({
-      cls: $('svClass').value, conf: $('svConf').value,
-      minDist: $('svMinDist').value, step: $('svStepFrac').value,
-      timeout: $('svTimeout').value, side: $('svSide').value,
-      dryRun: $('svDryRun').checked,
-      rx: $('svRx').value, ry: $('svRy').value, rz: $('svRz').value,
-    }));
-  } 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; };
-    setV('svClass', p.cls); setV('svConf', p.conf);
-    setV('svMinDist', p.minDist); setV('svStepFrac', p.step);
-    setV('svTimeout', p.timeout); setV('svSide', p.side);
-    setV('svRx', p.rx); setV('svRy', p.ry); setV('svRz', p.rz);
-    if ($('svDryRun') && p.dryRun != null) $('svDryRun').checked = !!p.dryRun;
-  } catch {}
-}
-
-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 saveRvecToConfig() {
-  const rx = +$('svRx').value, ry = +$('svRy').value, rz = +$('svRz').value;
-  const rvec = [rx * DEG, ry * DEG, rz * DEG];
-  try {
-    await api('PUT', '/api/config', {
-      grasp: { visual_servo: { wrist_marker_rvec: rvec } },
-    });
-    setStatus(`rvec salvato: [${rx}°, ${ry}°, ${rz}°]`, 'ok');
-  } catch (e) {
-    setStatus('salvataggio rvec fallito: ' + e.message, 'err');
-  }
-}
-
-function refreshImg() {
-  const img = $('servoImg');
-  if (!img) return;
-  // Cache-bust so we always see the current frame.
-  img.src = `/api/calibration/rgb.jpg?t=${Date.now()}`;
-}
-
-function resizeOverlay() {
-  const img = $('servoImg');
-  const cvs = $('servoOverlay');
-  if (!img || !cvs) return;
-  if (img.naturalWidth) { imgW = img.naturalWidth; imgH = img.naturalHeight; }
-  if (!imgW || !imgH) return;
-  // Match canvas pixel size to the native image size; CSS transform on the
-  // wrapper handles zoom/pan, keeping overlay and image perfectly aligned.
-  const dpr = window.devicePixelRatio || 1;
-  cvs.width = Math.round(imgW * dpr);
-  cvs.height = Math.round(imgH * dpr);
-  cvs.style.width = imgW + 'px';
-  cvs.style.height = imgH + 'px';
-  img.style.width = imgW + 'px';
-  img.style.height = imgH + 'px';
-  // Fit-to-viewport on first sizing.
-  if (view.scale === 1 && view.tx === 0 && view.ty === 0) fitView();
-  else applyTransform();
-  redrawOverlay();
-}
-
-function applyTransform() {
-  const wrap = $('servoPanZoom');
-  if (!wrap) return;
-  wrap.style.transform = `translate(${view.tx}px, ${view.ty}px) scale(${view.scale})`;
-}
-
-function fitView() {
-  const vp = $('servoViewport');
-  if (!vp || !imgW || !imgH) return;
-  const r = vp.getBoundingClientRect();
-  const s = Math.min(r.width / imgW, r.height / imgH);
-  view.scale = s;
-  view.tx = (r.width - imgW * s) / 2;
-  view.ty = (r.height - imgH * s) / 2;
-  applyTransform();
-}
-
-function zoomAt(clientX, clientY, factor) {
-  const vp = $('servoViewport');
-  const r = vp.getBoundingClientRect();
-  const px = clientX - r.left, py = clientY - r.top;
-  // Image coords under the cursor BEFORE scaling:
-  const ix = (px - view.tx) / view.scale;
-  const iy = (py - view.ty) / view.scale;
-  const newScale = Math.min(16, Math.max(0.1, view.scale * factor));
-  // Keep (ix, iy) under the cursor after the new scale.
-  view.tx = px - ix * newScale;
-  view.ty = py - iy * newScale;
-  view.scale = newScale;
-  applyTransform();
-}
-
-function clientToImage(clientX, clientY) {
-  const vp = $('servoViewport');
-  const r = vp.getBoundingClientRect();
-  const px = clientX - r.left, py = clientY - r.top;
-  return [(px - view.tx) / view.scale, (py - view.ty) / view.scale];
-}
-
-function wireViewport() {
-  const vp = $('servoViewport');
-  if (!vp) return;
-  vp.addEventListener('wheel', (e) => {
-    e.preventDefault();
-    const factor = e.deltaY < 0 ? 1.15 : 1 / 1.15;
-    zoomAt(e.clientX, e.clientY, factor);
-  }, { passive: false });
-  vp.addEventListener('pointerdown', (e) => {
-    // ROI selection takes priority when enabled, OR when user holds Shift.
-    if (roiMode || e.shiftKey) {
-      const [ix, iy] = clientToImage(e.clientX, e.clientY);
-      roiDrag = { u0: ix, v0: iy, u1: ix, v1: iy, id: e.pointerId };
-      vp.setPointerCapture(e.pointerId);
-      redrawOverlay();
-      return;
-    }
-    drag = { x: e.clientX, y: e.clientY, tx: view.tx, ty: view.ty };
-    vp.setPointerCapture(e.pointerId);
-    vp.style.cursor = 'grabbing';
-  });
-  vp.addEventListener('pointermove', (e) => {
-    if (roiDrag) {
-      const [ix, iy] = clientToImage(e.clientX, e.clientY);
-      roiDrag.u1 = ix; roiDrag.v1 = iy;
-      redrawOverlay();
-      return;
-    }
-    if (!drag) return;
-    view.tx = drag.tx + (e.clientX - drag.x);
-    view.ty = drag.ty + (e.clientY - drag.y);
-    applyTransform();
-  });
-  const endDrag = (e) => {
-    if (roiDrag) {
-      try { vp.releasePointerCapture(e.pointerId); } catch {}
-      const r = roiDrag;
-      const u0 = Math.max(0, Math.min(r.u0, r.u1));
-      const v0 = Math.max(0, Math.min(r.v0, r.v1));
-      const u1 = Math.min(imgW || r.u1, Math.max(r.u0, r.u1));
-      const v1 = Math.min(imgH || r.v1, Math.max(r.v0, r.v1));
-      if (u1 - u0 > 5 && v1 - v0 > 5) {
-        roi = { u0: u0|0, v0: v0|0, u1: u1|0, v1: v1|0 };
-      }
-      roiDrag = null;
-      roiMode = false;
-      vp.style.cursor = 'grab';
-      setStatus(roi ? `ROI ${roi.u1-roi.u0}x${roi.v1-roi.v0} px · premi "Calcola piano"` : 'ROI troppo piccola', roi ? 'ok' : 'err');
-      redrawOverlay();
-      return;
-    }
-    if (drag) { try { vp.releasePointerCapture(e.pointerId); } catch {} }
-    drag = null;
-    vp.style.cursor = 'grab';
-  };
-  vp.addEventListener('pointerup', endDrag);
-  vp.addEventListener('pointercancel', endDrag);
-  vp.addEventListener('dblclick', () => fitView());
-}
-
-function drawCross(ctx, x, y, color) {
-  // Sizes expressed in IMAGE pixels so they stay legible after CSS zoom.
-  const r = 18;
-  ctx.strokeStyle = color;
-  ctx.lineWidth = 3;
-  ctx.beginPath();
-  ctx.arc(x, y, r, 0, Math.PI * 2);
-  ctx.stroke();
-  ctx.beginPath();
-  ctx.moveTo(x - r - 6, y); ctx.lineTo(x + r + 6, y);
-  ctx.moveTo(x, y - r - 6); ctx.lineTo(x, y + r + 6);
-  ctx.stroke();
-}
-
-function drawRect(ctx, u0, v0, u1, v1, color, label) {
-  ctx.strokeStyle = color;
-  ctx.lineWidth = 2;
-  ctx.setLineDash([6, 4]);
-  ctx.strokeRect(u0, v0, u1 - u0, v1 - v0);
-  ctx.setLineDash([]);
-  ctx.fillStyle = 'rgba(0,0,0,0.35)';
-  ctx.fillRect(u0, v0, u1 - u0, v1 - v0);
-  if (label) {
-    ctx.font = 'bold 16px ui-monospace, monospace';
-    const w = ctx.measureText(label).width + 10;
-    ctx.fillStyle = color;
-    ctx.fillRect(u0, v0, w, 20);
-    ctx.fillStyle = '#000';
-    ctx.fillText(label, u0 + 5, v0 + 15);
-  }
-}
-
-function redrawOverlay() {
-  const cvs = $('servoOverlay');
-  if (!cvs) return;
-  const ctx = cvs.getContext('2d');
-  const dpr = window.devicePixelRatio || 1;
-  ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
-  ctx.clearRect(0, 0, imgW || cvs.width, imgH || cvs.height);
-  if (!imgW || !imgH) return;
-
-  // Committed ROI
-  if (roi) drawRect(ctx, roi.u0, roi.v0, roi.u1, roi.v1, '#9b6dff', 'ROI piano');
-  // In-progress drag
-  if (roiDrag) {
-    const u0 = Math.min(roiDrag.u0, roiDrag.u1);
-    const v0 = Math.min(roiDrag.v0, roiDrag.v1);
-    const u1 = Math.max(roiDrag.u0, roiDrag.u1);
-    const v1 = Math.max(roiDrag.v0, roiDrag.v1);
-    drawRect(ctx, u0, v0, u1, v1, '#9b6dff', null);
-  }
-
-  if (!lastPlan) return;
-  const p = lastPlan.plan;
-  const mx = p.marker_pixel[0], my = p.marker_pixel[1];
-  const ox = p.object_pixel[0], oy = p.object_pixel[1];
-  ctx.strokeStyle = 'rgba(255,255,255,0.6)';
-  ctx.setLineDash([8, 6]);
-  ctx.lineWidth = 2;
-  ctx.beginPath(); ctx.moveTo(mx, my); ctx.lineTo(ox, oy); ctx.stroke();
-  ctx.setLineDash([]);
-  drawCross(ctx, mx, my, '#4dd0e1');
-  drawCross(ctx, ox, oy, '#ffd54f');
-}
-
-async function planeSelectStart() {
-  roiMode = true;
-  setStatus('traccia un rettangolo sull\'immagine (shift+drag o dopo questo click, drag)', 'ok');
-}
-
-async function planeFit() {
-  if (!roi) { setStatus('seleziona prima un\'area', 'err'); return; }
-  try {
-    setStatus('fit piano…');
-    const body = {
-      u_min: roi.u0, v_min: roi.v0,
-      u_max: roi.u1, v_max: roi.v1,
-    };
-    const r = await api('POST', '/api/grasp/visual_servo/plane/fit', body);
-    planeInfo = r.plane;
-    updatePlaneInfo();
-    redraw3d();
-    setStatus(`piano OK · rms=${r.plane.fit_rms_mm.toFixed(1)} mm · ${r.plane.n_inliers} punti`, 'ok');
-  } catch (e) {
-    setStatus('fit fallito: ' + e.message, 'err');
-  }
-}
-
-async function planeClear() {
-  try {
-    await api('DELETE', '/api/grasp/visual_servo/plane');
-    planeInfo = null;
-    roi = null;
-    updatePlaneInfo();
-    redrawOverlay();
-    redraw3d();
-    setStatus('piano rimosso', 'ok');
-  } catch (e) {
-    setStatus('rimozione piano fallita: ' + e.message, 'err');
-  }
-}
-
-function updatePlaneInfo() {
-  const el = $('svPlaneInfo');
-  if (!el) return;
-  if (!planeInfo) { el.textContent = 'piano: —'; return; }
-  const n = planeInfo.normal.map(v => v.toFixed(3)).join(', ');
-  el.innerHTML = `piano: n=[${n}] · offset=${planeInfo.offset.toFixed(3)} m · rms=${planeInfo.fit_rms_mm.toFixed(1)} mm · ${planeInfo.n_inliers} pts`;
-}
-
-async function planeLoadCurrent() {
-  try {
-    const r = await api('GET', '/api/grasp/visual_servo/plane');
-    if (r.set) {
-      planeInfo = r.plane;
-      roi = { u0: r.plane.roi[0], v0: r.plane.roi[1], u1: r.plane.roi[2], v1: r.plane.roi[3] };
-      updatePlaneInfo();
-      redrawOverlay();
-      redraw3d();
-    }
-  } catch {}
-}
-
-function fmtMm(m) { return (m * 1000).toFixed(1) + ' mm'; }
-
-function renderStats(resp) {
-  const p = resp.plan;
-  const cls = p.detection && p.detection.class;
-  const conf = p.detection && p.detection.confidence;
-  const reproj = p.marker_reprojection_error_px;
-  const doneBadge = p.done
-    ? 'RAGGIUNTO'
-    : 'in avvicinamento';
-  $('servoStats').innerHTML = `
-    
-      
-        
-        
-        
-        
-        
-        
-        ${p.plane ? `
-          
-          
-          
-          
-          
-          
-        ` : ''}
-        
-        
-        
-        
-        
-        
-        
-        
-        
-        
-      
-    
stato${doneBadge}
marker · pixel(${p.marker_pixel[0].toFixed(0)}, ${p.marker_pixel[1].toFixed(0)})
marker · Z / dist / scala${fmtMm(p.marker_depth_z_m)} · ${fmtMm(p.marker_distance_cam_m)} · ${p.marker_mm_per_px ? p.marker_mm_per_px.toFixed(2) + ' mm/px' : '—'}
oggetto · pixel(${p.object_pixel[0].toFixed(0)}, ${p.object_pixel[1].toFixed(0)})
oggetto · Z / dist / scala${fmtMm(p.object_depth_z_m)} · ${fmtMm(p.object_distance_cam_m)} · ${p.object_mm_per_px ? p.object_mm_per_px.toFixed(2) + ' mm/px' : '—'}
marker ↔ oggetto (3D)${fmtMm(p.distance_m)}
piano · rms / punti${p.plane.fit_rms_mm.toFixed(1)} mm · ${planeInfo ? planeInfo.n_inliers : '—'}
marker · altezza dal piano${(p.plane.marker_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.marker_signed_distance_m))} (${p.plane.marker_signed_distance_m >= 0 ? 'sopra' : 'sotto'})
oggetto · altezza dal piano${(p.plane.object_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.object_signed_distance_m))} (${p.plane.object_signed_distance_m >= 0 ? 'sopra' : 'sotto'})
Δ altezza marker−oggetto${fmtMm(Math.abs(p.plane.marker_signed_distance_m - p.plane.object_signed_distance_m))}
in-piano (perp-foot)${fmtMm(p.plane.in_plane_distance_m)}
in-piano (pixel-hit)${fmtMm(p.plane.in_plane_pixel_distance_m)}
prossimo step (base)${p.target_position_base ? fmtMm(p.planned_step_m) : '—'}
limite stop${fmtMm(resp.min_distance_m)}
frazione step${resp.step_fraction}
lato${resp.side}
oggetto · classe${cls || '—'} ${conf != null ? `(${(conf*100).toFixed(0)}%)` : ''}
punti bbox${p.n_object_points}
reproj err marker${reproj != null ? reproj.toFixed(2) + ' px' : '—'}
sorgente posizione marker${p.marker_translation_source || '—'}
sorgente punto oggetto${p.object_point_source || '—'}
note${p.reason || '—'}
`;
-  $('servoResult').textContent = JSON.stringify({
-    wrist_position_base: p.wrist_position_base,
-    marker_position_cam: p.marker_position_cam,
-    object_position_cam: p.object_position_cam,
-    target_position_base: p.target_position_base,
-    distance_m: p.distance_m,
-  }, null, 2);
-}
-
-async function doPlan() {
-  if (busy) return;
-  busy = true;
-  persistParams();
-  try {
-    setStatus('rilevamento…');
-    refreshImg();
-    const body = readParams();
-    const resp = await api('POST', '/api/grasp/visual_servo/plan', body);
-    lastPlan = resp;
-    renderStats(resp);
-    redrawOverlay();
-    redraw3d();
-    setStatus(resp.plan.done ? 'target raggiunto' : 'piano pronto · premi "Vai al prossimo"', 'ok');
-  } catch (e) {
-    setStatus('errore plan: ' + e.message, 'err');
-  } finally { busy = false; }
-}
-
-async function doStep() {
-  if (busy) return;
-  busy = true;
-  persistParams();
-  const dryRun = $('svDryRun').checked;
-  try {
-    setStatus(dryRun ? 'step (dry-run)…' : 'movimento in corso…');
-    refreshImg();
-    const body = readParams();
-    body.dry_run = dryRun;
-    body.timeout_s = +$('svTimeout').value;
-    const resp = await api('POST', '/api/grasp/visual_servo/step', body);
-    lastPlan = resp;
-    renderStats(resp);
-    redrawOverlay();
-    if (!resp.moved) {
-      setStatus('target raggiunto, nessun moto', 'ok');
-      return;
-    }
-    setStatus('ricalcolo post-moto…');
-    refreshImg();
-    const replan = await api('POST', '/api/grasp/visual_servo/plan', readParams());
-    lastPlan = replan;
-    renderStats(replan);
-    redrawOverlay();
-    redraw3d();
-    setStatus(replan.plan.done ? 'target raggiunto' : 'pronto per il prossimo step', 'ok');
-  } catch (e) {
-    setStatus('errore step: ' + e.message, 'err');
-  } finally { busy = false; }
-}
-
-function doReset() {
-  lastPlan = null;
-  const ctx = $('servoOverlay').getContext('2d');
-  ctx.clearRect(0, 0, $('servoOverlay').width, $('servoOverlay').height);
-  $('servoStats').innerHTML = '
nessun piano calcolato
';
-  $('servoResult').textContent = '—';
-  setStatus('reset');
-}
-
-function startLiveImg() {
-  stopLiveImg();
-  imgRefreshTimer = setInterval(refreshImg, 500);
-  refreshImg();
-}
-function stopLiveImg() {
-  if (imgRefreshTimer) { clearInterval(imgRefreshTimer); imgRefreshTimer = null; }
-}
-
-document.addEventListener('DOMContentLoaded', () => {
-  if (!$('servoImg')) return;
-  restoreParams();
-  ['svClass','svConf','svMinDist','svStepFrac','svTimeout','svSide','svDryRun',
-   'svRx','svRy','svRz']
-    .forEach(id => { const el = $(id); if (el) el.addEventListener('change', persistParams); });
-  $('svPlanBtn').addEventListener('click', doPlan);
-  $('svStepBtn').addEventListener('click', doStep);
-  $('svResetBtn').addEventListener('click', doReset);
-  $('svSaveRvecBtn').addEventListener('click', saveRvecToConfig);
-  $('svPlaneSelBtn').addEventListener('click', planeSelectStart);
-  $('svPlaneFitBtn').addEventListener('click', planeFit);
-  $('svPlaneClearBtn').addEventListener('click', planeClear);
-  boot3d();
-  planeLoadCurrent();
-
-  const img = $('servoImg');
-  img.addEventListener('load', () => {
-    imgW = img.naturalWidth; imgH = img.naturalHeight;
-    resizeOverlay();
-  });
-  wireViewport();
-  window.addEventListener('resize', () => { resizeOverlay(); });
-});
-
-function resize3d() {
-  const canvas = $('servo3dCanvas');
-  if (!canvas || !renderer3d || !cam3d) return;
-  const rect = canvas.getBoundingClientRect();
-  const w = Math.max(100, Math.round(rect.width));
-  const h = Math.max(60, Math.round(rect.height));
-  renderer3d.setSize(w, h, false);
-  cam3d.aspect = w / h;
-  cam3d.updateProjectionMatrix();
-}
-
-// Only stream the RGB while the servo tab is active (avoid hammering Nuwa).
-document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
-  const active = btn.dataset.tab === 'servo';
-  if (active) {
-    startLiveImg();
-    // The 3D canvas may have rendered at 0x0 while the panel was display:none.
-    // Re-boot (no-op if already ready) and resize once visible.
-    boot3d();
-    setTimeout(() => { resizeOverlay(); resize3d(); redraw3d(); }, 100);
-  } else { stopLiveImg(); }
-}));
-window.addEventListener('resize', () => { resize3d(); });
diff --git a/web/work_plane.js b/web/work_plane.js
new file mode 100644
index 0000000..cda9f4e
--- /dev/null
+++ b/web/work_plane.js
@@ -0,0 +1,285 @@
+// Calibrazione Piano tab: shift+drag ROI on RGB, fit preview, offset
+// slider, save / clear, and a 3D scatter with the plane mesh.
+
+import * as THREE from 'three';
+import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
+
+const $ = (id) => document.getElementById(id);
+const RGB_REFRESH_MS = 1000;
+
+let roi = null;            // {u0,v0,u1,v1} in image pixel coords
+let previewPlane = null;   // last response.plane (preview, not saved)
+let previewPoints = null;  // list of [x,y,z] in CV camera frame, meters
+let savedPlane = null;     // last GET /api/work_plane response.plane
+
+function setChip(text, level) {
+  const el = $('wpStatusChip');
+  if (!el) return;
+  el.textContent = text;
+  el.className = 'post-health ' + (level || '');
+}
+
+function setRoiInfo() {
+  const el = $('wpRoiInfo');
+  if (!roi) { el.textContent = '— · disegna con shift+drag'; el.classList.add('dim'); return; }
+  el.classList.remove('dim');
+  el.textContent = `u=${roi.u0}…${roi.u1} v=${roi.v0}…${roi.v1} (${roi.u1 - roi.u0}×${roi.v1 - roi.v0} px)`;
+}
+
+function updateButtonStates() {
+  $('wpFitBtn').disabled = !roi;
+  $('wpSaveBtn').disabled = !previewPlane;
+  $('wpClearBtn').disabled = !savedPlane;
+}
+
+function rgbUrl() {
+  // Nuwa is reachable on host network on port 8877; the GUI is same-origin to
+  // presa-mano (8890). Use the upstream host directly.
+  const host = window.location.hostname;
+  return `http://${host}:8877/api/rgb?t=${Date.now()}`;
+}
+
+function startRgbLoop() {
+  const tick = () => { $('wpImg').src = rgbUrl(); };
+  tick();
+  setInterval(tick, RGB_REFRESH_MS);
+}
+
+// ---------------- ROI drawing on the RGB image ----------------
+(function setupRoiDrawing() {
+  const viewport = $('wpViewport');
+  const img = $('wpImg');
+  const overlay = $('wpOverlay');
+  let drawing = null;
+
+  function imgPixelFromEvent(e) {
+    const rect = img.getBoundingClientRect();
+    const sx = img.naturalWidth / rect.width;
+    const sy = img.naturalHeight / rect.height;
+    return {
+      u: Math.round((e.clientX - rect.left) * sx),
+      v: Math.round((e.clientY - rect.top) * sy),
+    };
+  }
+
+  function syncOverlaySize() {
+    overlay.width = img.naturalWidth || 640;
+    overlay.height = img.naturalHeight || 480;
+    overlay.style.width = img.clientWidth + 'px';
+    overlay.style.height = img.clientHeight + 'px';
+  }
+  img.addEventListener('load', syncOverlaySize);
+
+  function drawOverlay() {
+    const ctx = overlay.getContext('2d');
+    ctx.clearRect(0, 0, overlay.width, overlay.height);
+    if (roi) {
+      ctx.strokeStyle = '#9b6dff';
+      ctx.lineWidth = 2;
+      ctx.strokeRect(roi.u0, roi.v0, roi.u1 - roi.u0, roi.v1 - roi.v0);
+    }
+  }
+
+  viewport.addEventListener('mousedown', (e) => {
+    if (!e.shiftKey) return;   // shift+drag = ROI; plain drag = pan (no-op here)
+    const p = imgPixelFromEvent(e);
+    drawing = { u0: p.u, v0: p.v, u1: p.u, v1: p.v };
+    e.preventDefault();
+  });
+  viewport.addEventListener('mousemove', (e) => {
+    if (!drawing) return;
+    const p = imgPixelFromEvent(e);
+    drawing.u1 = p.u;
+    drawing.v1 = p.v;
+    roi = {
+      u0: Math.min(drawing.u0, drawing.u1),
+      v0: Math.min(drawing.v0, drawing.v1),
+      u1: Math.max(drawing.u0, drawing.u1),
+      v1: Math.max(drawing.v0, drawing.v1),
+    };
+    drawOverlay();
+    setRoiInfo();
+    updateButtonStates();
+  });
+  viewport.addEventListener('mouseup', () => { drawing = null; });
+  viewport.addEventListener('mouseleave', () => { drawing = null; });
+})();
+
+// ---------------- Three.js 3D viewer ----------------
+const three = (function setupThree() {
+  const canvas = $('wp3dCanvas');
+  const scene = new THREE.Scene();
+  const camera = new THREE.PerspectiveCamera(45, canvas.width / canvas.height, 0.01, 20);
+  camera.position.set(0.5, -0.5, -1.2);
+  camera.lookAt(0, 0, 1);
+  const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
+  renderer.setSize(canvas.width, canvas.height);
+  const controls = new OrbitControls(camera, canvas);
+  scene.add(new THREE.AxesHelper(0.1));
+  const planeMesh = new THREE.Mesh(
+    new THREE.PlaneGeometry(0.4, 0.4),
+    new THREE.MeshBasicMaterial({ color: 0x9b6dff, transparent: true, opacity: 0.3, side: THREE.DoubleSide }),
+  );
+  planeMesh.visible = false;
+  scene.add(planeMesh);
+  let scatter = null;
+
+  function render() { renderer.render(scene, camera); }
+  (function loop() { controls.update(); render(); requestAnimationFrame(loop); })();
+
+  function setPlane(plane) {
+    if (!plane) { planeMesh.visible = false; return; }
+    const c = plane.centroid;
+    const n = plane.normal;
+    planeMesh.position.set(c[0], c[1], c[2]);
+    // Align plane to its normal.
+    planeMesh.lookAt(c[0] + n[0], c[1] + n[1], c[2] + n[2]);
+    planeMesh.visible = true;
+  }
+
+  function setScatter(pointsCv, plane, offsetMm) {
+    if (scatter) { scene.remove(scatter); scatter.geometry.dispose(); scatter.material.dispose(); scatter = null; }
+    if (!pointsCv || pointsCv.length === 0) return;
+    const n = pointsCv.length;
+    const positions = new Float32Array(n * 3);
+    const colors = new Float32Array(n * 3);
+    const nx = plane ? plane.normal[0] : 0;
+    const ny = plane ? plane.normal[1] : 0;
+    const nz = plane ? plane.normal[2] : 0;
+    const d = plane ? plane.offset : 0;
+    const offsetM = (offsetMm || 0) / 1000.0;
+    const rmsM = plane ? (plane.fit_rms_mm / 1000.0) * 2.0 : 0.01;
+    for (let i = 0; i < n; i++) {
+      const x = pointsCv[i][0], y = pointsCv[i][1], z = pointsCv[i][2];
+      positions[3 * i] = x; positions[3 * i + 1] = y; positions[3 * i + 2] = z;
+      const signed = plane ? (nx * x + ny * y + nz * z + d) : 0;
+      let r = 0.5, g = 0.5, b = 0.5;
+      if (plane) {
+        if (signed > offsetM)       { r = 1.0; g = 0.83; b = 0.31; }   // yellow: above plane+offset
+        else if (signed < -offsetM) { r = 0.93; g = 0.27; b = 0.27; }  // red: below plane-offset
+        else if (Math.abs(signed) <= rmsM) { r = 0.30; g = 0.78; b = 0.40; }  // green: inlier
+        else                        { r = 0.55; g = 0.55; b = 0.55; }  // grey: buffer
+      }
+      colors[3 * i] = r; colors[3 * i + 1] = g; colors[3 * i + 2] = b;
+    }
+    const geom = new THREE.BufferGeometry();
+    geom.setAttribute('position', new THREE.BufferAttribute(positions, 3));
+    geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
+    const mat = new THREE.PointsMaterial({ size: 0.005, vertexColors: true });
+    scatter = new THREE.Points(geom, mat);
+    scene.add(scatter);
+  }
+
+  return { setPlane, setScatter };
+})();
+
+// ---------------- State + button handlers ----------------
+async function fetchSaved() {
+  try {
+    const r = await fetch('/api/work_plane').then((r) => r.json());
+    savedPlane = r.plane || null;
+  } catch (e) {
+    savedPlane = null;
+  }
+  if (savedPlane && !previewPlane) {
+    three.setPlane(savedPlane);
+    renderDetails(savedPlane);
+    $('wpStateBlock').innerHTML = `piano salvato · RMS ${savedPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${savedPlane.n_inliers} · offset ${savedPlane.offset_mm} mm`;
+    setChip('piano salvato', 'ok');
+  } else if (!savedPlane && !previewPlane) {
+    $('wpStateBlock').innerHTML = '
nessun piano
';
+    setChip('nessun piano');
+  }
+  updateButtonStates();
+}
+
+function renderDetails(plane) {
+  $('wpDetails').textContent = JSON.stringify(plane, null, 2);
+}
+
+async function doFit() {
+  if (!roi) return;
+  setChip('fit…');
+  $('wpFitBtn').disabled = true;
+  try {
+    const body = {
+      u_min: roi.u0, v_min: roi.v0, u_max: roi.u1, v_max: roi.v1,
+      offset_mm: parseFloat($('wpOffset').value || '5'),
+    };
+    const r = await fetch('/api/work_plane/fit', {
+      method: 'POST', headers: { 'Content-Type': 'application/json' },
+      body: JSON.stringify(body),
+    }).then((r) => r.json());
+    if (r.detail) throw new Error(r.detail);
+    previewPlane = r.plane;
+    previewPoints = r.points_camera;
+    three.setPlane(previewPlane);
+    three.setScatter(previewPoints, previewPlane, previewPlane.offset_mm);
+    renderDetails(previewPlane);
+    $('wpStateBlock').innerHTML = `preview · RMS ${previewPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${previewPlane.n_inliers} · offset ${previewPlane.offset_mm} mm · non salvato`;
+    setChip('preview pronto', 'ok');
+  } catch (e) {
+    setChip('fit failed: ' + e.message, 'err');
+  } finally {
+    updateButtonStates();
+  }
+}
+
+async function doSave() {
+  if (!previewPlane) return;
+  setChip('save…');
+  try {
+    const r = await fetch('/api/work_plane/save', {
+      method: 'POST', headers: { 'Content-Type': 'application/json' },
+      body: JSON.stringify({ plane: previewPlane }),
+    }).then((r) => r.json());
+    if (r.detail) throw new Error(r.detail);
+    savedPlane = r.plane;
+    previewPlane = null;
+    $('wpStateBlock').innerHTML = `piano salvato · RMS ${savedPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${savedPlane.n_inliers} · offset ${savedPlane.offset_mm} mm`;
+    renderDetails(savedPlane);
+    setChip('piano salvato', 'ok');
+  } catch (e) {
+    setChip('save failed: ' + e.message, 'err');
+  } finally {
+    updateButtonStates();
+  }
+}
+
+async function doClear() {
+  if (!confirm('Rimuovere il piano salvato?')) return;
+  setChip('clear…');
+  try {
+    await fetch('/api/work_plane', { method: 'DELETE' }).then((r) => r.json());
+    savedPlane = null;
+    previewPlane = null;
+    previewPoints = null;
+    three.setPlane(null);
+    three.setScatter(null);
+    $('wpStateBlock').innerHTML = '
nessun piano
';
+    $('wpDetails').textContent = '—';
+    setChip('nessun piano');
+  } catch (e) {
+    setChip('clear failed: ' + e.message, 'err');
+  } finally {
+    updateButtonStates();
+  }
+}
+
+// ---------------- Offset slider: re-color in real time ----------------
+$('wpOffset').addEventListener('input', () => {
+  if (previewPlane && previewPoints) {
+    const off = parseFloat($('wpOffset').value || '0');
+    previewPlane.offset_mm = off;
+    three.setScatter(previewPoints, previewPlane, off);
+    $('wpStateBlock').innerHTML = `preview · RMS ${previewPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${previewPlane.n_inliers} · offset ${previewPlane.offset_mm.toFixed(1)} mm · non salvato`;
+  }
+});
+
+$('wpFitBtn').addEventListener('click', doFit);
+$('wpSaveBtn').addEventListener('click', doSave);
+$('wpClearBtn').addEventListener('click', doClear);
+
+// Initial load.
+startRgbLoop();
+fetchSaved();