diff --git a/src/presa_mano/api/grasp_routes.py b/src/presa_mano/api/grasp_routes.py index 103c4e3..2cb541c 100644 --- a/src/presa_mano/api/grasp_routes.py +++ b/src/presa_mano/api/grasp_routes.py @@ -5,32 +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 ( - WorkPlane as PerceptionWorkPlane, - apply_below_plane_filter, - get_work_plane, -) -from ..perception.work_plane import set_work_plane as _set_perception_work_plane +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 @@ -187,184 +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) - # Keep perception.work_plane singleton in sync until Task 4 removes - # the visual-servo duplicate. Default offset_mm so the filter has a - # reasonable threshold without forcing the user through the new endpoint. - _set_perception_work_plane(PerceptionWorkPlane( - normal=wp.normal, offset=wp.offset, centroid=wp.centroid, - n_inliers=wp.n_inliers, fit_rms_mm=wp.fit_rms_mm, - roi=wp.roi, offset_mm=5.0, created_at=wp.created_at, - )) - 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) - _set_perception_work_plane(None) - return {"ok": True, "set": False} diff --git a/src/presa_mano/grasp/visual_servo.py b/src/presa_mano/grasp/visual_servo.py deleted file mode 100644 index 0a376f0..0000000 --- a/src/presa_mano/grasp/visual_servo.py +++ /dev/null @@ -1,504 +0,0 @@ -"""Marker-relative visual servoing (no static hand-eye calibration). - -Idea: the wrist-mounted ArUco marker is our live reference. Each step we read -current wrist pose from G1, detect the marker in the camera frame, detect the -object (YOLO bbox → Nuwa ROI point cloud → centroid), and plan a wrist move -that advances a fraction of the camera-frame marker→object vector. - -Mapping a camera-frame delta to a base-frame wrist delta needs `R_base_camera`, -which is derived live each step: - - R_base_camera = R_base_wrist @ R_wrist_marker @ R_cam_marker^-1 - -Only the *rotation* part of `T_wrist_marker` matters for deltas — translation -cancels. We take `R_wrist_marker` from config (rvec), defaulting to identity -when the marker is roughly aligned with the wrist frame. - -Stop condition: camera-frame marker→object distance ≤ `min_distance_m`. This -is a direct physical proxy for wrist-to-object proximity, independent of the -(unknown) marker-to-wrist translation offset. -""" - -from __future__ import annotations - -import json -import logging -import time -from dataclasses import asdict, dataclass -from pathlib import Path -from typing import Any - -import numpy as np - -from ..clients import G1Client, MarkerClient, NuwaClient, YoloClient -from ..config import data_dir -from ..geometry import ( - T_from_pos_quat, - invert_T, - make_T, - quat_xyzw_to_R, - rvec_to_R, -) -from ..perception import filter_outliers, nuwa_to_cv_frame -from ..perception.aruco_pose import nuwa_intrinsics_to_matrix, solve_square_marker -from ..perception.detector import ObjectDetector - -log = logging.getLogger(__name__) - - -class ServoError(RuntimeError): - pass - - -@dataclass -class WorkPlane: - """Plane in camera frame: normal·X + offset = 0, normal is unit-length and - pointing roughly toward the camera (i.e. has negative Z in OpenCV frame).""" - normal: list[float] - offset: float - centroid: list[float] - n_inliers: int - fit_rms_mm: float - roi: list[int] - created_at: float - - -_WORK_PLANE: WorkPlane | None = None -_WORK_PLANE_LOADED = False - - -def _work_plane_path() -> Path: - return data_dir() / "work_plane.json" - - -def _save_work_plane_to_disk(wp: WorkPlane | None) -> None: - path = _work_plane_path() - try: - if wp is None: - if path.exists(): - path.unlink() - return - path.parent.mkdir(parents=True, exist_ok=True) - with open(path, "w") as f: - json.dump(asdict(wp), f, indent=2) - except Exception as e: - log.warning("failed to persist work plane: %s", e) - - -def _load_work_plane_from_disk() -> WorkPlane | None: - path = _work_plane_path() - try: - with open(path, "r") as f: - d = json.load(f) - return WorkPlane(**d) - except FileNotFoundError: - return None - except Exception as e: - log.warning("failed to load persisted work plane (%s)", e) - return None - - -def get_work_plane() -> WorkPlane | None: - global _WORK_PLANE, _WORK_PLANE_LOADED - if not _WORK_PLANE_LOADED: - _WORK_PLANE = _load_work_plane_from_disk() - _WORK_PLANE_LOADED = True - return _WORK_PLANE - - -def set_work_plane(wp: WorkPlane | None) -> None: - global _WORK_PLANE, _WORK_PLANE_LOADED - _WORK_PLANE = wp - _WORK_PLANE_LOADED = True - _save_work_plane_to_disk(wp) - - -def fit_plane_svd(points_xyz: np.ndarray - ) -> tuple[np.ndarray, float, np.ndarray, float]: - """Least-squares plane via SVD. Returns (normal, offset, centroid, rms_mm). - - Plane equation: normal·X + offset = 0. Normal is oriented to point toward - the camera origin (negative Z in CV frame).""" - pts = np.asarray(points_xyz, dtype=np.float64) - if pts.ndim != 2 or pts.shape[1] < 3: - raise ServoError(f"unexpected points shape for plane fit: {pts.shape}") - xyz = pts[:, :3] - if len(xyz) < 10: - raise ServoError(f"too few points to fit plane: {len(xyz)}") - c = xyz.mean(axis=0) - centered = xyz - c - _, _, Vt = np.linalg.svd(centered, full_matrices=False) - normal = Vt[-1] - n = float(np.linalg.norm(normal)) - if n < 1e-9: - raise ServoError("degenerate plane (zero normal)") - normal = normal / n - # Orient normal to point toward the camera origin: the signed distance of - # the origin from the plane must be positive, i.e. d = -n·c > 0 → n·c < 0. - # Flipping only on normal[2] is wrong for tilted planes (the camera may - # not be above the plane in Z-only sense). - if float(normal @ c) > 0: - normal = -normal - offset = -float(normal @ c) - signed = centered @ normal - rms_mm = float(np.sqrt(np.mean(signed * signed)) * 1000.0) - return normal, offset, c, rms_mm - - -def project_point_on_plane(P: np.ndarray, normal: np.ndarray, offset: float - ) -> tuple[np.ndarray, float]: - """Perpendicular projection of a 3D point onto the plane. - Returns (foot, signed_distance). Signed distance > 0 means P is on the - side of the plane the normal points toward (camera side in our convention). - """ - s = float(normal @ P + offset) - return P - s * normal, s - - -def backproject_pixel_on_plane(uv: np.ndarray, K: np.ndarray, - normal: np.ndarray, offset: float - ) -> np.ndarray: - """Intersect the viewing ray through pixel (u,v) with the work plane.""" - r = np.linalg.inv(K) @ np.array([float(uv[0]), float(uv[1]), 1.0]) - denom = float(normal @ r) - if abs(denom) < 1e-9: - raise ServoError("ray parallel to work plane") - t = -offset / denom - if t <= 0: - raise ServoError("plane intersection is behind the camera") - return t * r - - -@dataclass -class ServoPlan: - # Truth (from G1) - wrist_position_base: list[float] - wrist_quaternion_base: list[float] - - # Camera-frame observations - marker_position_cam: list[float] - object_position_cam: list[float] - - # Pixel-space positions for the 2D overlay (u, v in image coordinates). - marker_pixel: list[float] - object_pixel: list[float] - - # Perpendicular depth to the camera plane (Z component, m). This is what - # drives the pinhole mm/px scale — NOT the slant range. - marker_depth_z_m: float - object_depth_z_m: float - # Euclidean distance camera origin → point (slant range, m). - marker_distance_cam_m: float - object_distance_cam_m: float - - # Pinhole scale at the point's depth: mm-per-pixel = (z_mm / fx), i.e. - # physical size of one image pixel at that depth. Depends on Z only, so - # two points at the same Z but different XY will share mm/px. - marker_mm_per_px: float - object_mm_per_px: float - - # Approximate base-frame positions for visualisation only. Computed with - # T_wrist_marker = identity: relative distances are correct, absolute - # positions are offset by the unknown marker-to-wrist translation. - marker_position_base_est: list[float] - object_position_base_est: list[float] - T_base_camera_est: list[list[float]] - - # Scalar cues - distance_m: float # camera-frame magnitude of marker→object - done: bool # True when distance ≤ min_distance_m - - # Next wrist target in base frame (None when done) - target_position_base: list[float] | None - target_quaternion_base: list[float] | None - planned_step_m: float # how much the wrist is about to move (base frame) - - # Work-plane projection block — present only when a plane has been fitted. - # Everything camera-frame. `in_plane_distance_m` is the Euclidean distance - # between the two foot-of-perpendicular points (lives on the plane). - plane: dict[str, Any] | None = None - - # Diagnostics - detection: dict[str, Any] | None = None - n_object_points: int = 0 - marker_reprojection_error_px: float | None = None - marker_translation_source: str = "solvepnp" # "nuwa_depth" | "solvepnp" - object_point_source: str = "centroid" # "centroid" | "highest_above_plane(...)" - reason: str | None = None - - -def _resolve_wrist_pose(g1: G1Client, side: str) -> tuple[np.ndarray, np.ndarray, list[float]]: - w = g1.wrist() - body = w.get("data") if isinstance(w.get("data"), dict) else w - pose = body.get(f"{side}_wrist") or body.get(side) - if pose is None or "position" not in pose or "quaternion" not in pose: - raise ServoError(f"G1 /wrist missing '{side}' pose: {w!r}") - pos = np.asarray(pose["position"], dtype=np.float64) - quat = np.asarray(pose["quaternion"], dtype=np.float64) - return pos, quat, list(map(float, pose["quaternion"])) - - -def _detect_marker_cam(nuwa: NuwaClient, marker: MarkerClient, - marker_id: int, size_mm: float - ) -> tuple[np.ndarray, np.ndarray, np.ndarray, float | None, str]: - """Return (R_cam_marker, t_cam_marker, corners_px, reprojection_error_px, source). - - Translation comes from the Nuwa depth at the marker centre pixel whenever - possible — much more stable than solvePnP on a small planar marker, which - suffers from the IPPE_SQUARE planar ambiguity (two solutions, same - reprojection error) at ~1.4 m distance. Rotation still comes from solvePnP - because the servo mapping cam→base needs it. - - `source` reports how `t_cam_marker` was obtained: "nuwa_depth" when the - depth-sensor value at the marker pixel was usable, "solvepnp" otherwise. - """ - result = marker.detect_url( - markers=[{"id": int(marker_id), "size_mm": float(size_mm)}], - image_url=nuwa.rgb_url(), - ) - found = result.get("found", []) - entry = next((m for m in found if int(m["id"]) == int(marker_id)), None) - if entry is None: - raise ServoError(f"marker id={marker_id} not detected (not_found={result.get('not_found')})") - corners = np.asarray(entry["corners"], dtype=np.float64) - - R_cm = None - t_pnp = None - reproj_err: float | None = None - try: - intr = nuwa.intrinsics() - K, dist = nuwa_intrinsics_to_matrix(intr) - sol = solve_square_marker(corners, float(size_mm) / 1000.0, K, dist) - R_cm = sol["R"] - t_pnp = sol["t"].flatten() - reproj_err = float(sol["reprojection_error_px"]) - except Exception as e: - log.warning("local pnp failed (%s) — using marker-finder orientation", e) - R_cm = np.asarray(entry["rotation_matrix"], dtype=np.float64).reshape(3, 3) - t_pnp = np.asarray(entry["position_m"], dtype=np.float64).flatten() - - # Prefer Nuwa depth at the marker centre pixel for the translation. - centre = corners.mean(axis=0) - u = int(round(float(centre[0]))) - v = int(round(float(centre[1]))) - t_depth = None - try: - m = nuwa.measure(u, v, radius=8) - pt = m.get("point_m") if isinstance(m, dict) else None - if not isinstance(pt, dict): - pt = m - x = float(pt.get("x", pt.get("X"))) - y = float(pt.get("y", pt.get("Y"))) - z = float(pt.get("z", pt.get("Z"))) - # /api/measure returns already-OpenCV-frame coords (Z>0 in front of - # the camera). No flip. - t_depth = np.array([x, y, z], dtype=np.float64) - if not np.all(np.isfinite(t_depth)) or abs(t_depth[2]) < 1e-3: - t_depth = None - except Exception as e: - log.warning("nuwa.measure failed at (%d,%d): %s", u, v, e) - t_depth = None - - if t_depth is not None: - return R_cm, t_depth, corners, reproj_err, "nuwa_depth" - return R_cm, t_pnp, corners, reproj_err, "solvepnp" - - -def _detect_object_point_cam(nuwa: NuwaClient, yolo: YoloClient, - conf: float | None, class_name: str | None, - pointcloud_step: int, pointcloud_max_depth_mm: int, - work_plane: WorkPlane | None, - top_fraction: float = 0.05, - ) -> tuple[np.ndarray, dict[str, Any], int, str]: - """Return (point_cam_xyz, top_detection, n_points, source). - - When a work plane is available, returns the mean of the top `top_fraction` - of points (by signed distance from the plane) — this is the highest part - of the object above the work surface, which is the natural grasp target. - Without a plane, falls back to the centroid of all filtered points.""" - detector = ObjectDetector(yolo, nuwa) - dets = detector.detect(conf=conf) - if class_name: - dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()] - if not dets: - raise ServoError("no object detected" + (f" for class '{class_name}'" if class_name else "")) - dets.sort(key=lambda d: d["confidence"], reverse=True) - top = dets[0] - b = top["bbox"] - u_min = max(0, int(np.floor(b["x1"]))) - v_min = max(0, int(np.floor(b["y1"]))) - u_max = int(np.ceil(b["x2"])) - v_max = int(np.ceil(b["y2"])) - pc, _meta = nuwa.pointcloud_roi(u_min, v_min, u_max, v_max, - step=pointcloud_step, - max_depth_mm=pointcloud_max_depth_mm) - # Empirical: Nuwa ROI points come in Nuwa-native frame (Z<0 in front of - # camera) despite the docstring. Flip to true OpenCV frame so comparisons - # with the plane (also CV) are consistent. - cropped = filter_outliers(pc) - if len(cropped) < 10: - raise ServoError(f"too few valid depth points in bbox: {len(cropped)}") - pts_cv = nuwa_to_cv_frame(cropped[:, :3]) - if work_plane is None: - return pts_cv.mean(axis=0), top, int(len(pts_cv)), "centroid" - n_vec = np.asarray(work_plane.normal, dtype=np.float64) - d_off = float(work_plane.offset) - signed = pts_cv @ n_vec + d_off # Nx (positive = above plane) - k = max(1, int(round(float(top_fraction) * len(pts_cv)))) - idx = np.argpartition(signed, -k)[-k:] - highest = pts_cv[idx].mean(axis=0) - return highest, top, int(len(pts_cv)), f"highest_above_plane(top {k}/{len(pts_cv)})" - - -def _r_wrist_marker_from_config(cfg: dict[str, Any]) -> np.ndarray: - """Rotation part of the marker-on-wrist rigid offset. Translation does not - matter for camera-frame delta servo. Read from - `grasp.visual_servo.wrist_marker_rvec` as a 3-vector (Rodrigues), default - identity.""" - vs = cfg.get("grasp", {}).get("visual_servo", {}) or {} - rvec = vs.get("wrist_marker_rvec") or [0.0, 0.0, 0.0] - return rvec_to_R(np.asarray(rvec, dtype=np.float64)) - - -def plan(*, side: str, marker_id: int, size_mm: float, - conf: float | None, class_name: str | None, - min_distance_m: float, step_fraction: float, - pointcloud_step: int, pointcloud_max_depth_mm: int, - cfg: dict[str, Any], - nuwa: NuwaClient, marker: MarkerClient, yolo: YoloClient, g1: G1Client, - ) -> ServoPlan: - # 1) Wrist pose in base - wrist_pos, wrist_quat, wrist_quat_list = _resolve_wrist_pose(g1, side) - R_bw = quat_xyzw_to_R(wrist_quat) - - # 2) Marker pose in camera - R_cm, t_cm, corners_px, reproj_err, marker_t_source = _detect_marker_cam( - nuwa, marker, marker_id, size_mm, - ) - marker_pixel = corners_px.mean(axis=0) # centre of the 4 corners - # Intrinsics for mm/px scale at each point's depth. - try: - intr = nuwa.intrinsics() - K, _ = nuwa_intrinsics_to_matrix(intr) - fx = float(K[0, 0]) - except Exception as e: - log.warning("intrinsics unavailable for mm/px (%s)", e) - fx = float("nan") - - # 3) Object reference point in camera: highest-above-plane if a plane is - # set, otherwise centroid of the bbox pointcloud. - wp_here = get_work_plane() - centroid_cam, detection, n_pts, object_point_source = _detect_object_point_cam( - nuwa, yolo, conf, class_name, - pointcloud_step, pointcloud_max_depth_mm, - work_plane=wp_here, - ) - b = detection["bbox"] - object_pixel = np.array([0.5 * (b["x1"] + b["x2"]), 0.5 * (b["y1"] + b["y2"])]) - - # 4) Rotation mapping cam→base (identity marker-to-wrist rotation by default) - R_wm = _r_wrist_marker_from_config(cfg) - R_base_cam = R_bw @ R_wm @ np.linalg.inv(R_cm) - - # 5) Camera-frame vector & distance (marker → object) - d_cam = centroid_cam - t_cm - distance_m = float(np.linalg.norm(d_cam)) - done = distance_m <= float(min_distance_m) - - # 6) Estimate T_base_camera for visualisation (identity T_wrist_marker): - # translation part is off by the real marker-to-wrist translation, but - # relative positions of marker and object in base frame are preserved. - T_bc_est = np.eye(4) - T_bc_est[:3, :3] = R_base_cam - T_bw_full = T_from_pos_quat(wrist_pos, wrist_quat) - T_wm_identity = np.eye(4) # translation part left at zero - T_wm_identity[:3, :3] = R_wm - T_cm_full = make_T(R_cm, t_cm) - T_bc_est = T_bw_full @ T_wm_identity @ invert_T(T_cm_full) - - marker_base_est = (T_bc_est @ np.array([*t_cm, 1.0]))[:3] - object_base_est = (T_bc_est @ np.array([*centroid_cam, 1.0]))[:3] - - # 7) Plan step - target_pos = None - target_quat = None - planned_step_m = 0.0 - reason = None - if done: - reason = f"distance {distance_m*1000:.1f} mm ≤ {min_distance_m*1000:.0f} mm — reached" - else: - step_cam = d_cam * float(step_fraction) - step_len = float(np.linalg.norm(step_cam)) - # Don't overshoot below min_distance_m - if distance_m - step_len < float(min_distance_m): - step_cam = d_cam * ((distance_m - float(min_distance_m)) / distance_m) - step_len = float(np.linalg.norm(step_cam)) - reason = "clamped to stop at min_distance_m" - step_base = R_base_cam @ step_cam - target_pos = wrist_pos + step_base - target_quat = wrist_quat_list - planned_step_m = float(np.linalg.norm(step_base)) - - # 8) Work-plane projection (optional) - plane_block = None - wp = get_work_plane() - if wp is not None: - try: - K_plane, _ = nuwa_intrinsics_to_matrix(nuwa.intrinsics()) - n_vec = np.asarray(wp.normal, dtype=np.float64) - d_off = float(wp.offset) - # Perpendicular foot of the measured 3D points. - m_foot, m_signed = project_point_on_plane(np.asarray(t_cm), n_vec, d_off) - o_foot, o_signed = project_point_on_plane(np.asarray(centroid_cam), n_vec, d_off) - # Back-projected pixel hits on the plane (trust pixel + depth plane, - # ignoring the measured Z of each point — more stable when depth is - # noisy, at the cost of assuming the point actually lies on the - # plane). - m_hit = backproject_pixel_on_plane(np.asarray(marker_pixel), K_plane, n_vec, d_off) - o_hit = backproject_pixel_on_plane(np.asarray(object_pixel), K_plane, n_vec, d_off) - plane_block = { - "normal": wp.normal, - "offset": wp.offset, - "fit_rms_mm": wp.fit_rms_mm, - "roi": wp.roi, - "marker_foot_cam": [float(x) for x in m_foot], - "object_foot_cam": [float(x) for x in o_foot], - "marker_signed_distance_m": float(m_signed), - "object_signed_distance_m": float(o_signed), - "marker_pixel_hit_cam": [float(x) for x in m_hit], - "object_pixel_hit_cam": [float(x) for x in o_hit], - "in_plane_distance_m": float(np.linalg.norm(o_foot - m_foot)), - "in_plane_pixel_distance_m": float(np.linalg.norm(o_hit - m_hit)), - } - except Exception as e: - log.warning("plane projection failed: %s", e) - - return ServoPlan( - wrist_position_base=[float(x) for x in wrist_pos], - wrist_quaternion_base=wrist_quat_list, - marker_position_cam=[float(x) for x in t_cm], - object_position_cam=[float(x) for x in centroid_cam], - marker_pixel=[float(x) for x in marker_pixel], - object_pixel=[float(x) for x in object_pixel], - marker_depth_z_m=float(abs(t_cm[2])), - object_depth_z_m=float(abs(centroid_cam[2])), - marker_distance_cam_m=float(np.linalg.norm(t_cm)), - object_distance_cam_m=float(np.linalg.norm(centroid_cam)), - marker_mm_per_px=float(abs(t_cm[2]) * 1000.0 / fx) if fx == fx and fx > 0 else 0.0, - object_mm_per_px=float(abs(centroid_cam[2]) * 1000.0 / fx) if fx == fx and fx > 0 else 0.0, - marker_position_base_est=[float(x) for x in marker_base_est], - object_position_base_est=[float(x) for x in object_base_est], - T_base_camera_est=T_bc_est.tolist(), - distance_m=distance_m, - done=done, - target_position_base=[float(x) for x in target_pos] if target_pos is not None else None, - target_quaternion_base=target_quat, - planned_step_m=planned_step_m, - plane=plane_block, - detection=detection, - n_object_points=n_pts, - marker_reprojection_error_px=reproj_err, - marker_translation_source=marker_t_source, - object_point_source=object_point_source, - reason=reason, - ) diff --git a/src/presa_mano/main.py b/src/presa_mano/main.py index 3fcbd97..6be1893 100644 --- a/src/presa_mano/main.py +++ b/src/presa_mano/main.py @@ -103,7 +103,7 @@ def index() -> HTMLResponse: idx = WEB_DIR / "index.html" html = idx.read_text(encoding="utf-8") # Cache-bust app.js and style.css with their mtime so a rebuild forces refetch. - for name in ("app.js", "servo.js", "style.css"): + for name in ("app.js", "work_plane.js", "style.css"): f = WEB_DIR / name if f.exists(): v = int(f.stat().st_mtime) diff --git a/web/index.html b/web/index.html index 20a6fb1..1ee5cb1 100644 --- a/web/index.html +++ b/web/index.html @@ -14,7 +14,6 @@ - @@ -234,87 +233,6 @@ - -
-
- — -
-
-
-
-
- Nuwa RGB - -
-
-
RGB live · ciano = marker · giallo = oggetto · wheel zoom · drag pan · doppio click reset
-

Vista 3D (frame camera)

-
- -
-
origine = camera · piano viola · marker ciano · oggetto giallo · proiezioni più chiare · drag ruota · wheel zoom
-
-
marker polso
-
oggetto (centro bbox)
-
-
-
-

Parametri

-
-
- Avvicinamento marker-relativo senza hand-eye calibration: la camera vede marker e oggetto nello stesso frame, ogni step sposta il polso della frazione indicata della distanza rimanente. Si ferma a min_distance_m. -
-
- - - - - - - -
-
- Orientazione marker rispetto al polso (rvec in gradi). Se non allineato al frame polso il target risulta spostato: tuna finché marker, oggetto e target sono coerenti. -
-
- - - - -
-
- - - -
-
- Piano di lavoro. Clicca "Seleziona area", poi traccia un rettangolo sull'immagine con shift+drag: il backend calcola il piano dalla depth in quell'area. Marker e oggetto vengono poi proiettati su questo piano. -
-
- - - -
-
piano: —
-
- -

Stato

-
-
nessun piano calcolato
-
- -

Dettagli piano

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