refactor: remove visual-servo feature
The marker-relative servo (endpoints /api/grasp/visual_servo/{plan,step,
plane/*}, helpers in grasp_routes.py, the bulk of grasp/visual_servo.py,
web/servo.js, and the Servo Visuale GUI tab) is gone. Plane fitting now
lives in /api/work_plane/* and perception/work_plane.py; Task 5 wires the
new Calibrazione Piano tab.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
1 parent
565c3df2f4
commit
eb53f30e23
5 files changed
+3
-1455
No files matched your search
@@ -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}
|
||||
@@ -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,
|
||||
)
|
||||
@@ -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)
|
||||
|
||||
@@ -14,7 +14,6 @@
|
||||
<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="servo">Servo Visuale</button>
|
||||
<button class="tab" data-tab="view3d">Vista 3D</button>
|
||||
<button class="tab" data-tab="ee">Movimento EE</button>
|
||||
</nav>
|
||||
@@ -234,87 +233,6 @@
|
||||
</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>
|
||||
|
||||
<!-- ============================== VISTA 3D ============================== -->
|
||||
<section class="panel" id="tab-view3d">
|
||||
<div class="grid-2">
|
||||
@@ -419,6 +337,5 @@
|
||||
}
|
||||
</script>
|
||||
<script src="/static/app.js"></script>
|
||||
<script type="module" src="/static/servo.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
-669
@@ -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(); });
|
||||
Reference in new issue
Block a user