Merge feat/work-plane-filter: work-plane calibration + below-plane filter

8 commits — perception/work_plane module, filter wiring on 3 sites, /api/work_plane/* endpoints, visual-servo removal, Calibrazione Piano tab, GUI cleanup (Cerca modello + Precisione/Dettagli removed). - New: perception/work_plane.py owns WorkPlane (+offset_mm), fit_plane_svd, disk persistence, apply_below_plane_filter. 8 unit tests. - Filter applied at all three cloud sites (sam21_bridge, _points_via_bbox, _object_cloud_cv bbox branch). Responses carry plane_applied + n_points_before_plane on /scan, /object_points, /grasp/{plan,execute}. - Endpoints: POST /api/work_plane/fit (preview, no save), /save, GET, DELETE. Two-stage fit/save UX. - Visual-servo feature removed entirely (~1500 lines: endpoints, helpers, visual_servo.py, servo.js, tab GUI). - New tab "Calibrazione Piano": shift+drag ROI on RGB, fit preview, 3D viewer with colored scatter (green/yellow/red/grey by signed distance), offset slider re-colors client-side, save/clear. - GUI cleanup: "TrovaUtensile" renamed "Cerca modello"; removed "Precisione (ultime 5 detection)" stats panel and "Dettagli" JSON dump. - Verified live: baseline (no plane) → plane_applied:false everywhere; with saved plane → plane_applied:true on all 3 endpoints + JSON file on disk; DELETE → file gone + plane_applied:false. Zero ERROR logs.
2026-05-11 17:27:15 +02:00
commit 6f1649b384
@@ -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",
 ]
@@ -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:
+                     sw: Stopwatch) -> tuple[np.ndarray, dict]:
-    """Legacy path: bbox → Nuwa ROI cloud → filter outliers → base frame.
+    """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:
+    pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64)
-        raise HTTPException(status_code=422, detail=f"too few valid depth points in bbox: {len(cropped)}")
+    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}
@@ -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
@@ -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}
@@ -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,
    )
@@ -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)
@@ -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])
+        log.warning("SAM21 cloud has %d points after plane filter (<10), falling back to bbox",
-        raise Sam21Fallback(f"too few points: {pts_cv.shape[0]}")
+                    pts_cv.shape[0])
-    return pts_cv
+        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
@@ -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
@@ -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
@@ -13,8 +13,8 @@
    <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="grasp">TrovaUtensile</button>
+    <button class="tab" data-tab="workplane">Calibrazione Piano</button>
-    <button class="tab" data-tab="servo">Servo Visuale</button>
+    <button class="tab" data-tab="grasp">Cerca modello</button>
    <button class="tab" data-tab="view3d">Vista 3D</button>
    <button class="tab" data-tab="ee">Movimento EE</button>
  </nav>
@@ -174,6 +174,56 @@
  </div>
 </section>
 <!-- ============================== CALIBRAZIONE PIANO ============================== -->
 <section class="panel" id="tab-workplane">
  <div class="diag3d-toolbar">
    <span class="post-health" id="wpStatusChip">—</span>
  </div>
  <div class="grid-2 diag3d-layout">
    <div>
      <div id="wpViewport" style="position:relative;width:min(100%,720px);height:440px;overflow:hidden;background:#000;border:1px solid var(--border);border-radius:4px;cursor:grab;user-select:none">
        <div id="wpPanZoom" style="position:absolute;left:0;top:0;transform-origin:0 0;will-change:transform">
          <img id="wpImg" alt="Nuwa RGB" style="display:block;max-width:none;pointer-events:none" draggable="false" />
          <canvas id="wpOverlay" style="position:absolute;left:0;top:0;pointer-events:none"></canvas>
        </div>
      </div>
      <div class="marker-info">RGB live · shift+drag = ROI</div>
      <h3 style="margin-top:14px">Vista 3D (frame camera)</h3>
      <div class="canvas-wrap" style="width:min(100%,720px)">
        <canvas id="wp3dCanvas" width="720" height="360" style="display:block;background:#0a0d11;border:1px solid var(--border);border-radius:4px"></canvas>
      </div>
      <div class="marker-info">piano viola · verde = inlier · giallo = sopra piano+offset · rosso = sotto piano-offset · grigio = buffer · drag = ruota · wheel = zoom</div>
    </div>
    <div>
      <h2>Calibrazione</h2>
      <div class="ctrl-block">
        <div class="info-box">
          Disegna con shift+drag un rettangolo sul tavolo (senza oggetti), poi
          clicca <strong>Calcola piano</strong>. Regola l'offset finché i punti
          gialli rappresentano l'oggetto e i rossi spariscono. Poi <strong>Salva piano</strong>.
        </div>
        <div class="kv compact">
          <label>ROI <span id="wpRoiInfo" class="dim">— · disegna con shift+drag</span></label>
          <label>offset (mm) <input type="number" id="wpOffset" value="5.0" step="0.5" min="0" max="200" /></label>
        </div>
        <div class="actions">
          <button id="wpFitBtn" class="primary" disabled>Calcola piano (preview)</button>
          <button id="wpSaveBtn" disabled>Salva piano</button>
          <button id="wpClearBtn" class="danger" disabled>Rimuovi piano salvato</button>
        </div>
      </div>
      <h2>Stato</h2>
      <div id="wpStateBlock" class="ctrl-block">
        <div class="dim">nessun piano</div>
      </div>
      <h2>Dettagli piano</h2>
      <pre id="wpDetails" class="log small">—</pre>
    </div>
  </div>
 </section>
 <!-- ============================== TROVA UTENSILE ============================== -->
 <section class="panel" id="tab-grasp">
  <div class="grid-2">
@@ -215,103 +265,18 @@
        </div>
      </div>
-      <h3>Precisione (ultime <span id="statsWindow">5</span> detection)</h3>
+      <!-- Hidden DOM hooks consumed by app.js (Precisione + Dettagli UI removed). -->
-      <table class="stats" id="statsTable">
+      <div style="display:none">
-        <thead>
+        <span id="statsWindow">5</span>
-          <tr><th>asse</th><th>media [m]</th><th>σ [mm]</th><th>range [mm]</th><th>min</th><th>max</th></tr>
+        <table id="statsTable"><tbody>
        </thead>
        <tbody>
          <tr><td>x</td><td>—</td><td>—</td><td>—</td><td>—</td><td>—</td></tr>
          <tr><td>y</td><td>—</td><td>—</td><td>—</td><td>—</td><td>—</td></tr>
          <tr><td>z</td><td>—</td><td>—</td><td>—</td><td>—</td><td>—</td></tr>
-        </tbody>
+        </tbody></table>
-      </table>
+        <div id="statsMeta"></div>
-      <div id="statsMeta" class="marker-info">loop: inattivo</div>
+        <pre id="graspOut"></pre>
      <h3>Dettagli</h3>
      <pre id="graspOut" class="log">(nessuna detection)</pre>
      </div>
    </div>
 </section>
 <!-- ============================== SERVO VISUALE ============================== -->
 <section class="panel" id="tab-servo">
  <div class="diag3d-toolbar">
    <span class="post-health" id="servoStatus">—</span>
  </div>
  <div class="grid-2 diag3d-layout">
    <div>
      <div id="servoViewport" style="position:relative;width:min(100%,720px);height:440px;overflow:hidden;background:#000;border:1px solid var(--border);border-radius:4px;cursor:grab;user-select:none">
        <div id="servoPanZoom" style="position:absolute;left:0;top:0;transform-origin:0 0;will-change:transform">
          <img id="servoImg" alt="Nuwa RGB" style="display:block;max-width:none;pointer-events:none" draggable="false" />
          <canvas id="servoOverlay" style="position:absolute;left:0;top:0;pointer-events:none"></canvas>
        </div>
      </div>
      <div class="marker-info">RGB live · ciano = marker · giallo = oggetto · wheel zoom · drag pan · doppio click reset</div>
      <h3 style="margin-top:14px">Vista 3D (frame camera)</h3>
      <div class="canvas-wrap" style="width:min(100%,720px)">
        <canvas id="servo3dCanvas" width="720" height="360" style="display:block;background:#0a0d11;border:1px solid var(--border);border-radius:4px"></canvas>
      </div>
      <div class="marker-info">origine = camera · piano viola · marker ciano · oggetto giallo · proiezioni più chiare · drag ruota · wheel zoom</div>
      <div class="diag3d-legend">
        <div><span class="legend-sw" style="background:#4dd0e1;border-radius:50%"></span> marker polso</div>
        <div><span class="legend-sw" style="background:#ffd54f;border-radius:50%"></span> oggetto (centro bbox)</div>
      </div>
    </div>
    <div>
      <h2>Parametri</h2>
      <div class="ctrl-block">
        <div class="info-box">
          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 <code>min_distance_m</code>.
        </div>
        <div class="kv compact">
          <label>Classe (filtro) <input type="text" id="svClass" placeholder="(tutte)" /></label>
          <label>Confidenza <input type="number" id="svConf" value="0.5" step="0.05" min="0" max="1" /></label>
          <label>min dist (mm) <input type="number" id="svMinDist" value="100" step="10" min="20" max="500" /></label>
          <label>frazione step <input type="number" id="svStepFrac" value="0.5" step="0.05" min="0.05" max="1.0" /></label>
          <label>timeout (s) <input type="number" id="svTimeout" value="5" step="0.5" min="1" max="30" /></label>
          <label>side
            <select id="svSide">
              <option value="">(config)</option>
              <option value="right">right</option>
              <option value="left">left</option>
            </select>
          </label>
          <label class="inline"><input type="checkbox" id="svDryRun" checked /> dry-run (nessun moto reale)</label>
        </div>
        <div class="info-box" style="margin-top:8px">
          <strong>Orientazione marker rispetto al polso</strong> (rvec in gradi). Se non allineato al frame polso il target risulta spostato: tuna finché marker, oggetto e target sono coerenti.
        </div>
        <div class="kv compact">
          <label>rx (°) <input type="number" id="svRx" value="0" step="5" min="-180" max="180" /></label>
          <label>ry (°) <input type="number" id="svRy" value="0" step="5" min="-180" max="180" /></label>
          <label>rz (°) <input type="number" id="svRz" value="0" step="5" min="-180" max="180" /></label>
          <button id="svSaveRvecBtn" title="Salva l'rvec corrente in config.yaml">Salva rvec in config</button>
        </div>
        <div class="actions">
          <button id="svPlanBtn" class="primary">Rileva / ricalcola</button>
          <button id="svStepBtn">▶ Vai al prossimo</button>
          <button id="svResetBtn">Reset</button>
        </div>
        <div class="info-box" style="margin-top:8px">
          <strong>Piano di lavoro</strong>. 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.
        </div>
        <div class="actions">
          <button id="svPlaneSelBtn">Seleziona area</button>
          <button id="svPlaneFitBtn">Calcola piano</button>
          <button id="svPlaneClearBtn" class="danger">Rimuovi piano</button>
        </div>
        <div id="svPlaneInfo" class="marker-info">piano: —</div>
      </div>
      <h2>Stato</h2>
      <div id="servoStats" class="ctrl-block">
        <div class="dim">nessun piano calcolato</div>
      </div>
      <h2>Dettagli piano</h2>
      <pre id="servoResult" class="log small">—</pre>
    </div>
  </div>
 </section>
@@ -419,6 +384,6 @@
 }
 </script>
 <script src="/static/app.js"></script>
-<script type="module" src="/static/servo.js"></script>
+<script type="module" src="/static/work_plane.js"></script>
 </body>
 </html>
@@ -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
    ? '<span style="color:var(--ok);font-weight:600">RAGGIUNTO</span>'
    : '<span>in avvicinamento</span>';
  $('servoStats').innerHTML = `
    <table class="kvtable">
      <tbody>
        <tr><td>stato</td><td>${doneBadge}</td></tr>
        <tr><td>marker · pixel</td><td>(${p.marker_pixel[0].toFixed(0)}, ${p.marker_pixel[1].toFixed(0)})</td></tr>
        <tr><td>marker · Z / dist / scala</td><td>${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' : '—'}</td></tr>
        <tr><td>oggetto · pixel</td><td>(${p.object_pixel[0].toFixed(0)}, ${p.object_pixel[1].toFixed(0)})</td></tr>
        <tr><td>oggetto · Z / dist / scala</td><td>${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' : '—'}</td></tr>
        <tr><td>marker ↔ oggetto (3D)</td><td><strong>${fmtMm(p.distance_m)}</strong></td></tr>
        ${p.plane ? `
          <tr><td>piano · rms / punti</td><td>${p.plane.fit_rms_mm.toFixed(1)} mm · ${planeInfo ? planeInfo.n_inliers : '—'}</td></tr>
          <tr><td>marker · altezza dal piano</td><td><strong>${(p.plane.marker_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.marker_signed_distance_m))}</strong> <span class="dim">(${p.plane.marker_signed_distance_m >= 0 ? 'sopra' : 'sotto'})</span></td></tr>
          <tr><td>oggetto · altezza dal piano</td><td><strong>${(p.plane.object_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.object_signed_distance_m))}</strong> <span class="dim">(${p.plane.object_signed_distance_m >= 0 ? 'sopra' : 'sotto'})</span></td></tr>
          <tr><td>Δ altezza marker−oggetto</td><td>${fmtMm(Math.abs(p.plane.marker_signed_distance_m - p.plane.object_signed_distance_m))}</td></tr>
          <tr><td>in-piano (perp-foot)</td><td><strong>${fmtMm(p.plane.in_plane_distance_m)}</strong></td></tr>
          <tr><td>in-piano (pixel-hit)</td><td>${fmtMm(p.plane.in_plane_pixel_distance_m)}</td></tr>
        ` : ''}
        <tr><td>prossimo step (base)</td><td>${p.target_position_base ? fmtMm(p.planned_step_m) : '—'}</td></tr>
        <tr><td>limite stop</td><td>${fmtMm(resp.min_distance_m)}</td></tr>
        <tr><td>frazione step</td><td>${resp.step_fraction}</td></tr>
        <tr><td>lato</td><td>${resp.side}</td></tr>
        <tr><td>oggetto · classe</td><td>${cls || '—'} ${conf != null ? `(${(conf*100).toFixed(0)}%)` : ''}</td></tr>
        <tr><td>punti bbox</td><td>${p.n_object_points}</td></tr>
        <tr><td>reproj err marker</td><td>${reproj != null ? reproj.toFixed(2) + ' px' : '—'}</td></tr>
        <tr><td>sorgente posizione marker</td><td>${p.marker_translation_source || '—'}</td></tr>
        <tr><td>sorgente punto oggetto</td><td>${p.object_point_source || '—'}</td></tr>
        <tr><td>note</td><td>${p.reason || '—'}</td></tr>
      </tbody>
    </table>`;
  $('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 = '<div class="dim">nessun piano calcolato</div>';
  $('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(); });
@@ -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 = '<div class="dim">nessun piano</div>';
    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 = `<strong>preview</strong> · RMS ${previewPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${previewPlane.n_inliers} · offset ${previewPlane.offset_mm} mm · <span class="warn">non salvato</span>`;
    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 = '<div class="dim">nessun piano</div>';
    $('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 = `<strong>preview</strong> · RMS ${previewPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${previewPlane.n_inliers} · offset ${previewPlane.offset_mm.toFixed(1)} mm · <span class="warn">non salvato</span>`;
  }
 });
 $('wpFitBtn').addEventListener('click', doFit);
 $('wpSaveBtn').addEventListener('click', doSave);
 $('wpClearBtn').addEventListener('click', doClear);
 // Initial load.
 startRgbLoop();
 fetchSaved();