Merge feat/work-plane-filter: work-plane calibration + below-plane filter
8 commits — perception/work_plane module, filter wiring on 3 sites,
/api/work_plane/* endpoints, visual-servo removal, Calibrazione Piano tab,
GUI cleanup (Cerca modello + Precisione/Dettagli removed).
- New: perception/work_plane.py owns WorkPlane (+offset_mm), fit_plane_svd,
disk persistence, apply_below_plane_filter. 8 unit tests.
- Filter applied at all three cloud sites (sam21_bridge, _points_via_bbox,
_object_cloud_cv bbox branch). Responses carry plane_applied +
n_points_before_plane on /scan, /object_points, /grasp/{plan,execute}.
- Endpoints: POST /api/work_plane/fit (preview, no save), /save, GET,
DELETE. Two-stage fit/save UX.
- Visual-servo feature removed entirely (~1500 lines: endpoints, helpers,
visual_servo.py, servo.js, tab GUI).
- New tab "Calibrazione Piano": shift+drag ROI on RGB, fit preview, 3D
viewer with colored scatter (green/yellow/red/grey by signed distance),
offset slider re-colors client-side, save/clear.
- GUI cleanup: "TrovaUtensile" renamed "Cerca modello"; removed
"Precisione (ultime 5 detection)" stats panel and "Dettagli" JSON dump.
- Verified live: baseline (no plane) → plane_applied:false everywhere;
with saved plane → plane_applied:true on all 3 endpoints + JSON file on
disk; DELETE → file gone + plane_applied:false. Zero ERROR logs.
This commit is contained in:
Adriano Dal Pastro committed
commit
6f1649b384
12 files changed
+812
-1469
No files matched your search
@@ -11,6 +11,7 @@ from . import (
|
||||
perception_routes,
|
||||
posture_routes,
|
||||
robot_routes,
|
||||
work_plane_routes,
|
||||
wrist_routes,
|
||||
)
|
||||
|
||||
@@ -23,5 +24,6 @@ __all__ = [
|
||||
"perception_routes",
|
||||
"posture_routes",
|
||||
"robot_routes",
|
||||
"work_plane_routes",
|
||||
"wrist_routes",
|
||||
]
|
||||
@@ -5,26 +5,17 @@ from __future__ import annotations
|
||||
from typing import Any, Optional
|
||||
|
||||
import numpy as np
|
||||
import time
|
||||
from dataclasses import asdict
|
||||
from fastapi import APIRouter, HTTPException
|
||||
from pydantic import BaseModel, Field
|
||||
from pydantic import BaseModel
|
||||
|
||||
from ..calibration import CalibrationStore
|
||||
from ..clients import G1Unavailable
|
||||
from ..config import get_config
|
||||
from ..geometry import transform_points
|
||||
from ..grasp import GraspPlanner, compute_axes
|
||||
from ..grasp.visual_servo import (
|
||||
ServoError,
|
||||
WorkPlane,
|
||||
fit_plane_svd,
|
||||
get_work_plane,
|
||||
plan as servo_plan,
|
||||
set_work_plane,
|
||||
)
|
||||
from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame
|
||||
from ..perception.sam21_bridge import Sam21Fallback, points_via_sam21
|
||||
from ..perception.work_plane import apply_below_plane_filter, get_work_plane
|
||||
from .perception_routes import _bbox_to_roi
|
||||
from ..timing import Stopwatch
|
||||
from . import _deps
|
||||
@@ -71,27 +62,32 @@ def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopw
|
||||
|
||||
if cfg.get("grasp", {}).get("use_sam21", True):
|
||||
try:
|
||||
pts_cv = points_via_sam21(nuwa, _deps.sam21(), bbox, sw)
|
||||
pts_cv, plane_info = points_via_sam21(nuwa, _deps.sam21(), bbox, sw)
|
||||
with sw.step("to_base_frame"):
|
||||
pts_base = transform_points(T_base_cam, pts_cv)
|
||||
meta["source"] = "sam21"
|
||||
meta["n_points"] = int(len(pts_base))
|
||||
meta["plane_applied"] = plane_info["plane_applied"]
|
||||
meta["n_points_before_plane"] = plane_info["n_points_before_plane"]
|
||||
return pts_base, meta
|
||||
except Sam21Fallback as e:
|
||||
# Logging happens inside points_via_sam21 with the right level.
|
||||
meta["sam21_fallback_reason"] = str(e)
|
||||
|
||||
pts_base = _points_via_bbox(nuwa, T_base_cam, bbox, sw)
|
||||
pts_base, plane_info = _points_via_bbox(nuwa, T_base_cam, bbox, sw)
|
||||
meta["source"] = "bbox_roi"
|
||||
meta["n_points"] = int(len(pts_base))
|
||||
meta["plane_applied"] = plane_info["plane_applied"]
|
||||
meta["n_points_before_plane"] = plane_info["n_points_before_plane"]
|
||||
return pts_base, meta
|
||||
|
||||
|
||||
def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, int],
|
||||
sw: Stopwatch) -> np.ndarray:
|
||||
"""Legacy path: bbox → Nuwa ROI cloud → filter outliers → base frame.
|
||||
sw: Stopwatch) -> tuple[np.ndarray, dict]:
|
||||
"""Legacy path: bbox → Nuwa ROI cloud → outlier filter → plane filter → base frame.
|
||||
|
||||
Raises HTTPException(422) on <10 valid depth points (existing behaviour).
|
||||
Raises HTTPException(422) on <10 valid depth points (after both outlier
|
||||
filter and below-plane filter). Returns (pts_base, plane_info).
|
||||
"""
|
||||
cfg = get_config()
|
||||
u_min, v_min, u_max, v_max = bbox
|
||||
@@ -106,12 +102,17 @@ def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, in
|
||||
raise HTTPException(status_code=502, detail=f"Nuwa ROI pointcloud failed: {e}")
|
||||
with sw.step("filter_outliers"):
|
||||
cropped = filter_outliers(pc)
|
||||
if len(cropped) < 10:
|
||||
raise HTTPException(status_code=422, detail=f"too few valid depth points in bbox: {len(cropped)}")
|
||||
pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64)
|
||||
pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane())
|
||||
if len(pts_cv) < 10:
|
||||
raise HTTPException(
|
||||
status_code=422,
|
||||
detail=f"too few valid depth points after plane filter: {len(pts_cv)}"
|
||||
f" (before plane: {plane_info['n_points_before_plane']})"
|
||||
)
|
||||
with sw.step("to_base_frame"):
|
||||
pts_cv = nuwa_to_cv_frame(cropped[:, :3])
|
||||
pts_base = transform_points(T_base_cam, pts_cv)
|
||||
return pts_base
|
||||
return pts_base, plane_info
|
||||
|
||||
|
||||
@router.post("/plan")
|
||||
@@ -171,175 +172,3 @@ def execute(req: ExecuteReq) -> dict[str, Any]:
|
||||
raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
|
||||
return {"plan": g.to_dict(), "steps": steps, "timings_ms": sw.as_dict(), **meta}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Visual servo (marker-relative, no static hand-eye calibration).
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class VisualServoReq(BaseModel):
|
||||
conf: Optional[float] = None
|
||||
class_name: Optional[str] = None
|
||||
side: Optional[str] = None
|
||||
min_distance_m: Optional[float] = None # default from config (0.1 m)
|
||||
step_fraction: Optional[float] = None # default from config (0.5)
|
||||
# Override for grasp.visual_servo.wrist_marker_rvec (radians). Useful for
|
||||
# live tuning from the UI without persisting to config on every try.
|
||||
wrist_marker_rvec: Optional[list[float]] = None
|
||||
|
||||
|
||||
class VisualServoStepReq(VisualServoReq):
|
||||
dry_run: bool = True
|
||||
timeout_s: float = 5.0
|
||||
|
||||
|
||||
def _servo_plan_from_req(req: VisualServoReq, sw: Stopwatch):
|
||||
cfg = get_config()
|
||||
vs_cfg = (cfg.get("grasp", {}) or {}).get("visual_servo", {}) or {}
|
||||
side = req.side or cfg["robot"]["wrist_side"]
|
||||
min_dist = float(req.min_distance_m
|
||||
if req.min_distance_m is not None
|
||||
else vs_cfg.get("min_distance_m", 0.10))
|
||||
step_frac = float(req.step_fraction
|
||||
if req.step_fraction is not None
|
||||
else vs_cfg.get("step_fraction", 0.5))
|
||||
# Request-level rvec override: thread it into a shallow-copied cfg so the
|
||||
# core plan() function keeps reading from a single place.
|
||||
if req.wrist_marker_rvec is not None:
|
||||
cfg = {**cfg, "grasp": {**(cfg.get("grasp") or {}),
|
||||
"visual_servo": {**vs_cfg,
|
||||
"wrist_marker_rvec": list(req.wrist_marker_rvec)}}}
|
||||
marker_id = int(cfg["aruco"]["wrist_marker"]["id"])
|
||||
size_mm = float(cfg["aruco"]["wrist_marker"]["size_mm"])
|
||||
pc_step = int(cfg.get("grasp", {}).get("pointcloud_step", 1))
|
||||
pc_max_depth = int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000))
|
||||
try:
|
||||
with sw.step("servo_plan"):
|
||||
result = servo_plan(
|
||||
side=side, marker_id=marker_id, size_mm=size_mm,
|
||||
conf=req.conf, class_name=req.class_name,
|
||||
min_distance_m=min_dist, step_fraction=step_frac,
|
||||
pointcloud_step=pc_step, pointcloud_max_depth_mm=pc_max_depth,
|
||||
cfg=cfg,
|
||||
nuwa=_deps.nuwa(), marker=_deps.marker(),
|
||||
yolo=_deps.yolo(), g1=_deps.g1(dry_run=False),
|
||||
)
|
||||
except ServoError as e:
|
||||
raise HTTPException(status_code=422, detail=str(e))
|
||||
except Exception as e:
|
||||
raise HTTPException(status_code=502, detail=f"servo plan failed: {e}")
|
||||
return side, min_dist, step_frac, result
|
||||
|
||||
|
||||
@router.post("/visual_servo/plan")
|
||||
def visual_servo_plan(req: VisualServoReq) -> dict[str, Any]:
|
||||
"""Perception + next-step computation, no motion. Useful for UI preview."""
|
||||
sw = Stopwatch()
|
||||
side, min_dist, step_frac, p = _servo_plan_from_req(req, sw)
|
||||
return {
|
||||
"side": side,
|
||||
"min_distance_m": min_dist,
|
||||
"step_fraction": step_frac,
|
||||
"plan": p.__dict__,
|
||||
"timings_ms": sw.as_dict(),
|
||||
}
|
||||
|
||||
|
||||
@router.post("/visual_servo/step")
|
||||
def visual_servo_step(req: VisualServoStepReq) -> dict[str, Any]:
|
||||
"""Plan + execute one half-step toward the object. Returns the move result
|
||||
plus the plan (before the move) so the UI can visualise what just happened;
|
||||
the next click re-plans from the new wrist pose."""
|
||||
sw = Stopwatch()
|
||||
side, min_dist, step_frac, p = _servo_plan_from_req(req, sw)
|
||||
move_result: dict[str, Any] | None = None
|
||||
if p.done or p.target_position_base is None:
|
||||
return {
|
||||
"side": side,
|
||||
"min_distance_m": min_dist,
|
||||
"step_fraction": step_frac,
|
||||
"plan": p.__dict__,
|
||||
"moved": False,
|
||||
"reason": p.reason or "reached",
|
||||
"timings_ms": sw.as_dict(),
|
||||
}
|
||||
g1 = _deps.g1(dry_run=req.dry_run)
|
||||
try:
|
||||
with sw.step("wrist_absolute"):
|
||||
move_result = g1.wrist_absolute(
|
||||
side=side,
|
||||
position=list(p.target_position_base),
|
||||
quaternion=list(p.target_quaternion_base or p.wrist_quaternion_base),
|
||||
wait=True,
|
||||
timeout_s=req.timeout_s,
|
||||
)
|
||||
except G1Unavailable as e:
|
||||
raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
|
||||
return {
|
||||
"side": side,
|
||||
"min_distance_m": min_dist,
|
||||
"step_fraction": step_frac,
|
||||
"plan": p.__dict__,
|
||||
"moved": True,
|
||||
"move_result": move_result,
|
||||
"timings_ms": sw.as_dict(),
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Work plane (for the visual-servo tab): fit once from a user-selected ROI,
|
||||
# stored in-memory (singleton). Subsequent visual_servo/plan calls enrich the
|
||||
# response with plane projections of marker and object.
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class PlaneFitReq(BaseModel):
|
||||
u_min: int = Field(..., ge=0)
|
||||
v_min: int = Field(..., ge=0)
|
||||
u_max: int = Field(..., gt=0)
|
||||
v_max: int = Field(..., gt=0)
|
||||
max_depth_mm: Optional[int] = None
|
||||
|
||||
|
||||
@router.post("/visual_servo/plane/fit")
|
||||
def visual_servo_plane_fit(req: PlaneFitReq) -> dict[str, Any]:
|
||||
cfg = get_config()
|
||||
nuwa = _deps.nuwa()
|
||||
pc_max = req.max_depth_mm or int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000))
|
||||
try:
|
||||
pc, _meta = nuwa.pointcloud_roi(
|
||||
req.u_min, req.v_min, req.u_max, req.v_max,
|
||||
step=1, max_depth_mm=pc_max,
|
||||
)
|
||||
except Exception as e:
|
||||
raise HTTPException(status_code=502, detail=f"Nuwa pointcloud_roi failed: {e}")
|
||||
if len(pc) < 10:
|
||||
raise HTTPException(status_code=422, detail=f"too few depth points in ROI: {len(pc)}")
|
||||
pts_cv = nuwa_to_cv_frame(pc[:, :3])
|
||||
# Drop gross outliers before SVD (stray far-depth pixels around the edges).
|
||||
pts_cv = filter_outliers(pts_cv, n_std=2.0)
|
||||
try:
|
||||
normal, offset, centroid, rms_mm = fit_plane_svd(pts_cv)
|
||||
except ServoError as e:
|
||||
raise HTTPException(status_code=422, detail=str(e))
|
||||
wp = WorkPlane(
|
||||
normal=[float(x) for x in normal],
|
||||
offset=float(offset),
|
||||
centroid=[float(x) for x in centroid],
|
||||
n_inliers=int(len(pts_cv)),
|
||||
fit_rms_mm=float(rms_mm),
|
||||
roi=[int(req.u_min), int(req.v_min), int(req.u_max), int(req.v_max)],
|
||||
created_at=time.time(),
|
||||
)
|
||||
set_work_plane(wp)
|
||||
return {"ok": True, "plane": asdict(wp)}
|
||||
|
||||
|
||||
@router.get("/visual_servo/plane")
|
||||
def visual_servo_plane_get() -> dict[str, Any]:
|
||||
wp = get_work_plane()
|
||||
return {"set": wp is not None, "plane": asdict(wp) if wp is not None else None}
|
||||
|
||||
|
||||
@router.delete("/visual_servo/plane")
|
||||
def visual_servo_plane_clear() -> dict[str, Any]:
|
||||
set_work_plane(None)
|
||||
return {"ok": True, "set": False}
|
||||
@@ -12,6 +12,7 @@ from ..config import get_config
|
||||
from ..geometry import transform_points
|
||||
from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame
|
||||
from ..perception.sam21_bridge import Sam21Fallback, points_via_sam21
|
||||
from ..perception.work_plane import apply_below_plane_filter, get_work_plane
|
||||
from ..timing import Stopwatch
|
||||
from . import _deps
|
||||
|
||||
@@ -110,6 +111,9 @@ def scan(req: ScanReq) -> dict[str, Any]:
|
||||
out["source"] = source
|
||||
if extra.get("sam21_fallback_reason"):
|
||||
out["sam21_fallback_reason"] = extra["sam21_fallback_reason"]
|
||||
if "plane_applied" in extra:
|
||||
out["plane_applied"] = extra["plane_applied"]
|
||||
out["n_points_before_plane"] = extra["n_points_before_plane"]
|
||||
out["n_points"] = int(len(pts_cv))
|
||||
out["points_camera"] = pts_cv.tolist() if len(pts_cv) else []
|
||||
|
||||
@@ -153,6 +157,9 @@ def object_points(req: DetectReq) -> dict[str, Any]:
|
||||
}
|
||||
if extra.get("sam21_fallback_reason"):
|
||||
out["sam21_fallback_reason"] = extra["sam21_fallback_reason"]
|
||||
if "plane_applied" in extra:
|
||||
out["plane_applied"] = extra["plane_applied"]
|
||||
out["n_points_before_plane"] = extra["n_points_before_plane"]
|
||||
|
||||
T_base_cam = _load_calibration()
|
||||
if T_base_cam is not None and len(pts_cv) > 0:
|
||||
@@ -177,7 +184,9 @@ def _object_cloud_cv(nuwa, bbox: tuple[int, int, int, int], max_points: int,
|
||||
extra: dict[str, Any] = {}
|
||||
if cfg.get("grasp", {}).get("use_sam21", True):
|
||||
try:
|
||||
pts_cv = points_via_sam21(nuwa, _deps.sam21(), bbox, sw)
|
||||
pts_cv, plane_info = points_via_sam21(nuwa, _deps.sam21(), bbox, sw)
|
||||
extra["plane_applied"] = plane_info["plane_applied"]
|
||||
extra["n_points_before_plane"] = plane_info["n_points_before_plane"]
|
||||
if len(pts_cv) > max_points:
|
||||
idx = np.random.choice(len(pts_cv), max_points, replace=False)
|
||||
pts_cv = pts_cv[idx]
|
||||
@@ -198,10 +207,13 @@ def _object_cloud_cv(nuwa, bbox: tuple[int, int, int, int], max_points: int,
|
||||
raise HTTPException(status_code=502, detail=f"Nuwa pointcloud ROI failed: {e}")
|
||||
with sw.step("filter_outliers"):
|
||||
cropped = filter_outliers(pc)
|
||||
if len(cropped) > max_points:
|
||||
idx = np.random.choice(len(cropped), max_points, replace=False)
|
||||
cropped = cropped[idx]
|
||||
pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64)
|
||||
pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane())
|
||||
extra["plane_applied"] = plane_info["plane_applied"]
|
||||
extra["n_points_before_plane"] = plane_info["n_points_before_plane"]
|
||||
if len(pts_cv) > max_points:
|
||||
idx = np.random.choice(len(pts_cv), max_points, replace=False)
|
||||
pts_cv = pts_cv[idx]
|
||||
return pts_cv, "bbox_roi", extra
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,123 @@
|
||||
"""Work-plane calibration endpoints: fit (preview), save, get, delete."""
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from dataclasses import asdict
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
from fastapi import APIRouter, HTTPException
|
||||
from pydantic import BaseModel, Field
|
||||
|
||||
from ..perception import filter_outliers, nuwa_to_cv_frame
|
||||
from ..perception.work_plane import (
|
||||
WorkPlane,
|
||||
WorkPlaneError,
|
||||
fit_plane_svd,
|
||||
get_work_plane,
|
||||
set_work_plane,
|
||||
)
|
||||
from ..timing import Stopwatch
|
||||
from . import _deps
|
||||
|
||||
router = APIRouter()
|
||||
|
||||
|
||||
class FitReq(BaseModel):
|
||||
u_min: int = Field(..., ge=0)
|
||||
v_min: int = Field(..., ge=0)
|
||||
u_max: int = Field(..., gt=0)
|
||||
v_max: int = Field(..., gt=0)
|
||||
offset_mm: float = Field(5.0, ge=0.0, le=200.0)
|
||||
max_preview_points: int = Field(5000, ge=100, le=20000)
|
||||
|
||||
|
||||
class SaveReq(BaseModel):
|
||||
plane: dict[str, Any] # accept a serialised WorkPlane
|
||||
|
||||
|
||||
@router.post("/fit")
|
||||
def fit(req: FitReq) -> dict[str, Any]:
|
||||
"""Compute a candidate work plane from a rectangle on the RGB image.
|
||||
|
||||
Does NOT persist. Returns the plane + a downsampled CV-frame cloud for
|
||||
the GUI 3D preview.
|
||||
"""
|
||||
sw = Stopwatch()
|
||||
nuwa = _deps.nuwa()
|
||||
try:
|
||||
with sw.step("pointcloud_roi"):
|
||||
pc, _meta = nuwa.pointcloud_roi(
|
||||
req.u_min, req.v_min, req.u_max, req.v_max,
|
||||
step=1, max_depth_mm=4000,
|
||||
)
|
||||
except Exception as e:
|
||||
raise HTTPException(status_code=502, detail=f"Nuwa pointcloud_roi failed: {e}")
|
||||
if len(pc) < 10:
|
||||
raise HTTPException(
|
||||
status_code=422,
|
||||
detail=f"too few depth points in ROI: {len(pc)}",
|
||||
)
|
||||
pts_cv = nuwa_to_cv_frame(pc[:, :3])
|
||||
with sw.step("filter_outliers"):
|
||||
pts_cv = filter_outliers(pts_cv, n_std=2.0)
|
||||
if len(pts_cv) < 10:
|
||||
raise HTTPException(
|
||||
status_code=422,
|
||||
detail=f"too few inliers after outlier filter: {len(pts_cv)}",
|
||||
)
|
||||
try:
|
||||
with sw.step("fit_plane_svd"):
|
||||
normal, offset, centroid, rms_mm = fit_plane_svd(pts_cv)
|
||||
except WorkPlaneError as e:
|
||||
raise HTTPException(status_code=422, detail=str(e))
|
||||
|
||||
wp = WorkPlane(
|
||||
normal=[float(x) for x in normal],
|
||||
offset=float(offset),
|
||||
centroid=[float(x) for x in centroid],
|
||||
n_inliers=int(len(pts_cv)),
|
||||
fit_rms_mm=float(rms_mm),
|
||||
roi=[int(req.u_min), int(req.v_min), int(req.u_max), int(req.v_max)],
|
||||
offset_mm=float(req.offset_mm),
|
||||
created_at=time.time(),
|
||||
)
|
||||
|
||||
# Downsample for preview.
|
||||
if len(pts_cv) > req.max_preview_points:
|
||||
idx = np.random.choice(len(pts_cv), req.max_preview_points, replace=False)
|
||||
preview = pts_cv[idx]
|
||||
else:
|
||||
preview = pts_cv
|
||||
|
||||
return {
|
||||
"plane": asdict(wp),
|
||||
"preview": True,
|
||||
"points_camera": preview.tolist(),
|
||||
"timings_ms": sw.as_dict(),
|
||||
}
|
||||
|
||||
|
||||
@router.post("/save")
|
||||
def save(req: SaveReq) -> dict[str, Any]:
|
||||
"""Persist a previously-fitted work plane to disk."""
|
||||
try:
|
||||
d = dict(req.plane)
|
||||
d.setdefault("offset_mm", 5.0)
|
||||
wp = WorkPlane(**d)
|
||||
except Exception as e:
|
||||
raise HTTPException(status_code=422, detail=f"invalid plane payload: {e}")
|
||||
set_work_plane(wp)
|
||||
return {"ok": True, "plane": asdict(wp), "saved": True}
|
||||
|
||||
|
||||
@router.get("")
|
||||
def get_current() -> dict[str, Any]:
|
||||
wp = get_work_plane()
|
||||
return {"plane": asdict(wp) if wp is not None else None}
|
||||
|
||||
|
||||
@router.delete("")
|
||||
def delete_current() -> dict[str, Any]:
|
||||
set_work_plane(None)
|
||||
return {"ok": True, "plane": None}
|
||||
@@ -1,504 +0,0 @@
|
||||
"""Marker-relative visual servoing (no static hand-eye calibration).
|
||||
|
||||
Idea: the wrist-mounted ArUco marker is our live reference. Each step we read
|
||||
current wrist pose from G1, detect the marker in the camera frame, detect the
|
||||
object (YOLO bbox → Nuwa ROI point cloud → centroid), and plan a wrist move
|
||||
that advances a fraction of the camera-frame marker→object vector.
|
||||
|
||||
Mapping a camera-frame delta to a base-frame wrist delta needs `R_base_camera`,
|
||||
which is derived live each step:
|
||||
|
||||
R_base_camera = R_base_wrist @ R_wrist_marker @ R_cam_marker^-1
|
||||
|
||||
Only the *rotation* part of `T_wrist_marker` matters for deltas — translation
|
||||
cancels. We take `R_wrist_marker` from config (rvec), defaulting to identity
|
||||
when the marker is roughly aligned with the wrist frame.
|
||||
|
||||
Stop condition: camera-frame marker→object distance ≤ `min_distance_m`. This
|
||||
is a direct physical proxy for wrist-to-object proximity, independent of the
|
||||
(unknown) marker-to-wrist translation offset.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import logging
|
||||
import time
|
||||
from dataclasses import asdict, dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..clients import G1Client, MarkerClient, NuwaClient, YoloClient
|
||||
from ..config import data_dir
|
||||
from ..geometry import (
|
||||
T_from_pos_quat,
|
||||
invert_T,
|
||||
make_T,
|
||||
quat_xyzw_to_R,
|
||||
rvec_to_R,
|
||||
)
|
||||
from ..perception import filter_outliers, nuwa_to_cv_frame
|
||||
from ..perception.aruco_pose import nuwa_intrinsics_to_matrix, solve_square_marker
|
||||
from ..perception.detector import ObjectDetector
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class ServoError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass
|
||||
class WorkPlane:
|
||||
"""Plane in camera frame: normal·X + offset = 0, normal is unit-length and
|
||||
pointing roughly toward the camera (i.e. has negative Z in OpenCV frame)."""
|
||||
normal: list[float]
|
||||
offset: float
|
||||
centroid: list[float]
|
||||
n_inliers: int
|
||||
fit_rms_mm: float
|
||||
roi: list[int]
|
||||
created_at: float
|
||||
|
||||
|
||||
_WORK_PLANE: WorkPlane | None = None
|
||||
_WORK_PLANE_LOADED = False
|
||||
|
||||
|
||||
def _work_plane_path() -> Path:
|
||||
return data_dir() / "work_plane.json"
|
||||
|
||||
|
||||
def _save_work_plane_to_disk(wp: WorkPlane | None) -> None:
|
||||
path = _work_plane_path()
|
||||
try:
|
||||
if wp is None:
|
||||
if path.exists():
|
||||
path.unlink()
|
||||
return
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with open(path, "w") as f:
|
||||
json.dump(asdict(wp), f, indent=2)
|
||||
except Exception as e:
|
||||
log.warning("failed to persist work plane: %s", e)
|
||||
|
||||
|
||||
def _load_work_plane_from_disk() -> WorkPlane | None:
|
||||
path = _work_plane_path()
|
||||
try:
|
||||
with open(path, "r") as f:
|
||||
d = json.load(f)
|
||||
return WorkPlane(**d)
|
||||
except FileNotFoundError:
|
||||
return None
|
||||
except Exception as e:
|
||||
log.warning("failed to load persisted work plane (%s)", e)
|
||||
return None
|
||||
|
||||
|
||||
def get_work_plane() -> WorkPlane | None:
|
||||
global _WORK_PLANE, _WORK_PLANE_LOADED
|
||||
if not _WORK_PLANE_LOADED:
|
||||
_WORK_PLANE = _load_work_plane_from_disk()
|
||||
_WORK_PLANE_LOADED = True
|
||||
return _WORK_PLANE
|
||||
|
||||
|
||||
def set_work_plane(wp: WorkPlane | None) -> None:
|
||||
global _WORK_PLANE, _WORK_PLANE_LOADED
|
||||
_WORK_PLANE = wp
|
||||
_WORK_PLANE_LOADED = True
|
||||
_save_work_plane_to_disk(wp)
|
||||
|
||||
|
||||
def fit_plane_svd(points_xyz: np.ndarray
|
||||
) -> tuple[np.ndarray, float, np.ndarray, float]:
|
||||
"""Least-squares plane via SVD. Returns (normal, offset, centroid, rms_mm).
|
||||
|
||||
Plane equation: normal·X + offset = 0. Normal is oriented to point toward
|
||||
the camera origin (negative Z in CV frame)."""
|
||||
pts = np.asarray(points_xyz, dtype=np.float64)
|
||||
if pts.ndim != 2 or pts.shape[1] < 3:
|
||||
raise ServoError(f"unexpected points shape for plane fit: {pts.shape}")
|
||||
xyz = pts[:, :3]
|
||||
if len(xyz) < 10:
|
||||
raise ServoError(f"too few points to fit plane: {len(xyz)}")
|
||||
c = xyz.mean(axis=0)
|
||||
centered = xyz - c
|
||||
_, _, Vt = np.linalg.svd(centered, full_matrices=False)
|
||||
normal = Vt[-1]
|
||||
n = float(np.linalg.norm(normal))
|
||||
if n < 1e-9:
|
||||
raise ServoError("degenerate plane (zero normal)")
|
||||
normal = normal / n
|
||||
# Orient normal to point toward the camera origin: the signed distance of
|
||||
# the origin from the plane must be positive, i.e. d = -n·c > 0 → n·c < 0.
|
||||
# Flipping only on normal[2] is wrong for tilted planes (the camera may
|
||||
# not be above the plane in Z-only sense).
|
||||
if float(normal @ c) > 0:
|
||||
normal = -normal
|
||||
offset = -float(normal @ c)
|
||||
signed = centered @ normal
|
||||
rms_mm = float(np.sqrt(np.mean(signed * signed)) * 1000.0)
|
||||
return normal, offset, c, rms_mm
|
||||
|
||||
|
||||
def project_point_on_plane(P: np.ndarray, normal: np.ndarray, offset: float
|
||||
) -> tuple[np.ndarray, float]:
|
||||
"""Perpendicular projection of a 3D point onto the plane.
|
||||
Returns (foot, signed_distance). Signed distance > 0 means P is on the
|
||||
side of the plane the normal points toward (camera side in our convention).
|
||||
"""
|
||||
s = float(normal @ P + offset)
|
||||
return P - s * normal, s
|
||||
|
||||
|
||||
def backproject_pixel_on_plane(uv: np.ndarray, K: np.ndarray,
|
||||
normal: np.ndarray, offset: float
|
||||
) -> np.ndarray:
|
||||
"""Intersect the viewing ray through pixel (u,v) with the work plane."""
|
||||
r = np.linalg.inv(K) @ np.array([float(uv[0]), float(uv[1]), 1.0])
|
||||
denom = float(normal @ r)
|
||||
if abs(denom) < 1e-9:
|
||||
raise ServoError("ray parallel to work plane")
|
||||
t = -offset / denom
|
||||
if t <= 0:
|
||||
raise ServoError("plane intersection is behind the camera")
|
||||
return t * r
|
||||
|
||||
|
||||
@dataclass
|
||||
class ServoPlan:
|
||||
# Truth (from G1)
|
||||
wrist_position_base: list[float]
|
||||
wrist_quaternion_base: list[float]
|
||||
|
||||
# Camera-frame observations
|
||||
marker_position_cam: list[float]
|
||||
object_position_cam: list[float]
|
||||
|
||||
# Pixel-space positions for the 2D overlay (u, v in image coordinates).
|
||||
marker_pixel: list[float]
|
||||
object_pixel: list[float]
|
||||
|
||||
# Perpendicular depth to the camera plane (Z component, m). This is what
|
||||
# drives the pinhole mm/px scale — NOT the slant range.
|
||||
marker_depth_z_m: float
|
||||
object_depth_z_m: float
|
||||
# Euclidean distance camera origin → point (slant range, m).
|
||||
marker_distance_cam_m: float
|
||||
object_distance_cam_m: float
|
||||
|
||||
# Pinhole scale at the point's depth: mm-per-pixel = (z_mm / fx), i.e.
|
||||
# physical size of one image pixel at that depth. Depends on Z only, so
|
||||
# two points at the same Z but different XY will share mm/px.
|
||||
marker_mm_per_px: float
|
||||
object_mm_per_px: float
|
||||
|
||||
# Approximate base-frame positions for visualisation only. Computed with
|
||||
# T_wrist_marker = identity: relative distances are correct, absolute
|
||||
# positions are offset by the unknown marker-to-wrist translation.
|
||||
marker_position_base_est: list[float]
|
||||
object_position_base_est: list[float]
|
||||
T_base_camera_est: list[list[float]]
|
||||
|
||||
# Scalar cues
|
||||
distance_m: float # camera-frame magnitude of marker→object
|
||||
done: bool # True when distance ≤ min_distance_m
|
||||
|
||||
# Next wrist target in base frame (None when done)
|
||||
target_position_base: list[float] | None
|
||||
target_quaternion_base: list[float] | None
|
||||
planned_step_m: float # how much the wrist is about to move (base frame)
|
||||
|
||||
# Work-plane projection block — present only when a plane has been fitted.
|
||||
# Everything camera-frame. `in_plane_distance_m` is the Euclidean distance
|
||||
# between the two foot-of-perpendicular points (lives on the plane).
|
||||
plane: dict[str, Any] | None = None
|
||||
|
||||
# Diagnostics
|
||||
detection: dict[str, Any] | None = None
|
||||
n_object_points: int = 0
|
||||
marker_reprojection_error_px: float | None = None
|
||||
marker_translation_source: str = "solvepnp" # "nuwa_depth" | "solvepnp"
|
||||
object_point_source: str = "centroid" # "centroid" | "highest_above_plane(...)"
|
||||
reason: str | None = None
|
||||
|
||||
|
||||
def _resolve_wrist_pose(g1: G1Client, side: str) -> tuple[np.ndarray, np.ndarray, list[float]]:
|
||||
w = g1.wrist()
|
||||
body = w.get("data") if isinstance(w.get("data"), dict) else w
|
||||
pose = body.get(f"{side}_wrist") or body.get(side)
|
||||
if pose is None or "position" not in pose or "quaternion" not in pose:
|
||||
raise ServoError(f"G1 /wrist missing '{side}' pose: {w!r}")
|
||||
pos = np.asarray(pose["position"], dtype=np.float64)
|
||||
quat = np.asarray(pose["quaternion"], dtype=np.float64)
|
||||
return pos, quat, list(map(float, pose["quaternion"]))
|
||||
|
||||
|
||||
def _detect_marker_cam(nuwa: NuwaClient, marker: MarkerClient,
|
||||
marker_id: int, size_mm: float
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray, float | None, str]:
|
||||
"""Return (R_cam_marker, t_cam_marker, corners_px, reprojection_error_px, source).
|
||||
|
||||
Translation comes from the Nuwa depth at the marker centre pixel whenever
|
||||
possible — much more stable than solvePnP on a small planar marker, which
|
||||
suffers from the IPPE_SQUARE planar ambiguity (two solutions, same
|
||||
reprojection error) at ~1.4 m distance. Rotation still comes from solvePnP
|
||||
because the servo mapping cam→base needs it.
|
||||
|
||||
`source` reports how `t_cam_marker` was obtained: "nuwa_depth" when the
|
||||
depth-sensor value at the marker pixel was usable, "solvepnp" otherwise.
|
||||
"""
|
||||
result = marker.detect_url(
|
||||
markers=[{"id": int(marker_id), "size_mm": float(size_mm)}],
|
||||
image_url=nuwa.rgb_url(),
|
||||
)
|
||||
found = result.get("found", [])
|
||||
entry = next((m for m in found if int(m["id"]) == int(marker_id)), None)
|
||||
if entry is None:
|
||||
raise ServoError(f"marker id={marker_id} not detected (not_found={result.get('not_found')})")
|
||||
corners = np.asarray(entry["corners"], dtype=np.float64)
|
||||
|
||||
R_cm = None
|
||||
t_pnp = None
|
||||
reproj_err: float | None = None
|
||||
try:
|
||||
intr = nuwa.intrinsics()
|
||||
K, dist = nuwa_intrinsics_to_matrix(intr)
|
||||
sol = solve_square_marker(corners, float(size_mm) / 1000.0, K, dist)
|
||||
R_cm = sol["R"]
|
||||
t_pnp = sol["t"].flatten()
|
||||
reproj_err = float(sol["reprojection_error_px"])
|
||||
except Exception as e:
|
||||
log.warning("local pnp failed (%s) — using marker-finder orientation", e)
|
||||
R_cm = np.asarray(entry["rotation_matrix"], dtype=np.float64).reshape(3, 3)
|
||||
t_pnp = np.asarray(entry["position_m"], dtype=np.float64).flatten()
|
||||
|
||||
# Prefer Nuwa depth at the marker centre pixel for the translation.
|
||||
centre = corners.mean(axis=0)
|
||||
u = int(round(float(centre[0])))
|
||||
v = int(round(float(centre[1])))
|
||||
t_depth = None
|
||||
try:
|
||||
m = nuwa.measure(u, v, radius=8)
|
||||
pt = m.get("point_m") if isinstance(m, dict) else None
|
||||
if not isinstance(pt, dict):
|
||||
pt = m
|
||||
x = float(pt.get("x", pt.get("X")))
|
||||
y = float(pt.get("y", pt.get("Y")))
|
||||
z = float(pt.get("z", pt.get("Z")))
|
||||
# /api/measure returns already-OpenCV-frame coords (Z>0 in front of
|
||||
# the camera). No flip.
|
||||
t_depth = np.array([x, y, z], dtype=np.float64)
|
||||
if not np.all(np.isfinite(t_depth)) or abs(t_depth[2]) < 1e-3:
|
||||
t_depth = None
|
||||
except Exception as e:
|
||||
log.warning("nuwa.measure failed at (%d,%d): %s", u, v, e)
|
||||
t_depth = None
|
||||
|
||||
if t_depth is not None:
|
||||
return R_cm, t_depth, corners, reproj_err, "nuwa_depth"
|
||||
return R_cm, t_pnp, corners, reproj_err, "solvepnp"
|
||||
|
||||
|
||||
def _detect_object_point_cam(nuwa: NuwaClient, yolo: YoloClient,
|
||||
conf: float | None, class_name: str | None,
|
||||
pointcloud_step: int, pointcloud_max_depth_mm: int,
|
||||
work_plane: WorkPlane | None,
|
||||
top_fraction: float = 0.05,
|
||||
) -> tuple[np.ndarray, dict[str, Any], int, str]:
|
||||
"""Return (point_cam_xyz, top_detection, n_points, source).
|
||||
|
||||
When a work plane is available, returns the mean of the top `top_fraction`
|
||||
of points (by signed distance from the plane) — this is the highest part
|
||||
of the object above the work surface, which is the natural grasp target.
|
||||
Without a plane, falls back to the centroid of all filtered points."""
|
||||
detector = ObjectDetector(yolo, nuwa)
|
||||
dets = detector.detect(conf=conf)
|
||||
if class_name:
|
||||
dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()]
|
||||
if not dets:
|
||||
raise ServoError("no object detected" + (f" for class '{class_name}'" if class_name else ""))
|
||||
dets.sort(key=lambda d: d["confidence"], reverse=True)
|
||||
top = dets[0]
|
||||
b = top["bbox"]
|
||||
u_min = max(0, int(np.floor(b["x1"])))
|
||||
v_min = max(0, int(np.floor(b["y1"])))
|
||||
u_max = int(np.ceil(b["x2"]))
|
||||
v_max = int(np.ceil(b["y2"]))
|
||||
pc, _meta = nuwa.pointcloud_roi(u_min, v_min, u_max, v_max,
|
||||
step=pointcloud_step,
|
||||
max_depth_mm=pointcloud_max_depth_mm)
|
||||
# Empirical: Nuwa ROI points come in Nuwa-native frame (Z<0 in front of
|
||||
# camera) despite the docstring. Flip to true OpenCV frame so comparisons
|
||||
# with the plane (also CV) are consistent.
|
||||
cropped = filter_outliers(pc)
|
||||
if len(cropped) < 10:
|
||||
raise ServoError(f"too few valid depth points in bbox: {len(cropped)}")
|
||||
pts_cv = nuwa_to_cv_frame(cropped[:, :3])
|
||||
if work_plane is None:
|
||||
return pts_cv.mean(axis=0), top, int(len(pts_cv)), "centroid"
|
||||
n_vec = np.asarray(work_plane.normal, dtype=np.float64)
|
||||
d_off = float(work_plane.offset)
|
||||
signed = pts_cv @ n_vec + d_off # Nx (positive = above plane)
|
||||
k = max(1, int(round(float(top_fraction) * len(pts_cv))))
|
||||
idx = np.argpartition(signed, -k)[-k:]
|
||||
highest = pts_cv[idx].mean(axis=0)
|
||||
return highest, top, int(len(pts_cv)), f"highest_above_plane(top {k}/{len(pts_cv)})"
|
||||
|
||||
|
||||
def _r_wrist_marker_from_config(cfg: dict[str, Any]) -> np.ndarray:
|
||||
"""Rotation part of the marker-on-wrist rigid offset. Translation does not
|
||||
matter for camera-frame delta servo. Read from
|
||||
`grasp.visual_servo.wrist_marker_rvec` as a 3-vector (Rodrigues), default
|
||||
identity."""
|
||||
vs = cfg.get("grasp", {}).get("visual_servo", {}) or {}
|
||||
rvec = vs.get("wrist_marker_rvec") or [0.0, 0.0, 0.0]
|
||||
return rvec_to_R(np.asarray(rvec, dtype=np.float64))
|
||||
|
||||
|
||||
def plan(*, side: str, marker_id: int, size_mm: float,
|
||||
conf: float | None, class_name: str | None,
|
||||
min_distance_m: float, step_fraction: float,
|
||||
pointcloud_step: int, pointcloud_max_depth_mm: int,
|
||||
cfg: dict[str, Any],
|
||||
nuwa: NuwaClient, marker: MarkerClient, yolo: YoloClient, g1: G1Client,
|
||||
) -> ServoPlan:
|
||||
# 1) Wrist pose in base
|
||||
wrist_pos, wrist_quat, wrist_quat_list = _resolve_wrist_pose(g1, side)
|
||||
R_bw = quat_xyzw_to_R(wrist_quat)
|
||||
|
||||
# 2) Marker pose in camera
|
||||
R_cm, t_cm, corners_px, reproj_err, marker_t_source = _detect_marker_cam(
|
||||
nuwa, marker, marker_id, size_mm,
|
||||
)
|
||||
marker_pixel = corners_px.mean(axis=0) # centre of the 4 corners
|
||||
# Intrinsics for mm/px scale at each point's depth.
|
||||
try:
|
||||
intr = nuwa.intrinsics()
|
||||
K, _ = nuwa_intrinsics_to_matrix(intr)
|
||||
fx = float(K[0, 0])
|
||||
except Exception as e:
|
||||
log.warning("intrinsics unavailable for mm/px (%s)", e)
|
||||
fx = float("nan")
|
||||
|
||||
# 3) Object reference point in camera: highest-above-plane if a plane is
|
||||
# set, otherwise centroid of the bbox pointcloud.
|
||||
wp_here = get_work_plane()
|
||||
centroid_cam, detection, n_pts, object_point_source = _detect_object_point_cam(
|
||||
nuwa, yolo, conf, class_name,
|
||||
pointcloud_step, pointcloud_max_depth_mm,
|
||||
work_plane=wp_here,
|
||||
)
|
||||
b = detection["bbox"]
|
||||
object_pixel = np.array([0.5 * (b["x1"] + b["x2"]), 0.5 * (b["y1"] + b["y2"])])
|
||||
|
||||
# 4) Rotation mapping cam→base (identity marker-to-wrist rotation by default)
|
||||
R_wm = _r_wrist_marker_from_config(cfg)
|
||||
R_base_cam = R_bw @ R_wm @ np.linalg.inv(R_cm)
|
||||
|
||||
# 5) Camera-frame vector & distance (marker → object)
|
||||
d_cam = centroid_cam - t_cm
|
||||
distance_m = float(np.linalg.norm(d_cam))
|
||||
done = distance_m <= float(min_distance_m)
|
||||
|
||||
# 6) Estimate T_base_camera for visualisation (identity T_wrist_marker):
|
||||
# translation part is off by the real marker-to-wrist translation, but
|
||||
# relative positions of marker and object in base frame are preserved.
|
||||
T_bc_est = np.eye(4)
|
||||
T_bc_est[:3, :3] = R_base_cam
|
||||
T_bw_full = T_from_pos_quat(wrist_pos, wrist_quat)
|
||||
T_wm_identity = np.eye(4) # translation part left at zero
|
||||
T_wm_identity[:3, :3] = R_wm
|
||||
T_cm_full = make_T(R_cm, t_cm)
|
||||
T_bc_est = T_bw_full @ T_wm_identity @ invert_T(T_cm_full)
|
||||
|
||||
marker_base_est = (T_bc_est @ np.array([*t_cm, 1.0]))[:3]
|
||||
object_base_est = (T_bc_est @ np.array([*centroid_cam, 1.0]))[:3]
|
||||
|
||||
# 7) Plan step
|
||||
target_pos = None
|
||||
target_quat = None
|
||||
planned_step_m = 0.0
|
||||
reason = None
|
||||
if done:
|
||||
reason = f"distance {distance_m*1000:.1f} mm ≤ {min_distance_m*1000:.0f} mm — reached"
|
||||
else:
|
||||
step_cam = d_cam * float(step_fraction)
|
||||
step_len = float(np.linalg.norm(step_cam))
|
||||
# Don't overshoot below min_distance_m
|
||||
if distance_m - step_len < float(min_distance_m):
|
||||
step_cam = d_cam * ((distance_m - float(min_distance_m)) / distance_m)
|
||||
step_len = float(np.linalg.norm(step_cam))
|
||||
reason = "clamped to stop at min_distance_m"
|
||||
step_base = R_base_cam @ step_cam
|
||||
target_pos = wrist_pos + step_base
|
||||
target_quat = wrist_quat_list
|
||||
planned_step_m = float(np.linalg.norm(step_base))
|
||||
|
||||
# 8) Work-plane projection (optional)
|
||||
plane_block = None
|
||||
wp = get_work_plane()
|
||||
if wp is not None:
|
||||
try:
|
||||
K_plane, _ = nuwa_intrinsics_to_matrix(nuwa.intrinsics())
|
||||
n_vec = np.asarray(wp.normal, dtype=np.float64)
|
||||
d_off = float(wp.offset)
|
||||
# Perpendicular foot of the measured 3D points.
|
||||
m_foot, m_signed = project_point_on_plane(np.asarray(t_cm), n_vec, d_off)
|
||||
o_foot, o_signed = project_point_on_plane(np.asarray(centroid_cam), n_vec, d_off)
|
||||
# Back-projected pixel hits on the plane (trust pixel + depth plane,
|
||||
# ignoring the measured Z of each point — more stable when depth is
|
||||
# noisy, at the cost of assuming the point actually lies on the
|
||||
# plane).
|
||||
m_hit = backproject_pixel_on_plane(np.asarray(marker_pixel), K_plane, n_vec, d_off)
|
||||
o_hit = backproject_pixel_on_plane(np.asarray(object_pixel), K_plane, n_vec, d_off)
|
||||
plane_block = {
|
||||
"normal": wp.normal,
|
||||
"offset": wp.offset,
|
||||
"fit_rms_mm": wp.fit_rms_mm,
|
||||
"roi": wp.roi,
|
||||
"marker_foot_cam": [float(x) for x in m_foot],
|
||||
"object_foot_cam": [float(x) for x in o_foot],
|
||||
"marker_signed_distance_m": float(m_signed),
|
||||
"object_signed_distance_m": float(o_signed),
|
||||
"marker_pixel_hit_cam": [float(x) for x in m_hit],
|
||||
"object_pixel_hit_cam": [float(x) for x in o_hit],
|
||||
"in_plane_distance_m": float(np.linalg.norm(o_foot - m_foot)),
|
||||
"in_plane_pixel_distance_m": float(np.linalg.norm(o_hit - m_hit)),
|
||||
}
|
||||
except Exception as e:
|
||||
log.warning("plane projection failed: %s", e)
|
||||
|
||||
return ServoPlan(
|
||||
wrist_position_base=[float(x) for x in wrist_pos],
|
||||
wrist_quaternion_base=wrist_quat_list,
|
||||
marker_position_cam=[float(x) for x in t_cm],
|
||||
object_position_cam=[float(x) for x in centroid_cam],
|
||||
marker_pixel=[float(x) for x in marker_pixel],
|
||||
object_pixel=[float(x) for x in object_pixel],
|
||||
marker_depth_z_m=float(abs(t_cm[2])),
|
||||
object_depth_z_m=float(abs(centroid_cam[2])),
|
||||
marker_distance_cam_m=float(np.linalg.norm(t_cm)),
|
||||
object_distance_cam_m=float(np.linalg.norm(centroid_cam)),
|
||||
marker_mm_per_px=float(abs(t_cm[2]) * 1000.0 / fx) if fx == fx and fx > 0 else 0.0,
|
||||
object_mm_per_px=float(abs(centroid_cam[2]) * 1000.0 / fx) if fx == fx and fx > 0 else 0.0,
|
||||
marker_position_base_est=[float(x) for x in marker_base_est],
|
||||
object_position_base_est=[float(x) for x in object_base_est],
|
||||
T_base_camera_est=T_bc_est.tolist(),
|
||||
distance_m=distance_m,
|
||||
done=done,
|
||||
target_position_base=[float(x) for x in target_pos] if target_pos is not None else None,
|
||||
target_quaternion_base=target_quat,
|
||||
planned_step_m=planned_step_m,
|
||||
plane=plane_block,
|
||||
detection=detection,
|
||||
n_object_points=n_pts,
|
||||
marker_reprojection_error_px=reproj_err,
|
||||
marker_translation_source=marker_t_source,
|
||||
object_point_source=object_point_source,
|
||||
reason=reason,
|
||||
)
|
||||
@@ -19,6 +19,7 @@ from .api import (
|
||||
posture_routes,
|
||||
robot_routes,
|
||||
wrist_routes,
|
||||
work_plane_routes,
|
||||
)
|
||||
from .clients import G1StatusError
|
||||
from .config import load_config
|
||||
@@ -42,6 +43,7 @@ app.include_router(grasp_routes.router, prefix="/api/grasp", tags=["grasp"])
|
||||
app.include_router(wrist_routes.router, prefix="/api/wrist", tags=["wrist"])
|
||||
app.include_router(posture_routes.router, prefix="/api/posture", tags=["posture"])
|
||||
app.include_router(robot_routes.router, prefix="/api/robot", tags=["robot"])
|
||||
app.include_router(work_plane_routes.router, prefix="/api/work_plane", tags=["work_plane"])
|
||||
|
||||
WEB_DIR = Path(__file__).resolve().parents[2] / "web"
|
||||
if WEB_DIR.exists():
|
||||
@@ -101,7 +103,7 @@ def index() -> HTMLResponse:
|
||||
idx = WEB_DIR / "index.html"
|
||||
html = idx.read_text(encoding="utf-8")
|
||||
# Cache-bust app.js and style.css with their mtime so a rebuild forces refetch.
|
||||
for name in ("app.js", "servo.js", "style.css"):
|
||||
for name in ("app.js", "work_plane.js", "style.css"):
|
||||
f = WEB_DIR / name
|
||||
if f.exists():
|
||||
v = int(f.stat().st_mtime)
|
||||
|
||||
@@ -17,6 +17,7 @@ from ..clients import NuwaError, Sam21Error, Sam21Unavailable
|
||||
from ..timing import Stopwatch
|
||||
from .depth_align import align_depth_to_rgb
|
||||
from .depth_to_rgb import CameraRegistration
|
||||
from .work_plane import apply_below_plane_filter, get_work_plane
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
@@ -31,13 +32,17 @@ SAM21_BUG_CODES = frozenset({"SHAPE_MISMATCH", "BBOX_OUT_OF_BOUNDS",
|
||||
|
||||
|
||||
def points_via_sam21(nuwa, sam21, bbox: tuple[int, int, int, int],
|
||||
sw: Stopwatch) -> np.ndarray:
|
||||
sw: Stopwatch) -> tuple[np.ndarray, dict]:
|
||||
"""SAM21 path: bbox → segmented point cloud in RGB-camera/OpenCV frame.
|
||||
|
||||
Returns points in **meters**. Raises Sam21Fallback on failures that
|
||||
should trigger the bbox path. Re-raises HTTPException(502) for Nuwa
|
||||
failures (the bbox path would fail too — better to fail fast). All
|
||||
SAM21-specific logging happens here; callers do not log on fallback.
|
||||
|
||||
After the SAM21 segmentation the work-plane filter is applied (no-op if no
|
||||
plane is calibrated). Returns (points, plane_info) where plane_info has
|
||||
`plane_applied` and `n_points_before_plane`.
|
||||
"""
|
||||
try:
|
||||
with sw.step("nuwa_grab"):
|
||||
@@ -120,7 +125,12 @@ def points_via_sam21(nuwa, sam21, bbox: tuple[int, int, int, int],
|
||||
log.error("Unexpected SAM21 error: %r", e)
|
||||
raise Sam21Fallback(f"unexpected: {e!r}") from e
|
||||
|
||||
pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane())
|
||||
if pts_cv.shape[0] < 10:
|
||||
log.warning("SAM21 returned %d points (<10), falling back to bbox", pts_cv.shape[0])
|
||||
raise Sam21Fallback(f"too few points: {pts_cv.shape[0]}")
|
||||
return pts_cv
|
||||
log.warning("SAM21 cloud has %d points after plane filter (<10), falling back to bbox",
|
||||
pts_cv.shape[0])
|
||||
raise Sam21Fallback(
|
||||
f"too few points after plane filter: {pts_cv.shape[0]}"
|
||||
f" (before plane: {plane_info['n_points_before_plane']})"
|
||||
)
|
||||
return pts_cv, plane_info
|
||||
@@ -0,0 +1,146 @@
|
||||
"""Work-plane fit, persistence, and below-plane filter.
|
||||
|
||||
The work plane is fitted from a user-drawn rectangle on the RGB image. The
|
||||
fit uses SVD on the bbox depth cloud (after `nuwa_to_cv_frame`), so the
|
||||
plane normal lives in OpenCV RGB-camera frame and is oriented toward the
|
||||
camera (negative Z component in OpenCV convention).
|
||||
|
||||
The filter `apply_below_plane_filter` removes points more than `offset_mm`
|
||||
below the plane: keep `(normal · X + offset) >= -offset_mm/1000`. A
|
||||
positive `offset_mm` is permissive (preserves a buffer below the plane).
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import logging
|
||||
from dataclasses import asdict, dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..config import data_dir
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class WorkPlaneError(RuntimeError):
|
||||
"""Bad input to `fit_plane_svd` (too few points, degenerate cloud)."""
|
||||
|
||||
|
||||
@dataclass
|
||||
class WorkPlane:
|
||||
"""Plane in CV camera frame. `normal · X + offset = 0`; `normal` is
|
||||
unit-length and oriented toward the camera (negative Z in OpenCV)."""
|
||||
normal: list[float]
|
||||
offset: float
|
||||
centroid: list[float]
|
||||
n_inliers: int
|
||||
fit_rms_mm: float
|
||||
roi: list[int]
|
||||
offset_mm: float
|
||||
created_at: float
|
||||
|
||||
|
||||
_WORK_PLANE: WorkPlane | None = None
|
||||
_WORK_PLANE_LOADED = False
|
||||
|
||||
|
||||
def _work_plane_path() -> Path:
|
||||
return data_dir() / "work_plane.json"
|
||||
|
||||
|
||||
def _save_work_plane_to_disk(wp: WorkPlane | None) -> None:
|
||||
path = _work_plane_path()
|
||||
try:
|
||||
if wp is None:
|
||||
if path.exists():
|
||||
path.unlink()
|
||||
return
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_text(json.dumps(asdict(wp), indent=2))
|
||||
except Exception as e:
|
||||
log.warning("failed to persist work plane: %s", e)
|
||||
|
||||
|
||||
def _load_work_plane_from_disk() -> WorkPlane | None:
|
||||
path = _work_plane_path()
|
||||
try:
|
||||
with open(path, "r") as f:
|
||||
d = json.load(f)
|
||||
d.setdefault("offset_mm", 5.0) # legacy files
|
||||
return WorkPlane(**d)
|
||||
except FileNotFoundError:
|
||||
return None
|
||||
except Exception as e:
|
||||
log.warning("failed to load persisted work plane (%s)", e)
|
||||
return None
|
||||
|
||||
|
||||
def get_work_plane() -> WorkPlane | None:
|
||||
global _WORK_PLANE, _WORK_PLANE_LOADED
|
||||
if not _WORK_PLANE_LOADED:
|
||||
_WORK_PLANE = _load_work_plane_from_disk()
|
||||
_WORK_PLANE_LOADED = True
|
||||
return _WORK_PLANE
|
||||
|
||||
|
||||
def set_work_plane(wp: WorkPlane | None) -> None:
|
||||
global _WORK_PLANE, _WORK_PLANE_LOADED
|
||||
_WORK_PLANE = wp
|
||||
_WORK_PLANE_LOADED = True
|
||||
_save_work_plane_to_disk(wp)
|
||||
|
||||
|
||||
def fit_plane_svd(points_xyz: np.ndarray
|
||||
) -> tuple[np.ndarray, float, np.ndarray, float]:
|
||||
"""Least-squares plane via SVD. Returns (normal, offset, centroid, rms_mm).
|
||||
|
||||
Plane equation: `normal · X + offset = 0`. Normal is oriented toward
|
||||
the camera origin (so `signed_distance = normal · X + offset` is
|
||||
positive for points on the camera side of the plane)."""
|
||||
pts = np.asarray(points_xyz, dtype=np.float64)
|
||||
if pts.ndim != 2 or pts.shape[1] < 3:
|
||||
raise WorkPlaneError(f"unexpected points shape for plane fit: {pts.shape}")
|
||||
xyz = pts[:, :3]
|
||||
if len(xyz) < 10:
|
||||
raise WorkPlaneError(f"too few points to fit plane: {len(xyz)}")
|
||||
c = xyz.mean(axis=0)
|
||||
centered = xyz - c
|
||||
_, _, Vt = np.linalg.svd(centered, full_matrices=False)
|
||||
normal = Vt[-1]
|
||||
n = float(np.linalg.norm(normal))
|
||||
if n < 1e-9:
|
||||
raise WorkPlaneError("degenerate plane (zero normal)")
|
||||
normal = normal / n
|
||||
# Orient toward camera origin: signed distance of origin from plane must
|
||||
# be positive, i.e. `-n·c > 0`, i.e. `n·c < 0`.
|
||||
if float(normal @ c) > 0:
|
||||
normal = -normal
|
||||
offset = -float(normal @ c)
|
||||
signed = centered @ normal
|
||||
rms_mm = float(np.sqrt(np.mean(signed * signed)) * 1000.0)
|
||||
return normal, offset, c, rms_mm
|
||||
|
||||
|
||||
def apply_below_plane_filter(
|
||||
pts_cv: np.ndarray,
|
||||
wp: WorkPlane | None,
|
||||
) -> tuple[np.ndarray, dict]:
|
||||
"""Drop points more than `offset_mm` below the plane.
|
||||
|
||||
Returns `(filtered_pts, info)`. `info` always carries:
|
||||
plane_applied (bool): true iff a plane was provided AND there were
|
||||
points to filter
|
||||
n_points_before_plane (int): len(pts_cv) on entry
|
||||
"""
|
||||
info = {"plane_applied": False,
|
||||
"n_points_before_plane": int(len(pts_cv))}
|
||||
if wp is None or len(pts_cv) == 0:
|
||||
return pts_cv, info
|
||||
n_vec = np.asarray(wp.normal, dtype=np.float64)
|
||||
d_off = float(wp.offset)
|
||||
offset_m = float(wp.offset_mm) / 1000.0
|
||||
signed = pts_cv @ n_vec + d_off
|
||||
keep = signed >= -offset_m
|
||||
info["plane_applied"] = True
|
||||
return pts_cv[keep], info
|
||||
@@ -0,0 +1,142 @@
|
||||
"""Unit tests for perception.work_plane."""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from presa_mano.perception.work_plane import (
|
||||
WorkPlane,
|
||||
WorkPlaneError,
|
||||
apply_below_plane_filter,
|
||||
fit_plane_svd,
|
||||
_load_work_plane_from_disk,
|
||||
_save_work_plane_to_disk,
|
||||
_work_plane_path,
|
||||
)
|
||||
|
||||
|
||||
def _flat_plane_cloud(z: float = 1.0, n: int = 200, noise_m: float = 0.0):
|
||||
"""Cloud sampled on the plane z=z (camera frame, OpenCV: camera looks +Z)."""
|
||||
rng = np.random.default_rng(0)
|
||||
xy = rng.uniform(-0.2, 0.2, size=(n, 2))
|
||||
zs = np.full((n, 1), z) + rng.normal(0, noise_m, size=(n, 1))
|
||||
return np.hstack([xy, zs])
|
||||
|
||||
|
||||
def test_fit_plane_svd_horizontal_plane():
|
||||
"""Plane at z=1.0, normal must point toward camera (negative Z)."""
|
||||
pts = _flat_plane_cloud(z=1.0, n=200, noise_m=0.0)
|
||||
normal, offset, centroid, rms_mm = fit_plane_svd(pts)
|
||||
assert abs(normal[2]) > 0.99
|
||||
assert normal[2] < 0
|
||||
assert abs(offset - 1.0) < 1e-3
|
||||
assert rms_mm < 1.0
|
||||
|
||||
|
||||
def test_fit_plane_svd_raises_on_too_few_points():
|
||||
with pytest.raises(WorkPlaneError):
|
||||
fit_plane_svd(np.zeros((5, 3)))
|
||||
|
||||
|
||||
def test_apply_below_plane_filter_noop_without_plane():
|
||||
pts = _flat_plane_cloud(z=1.0)
|
||||
out, info = apply_below_plane_filter(pts, None)
|
||||
assert info["plane_applied"] is False
|
||||
assert info["n_points_before_plane"] == len(pts)
|
||||
assert out is pts
|
||||
|
||||
|
||||
def test_apply_below_plane_filter_keeps_above_drops_below():
|
||||
"""Plane at z=1.0, normal toward camera (-Z): points with z<1.0 are above."""
|
||||
above = _flat_plane_cloud(z=0.95, n=100)
|
||||
below = _flat_plane_cloud(z=1.10, n=100)
|
||||
pts = np.vstack([above, below])
|
||||
wp = WorkPlane(
|
||||
normal=[0.0, 0.0, -1.0], offset=1.0,
|
||||
centroid=[0.0, 0.0, 1.0],
|
||||
n_inliers=100, fit_rms_mm=0.0,
|
||||
roi=[0, 0, 10, 10],
|
||||
offset_mm=0.0,
|
||||
created_at=0.0,
|
||||
)
|
||||
out, info = apply_below_plane_filter(pts, wp)
|
||||
assert info["plane_applied"] is True
|
||||
assert info["n_points_before_plane"] == 200
|
||||
assert len(out) == 100
|
||||
assert np.all(out[:, 2] <= 1.0 + 1e-9)
|
||||
|
||||
|
||||
def test_apply_below_plane_filter_offset_buffer_keeps_below():
|
||||
"""offset_mm=50 keeps points up to 50mm below the plane."""
|
||||
above = _flat_plane_cloud(z=0.95, n=50)
|
||||
just_below = _flat_plane_cloud(z=1.03, n=50)
|
||||
far_below = _flat_plane_cloud(z=1.20, n=50)
|
||||
pts = np.vstack([above, just_below, far_below])
|
||||
wp = WorkPlane(
|
||||
normal=[0.0, 0.0, -1.0], offset=1.0,
|
||||
centroid=[0.0, 0.0, 1.0],
|
||||
n_inliers=50, fit_rms_mm=0.0,
|
||||
roi=[0, 0, 10, 10],
|
||||
offset_mm=50.0,
|
||||
created_at=0.0,
|
||||
)
|
||||
out, info = apply_below_plane_filter(pts, wp)
|
||||
assert info["plane_applied"] is True
|
||||
assert len(out) == 100 # above (50) + just_below (50); far_below dropped
|
||||
|
||||
|
||||
def test_apply_below_plane_filter_empty_input():
|
||||
wp = WorkPlane(
|
||||
normal=[0.0, 0.0, -1.0], offset=1.0,
|
||||
centroid=[0.0, 0.0, 1.0],
|
||||
n_inliers=10, fit_rms_mm=0.0,
|
||||
roi=[0, 0, 10, 10],
|
||||
offset_mm=5.0, created_at=0.0,
|
||||
)
|
||||
out, info = apply_below_plane_filter(np.empty((0, 3)), wp)
|
||||
assert info["plane_applied"] is False # short-circuit: empty input never "ran"
|
||||
assert info["n_points_before_plane"] == 0
|
||||
assert len(out) == 0
|
||||
|
||||
|
||||
def test_work_plane_disk_roundtrip(tmp_path, monkeypatch):
|
||||
"""Save then load preserves all fields including offset_mm."""
|
||||
monkeypatch.setenv("PRESA_MANO_DATA", str(tmp_path))
|
||||
wp = WorkPlane(
|
||||
normal=[0.0, 0.0, -1.0], offset=1.0,
|
||||
centroid=[0.1, 0.2, 1.0],
|
||||
n_inliers=42, fit_rms_mm=1.5,
|
||||
roi=[10, 20, 100, 200],
|
||||
offset_mm=7.5,
|
||||
created_at=1234567890.0,
|
||||
)
|
||||
_save_work_plane_to_disk(wp)
|
||||
loaded = _load_work_plane_from_disk()
|
||||
assert loaded is not None
|
||||
assert loaded.normal == wp.normal
|
||||
assert loaded.offset == wp.offset
|
||||
assert loaded.centroid == wp.centroid
|
||||
assert loaded.n_inliers == wp.n_inliers
|
||||
assert loaded.fit_rms_mm == wp.fit_rms_mm
|
||||
assert loaded.roi == wp.roi
|
||||
assert loaded.offset_mm == 7.5
|
||||
assert loaded.created_at == wp.created_at
|
||||
|
||||
|
||||
def test_work_plane_load_legacy_file_defaults_offset(tmp_path, monkeypatch):
|
||||
"""A pre-existing work_plane.json without offset_mm gets the 5.0 default."""
|
||||
monkeypatch.setenv("PRESA_MANO_DATA", str(tmp_path))
|
||||
path = _work_plane_path()
|
||||
legacy = {
|
||||
"normal": [0.0, 0.0, -1.0], "offset": 1.0,
|
||||
"centroid": [0.0, 0.0, 1.0],
|
||||
"n_inliers": 10, "fit_rms_mm": 1.0,
|
||||
"roi": [0, 0, 10, 10],
|
||||
"created_at": 0.0,
|
||||
}
|
||||
path.write_text(json.dumps(legacy))
|
||||
loaded = _load_work_plane_from_disk()
|
||||
assert loaded is not None
|
||||
assert loaded.offset_mm == 5.0
|
||||
+60
-95
@@ -13,8 +13,8 @@
|
||||
<button class="tab active" data-tab="home">Home</button>
|
||||
<button class="tab" data-tab="agent">Assistente AI</button>
|
||||
<button class="tab" data-tab="ai-settings">Impostazioni AI</button>
|
||||
<button class="tab" data-tab="grasp">TrovaUtensile</button>
|
||||
<button class="tab" data-tab="servo">Servo Visuale</button>
|
||||
<button class="tab" data-tab="workplane">Calibrazione Piano</button>
|
||||
<button class="tab" data-tab="grasp">Cerca modello</button>
|
||||
<button class="tab" data-tab="view3d">Vista 3D</button>
|
||||
<button class="tab" data-tab="ee">Movimento EE</button>
|
||||
</nav>
|
||||
@@ -174,6 +174,56 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== CALIBRAZIONE PIANO ============================== -->
|
||||
<section class="panel" id="tab-workplane">
|
||||
<div class="diag3d-toolbar">
|
||||
<span class="post-health" id="wpStatusChip">—</span>
|
||||
</div>
|
||||
<div class="grid-2 diag3d-layout">
|
||||
<div>
|
||||
<div id="wpViewport" style="position:relative;width:min(100%,720px);height:440px;overflow:hidden;background:#000;border:1px solid var(--border);border-radius:4px;cursor:grab;user-select:none">
|
||||
<div id="wpPanZoom" style="position:absolute;left:0;top:0;transform-origin:0 0;will-change:transform">
|
||||
<img id="wpImg" alt="Nuwa RGB" style="display:block;max-width:none;pointer-events:none" draggable="false" />
|
||||
<canvas id="wpOverlay" style="position:absolute;left:0;top:0;pointer-events:none"></canvas>
|
||||
</div>
|
||||
</div>
|
||||
<div class="marker-info">RGB live · shift+drag = ROI</div>
|
||||
<h3 style="margin-top:14px">Vista 3D (frame camera)</h3>
|
||||
<div class="canvas-wrap" style="width:min(100%,720px)">
|
||||
<canvas id="wp3dCanvas" width="720" height="360" style="display:block;background:#0a0d11;border:1px solid var(--border);border-radius:4px"></canvas>
|
||||
</div>
|
||||
<div class="marker-info">piano viola · verde = inlier · giallo = sopra piano+offset · rosso = sotto piano-offset · grigio = buffer · drag = ruota · wheel = zoom</div>
|
||||
</div>
|
||||
<div>
|
||||
<h2>Calibrazione</h2>
|
||||
<div class="ctrl-block">
|
||||
<div class="info-box">
|
||||
Disegna con shift+drag un rettangolo sul tavolo (senza oggetti), poi
|
||||
clicca <strong>Calcola piano</strong>. Regola l'offset finché i punti
|
||||
gialli rappresentano l'oggetto e i rossi spariscono. Poi <strong>Salva piano</strong>.
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>ROI <span id="wpRoiInfo" class="dim">— · disegna con shift+drag</span></label>
|
||||
<label>offset (mm) <input type="number" id="wpOffset" value="5.0" step="0.5" min="0" max="200" /></label>
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="wpFitBtn" class="primary" disabled>Calcola piano (preview)</button>
|
||||
<button id="wpSaveBtn" disabled>Salva piano</button>
|
||||
<button id="wpClearBtn" class="danger" disabled>Rimuovi piano salvato</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<h2>Stato</h2>
|
||||
<div id="wpStateBlock" class="ctrl-block">
|
||||
<div class="dim">nessun piano</div>
|
||||
</div>
|
||||
|
||||
<h2>Dettagli piano</h2>
|
||||
<pre id="wpDetails" class="log small">—</pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== TROVA UTENSILE ============================== -->
|
||||
<section class="panel" id="tab-grasp">
|
||||
<div class="grid-2">
|
||||
@@ -215,103 +265,18 @@
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<h3>Precisione (ultime <span id="statsWindow">5</span> detection)</h3>
|
||||
<table class="stats" id="statsTable">
|
||||
<thead>
|
||||
<tr><th>asse</th><th>media [m]</th><th>σ [mm]</th><th>range [mm]</th><th>min</th><th>max</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<!-- Hidden DOM hooks consumed by app.js (Precisione + Dettagli UI removed). -->
|
||||
<div style="display:none">
|
||||
<span id="statsWindow">5</span>
|
||||
<table id="statsTable"><tbody>
|
||||
<tr><td>x</td><td>—</td><td>—</td><td>—</td><td>—</td><td>—</td></tr>
|
||||
<tr><td>y</td><td>—</td><td>—</td><td>—</td><td>—</td><td>—</td></tr>
|
||||
<tr><td>z</td><td>—</td><td>—</td><td>—</td><td>—</td><td>—</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<div id="statsMeta" class="marker-info">loop: inattivo</div>
|
||||
|
||||
<h3>Dettagli</h3>
|
||||
<pre id="graspOut" class="log">(nessuna detection)</pre>
|
||||
</tbody></table>
|
||||
<div id="statsMeta"></div>
|
||||
<pre id="graspOut"></pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== SERVO VISUALE ============================== -->
|
||||
<section class="panel" id="tab-servo">
|
||||
<div class="diag3d-toolbar">
|
||||
<span class="post-health" id="servoStatus">—</span>
|
||||
</div>
|
||||
<div class="grid-2 diag3d-layout">
|
||||
<div>
|
||||
<div id="servoViewport" style="position:relative;width:min(100%,720px);height:440px;overflow:hidden;background:#000;border:1px solid var(--border);border-radius:4px;cursor:grab;user-select:none">
|
||||
<div id="servoPanZoom" style="position:absolute;left:0;top:0;transform-origin:0 0;will-change:transform">
|
||||
<img id="servoImg" alt="Nuwa RGB" style="display:block;max-width:none;pointer-events:none" draggable="false" />
|
||||
<canvas id="servoOverlay" style="position:absolute;left:0;top:0;pointer-events:none"></canvas>
|
||||
</div>
|
||||
</div>
|
||||
<div class="marker-info">RGB live · ciano = marker · giallo = oggetto · wheel zoom · drag pan · doppio click reset</div>
|
||||
<h3 style="margin-top:14px">Vista 3D (frame camera)</h3>
|
||||
<div class="canvas-wrap" style="width:min(100%,720px)">
|
||||
<canvas id="servo3dCanvas" width="720" height="360" style="display:block;background:#0a0d11;border:1px solid var(--border);border-radius:4px"></canvas>
|
||||
</div>
|
||||
<div class="marker-info">origine = camera · piano viola · marker ciano · oggetto giallo · proiezioni più chiare · drag ruota · wheel zoom</div>
|
||||
<div class="diag3d-legend">
|
||||
<div><span class="legend-sw" style="background:#4dd0e1;border-radius:50%"></span> marker polso</div>
|
||||
<div><span class="legend-sw" style="background:#ffd54f;border-radius:50%"></span> oggetto (centro bbox)</div>
|
||||
</div>
|
||||
</div>
|
||||
<div>
|
||||
<h2>Parametri</h2>
|
||||
<div class="ctrl-block">
|
||||
<div class="info-box">
|
||||
Avvicinamento marker-relativo senza hand-eye calibration: la camera vede marker e oggetto nello stesso frame, ogni step sposta il polso della frazione indicata della distanza rimanente. Si ferma a <code>min_distance_m</code>.
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>Classe (filtro) <input type="text" id="svClass" placeholder="(tutte)" /></label>
|
||||
<label>Confidenza <input type="number" id="svConf" value="0.5" step="0.05" min="0" max="1" /></label>
|
||||
<label>min dist (mm) <input type="number" id="svMinDist" value="100" step="10" min="20" max="500" /></label>
|
||||
<label>frazione step <input type="number" id="svStepFrac" value="0.5" step="0.05" min="0.05" max="1.0" /></label>
|
||||
<label>timeout (s) <input type="number" id="svTimeout" value="5" step="0.5" min="1" max="30" /></label>
|
||||
<label>side
|
||||
<select id="svSide">
|
||||
<option value="">(config)</option>
|
||||
<option value="right">right</option>
|
||||
<option value="left">left</option>
|
||||
</select>
|
||||
</label>
|
||||
<label class="inline"><input type="checkbox" id="svDryRun" checked /> dry-run (nessun moto reale)</label>
|
||||
</div>
|
||||
<div class="info-box" style="margin-top:8px">
|
||||
<strong>Orientazione marker rispetto al polso</strong> (rvec in gradi). Se non allineato al frame polso il target risulta spostato: tuna finché marker, oggetto e target sono coerenti.
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>rx (°) <input type="number" id="svRx" value="0" step="5" min="-180" max="180" /></label>
|
||||
<label>ry (°) <input type="number" id="svRy" value="0" step="5" min="-180" max="180" /></label>
|
||||
<label>rz (°) <input type="number" id="svRz" value="0" step="5" min="-180" max="180" /></label>
|
||||
<button id="svSaveRvecBtn" title="Salva l'rvec corrente in config.yaml">Salva rvec in config</button>
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="svPlanBtn" class="primary">Rileva / ricalcola</button>
|
||||
<button id="svStepBtn">▶ Vai al prossimo</button>
|
||||
<button id="svResetBtn">Reset</button>
|
||||
</div>
|
||||
<div class="info-box" style="margin-top:8px">
|
||||
<strong>Piano di lavoro</strong>. Clicca "Seleziona area", poi traccia un rettangolo sull'immagine con shift+drag: il backend calcola il piano dalla depth in quell'area. Marker e oggetto vengono poi proiettati su questo piano.
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="svPlaneSelBtn">Seleziona area</button>
|
||||
<button id="svPlaneFitBtn">Calcola piano</button>
|
||||
<button id="svPlaneClearBtn" class="danger">Rimuovi piano</button>
|
||||
</div>
|
||||
<div id="svPlaneInfo" class="marker-info">piano: —</div>
|
||||
</div>
|
||||
|
||||
<h2>Stato</h2>
|
||||
<div id="servoStats" class="ctrl-block">
|
||||
<div class="dim">nessun piano calcolato</div>
|
||||
</div>
|
||||
|
||||
<h2>Dettagli piano</h2>
|
||||
<pre id="servoResult" class="log small">—</pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
@@ -419,6 +384,6 @@
|
||||
}
|
||||
</script>
|
||||
<script src="/static/app.js"></script>
|
||||
<script type="module" src="/static/servo.js"></script>
|
||||
<script type="module" src="/static/work_plane.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
-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(); });
|
||||
@@ -0,0 +1,285 @@
|
||||
// Calibrazione Piano tab: shift+drag ROI on RGB, fit preview, offset
|
||||
// slider, save / clear, and a 3D scatter with the plane mesh.
|
||||
|
||||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
|
||||
|
||||
const $ = (id) => document.getElementById(id);
|
||||
const RGB_REFRESH_MS = 1000;
|
||||
|
||||
let roi = null; // {u0,v0,u1,v1} in image pixel coords
|
||||
let previewPlane = null; // last response.plane (preview, not saved)
|
||||
let previewPoints = null; // list of [x,y,z] in CV camera frame, meters
|
||||
let savedPlane = null; // last GET /api/work_plane response.plane
|
||||
|
||||
function setChip(text, level) {
|
||||
const el = $('wpStatusChip');
|
||||
if (!el) return;
|
||||
el.textContent = text;
|
||||
el.className = 'post-health ' + (level || '');
|
||||
}
|
||||
|
||||
function setRoiInfo() {
|
||||
const el = $('wpRoiInfo');
|
||||
if (!roi) { el.textContent = '— · disegna con shift+drag'; el.classList.add('dim'); return; }
|
||||
el.classList.remove('dim');
|
||||
el.textContent = `u=${roi.u0}…${roi.u1} v=${roi.v0}…${roi.v1} (${roi.u1 - roi.u0}×${roi.v1 - roi.v0} px)`;
|
||||
}
|
||||
|
||||
function updateButtonStates() {
|
||||
$('wpFitBtn').disabled = !roi;
|
||||
$('wpSaveBtn').disabled = !previewPlane;
|
||||
$('wpClearBtn').disabled = !savedPlane;
|
||||
}
|
||||
|
||||
function rgbUrl() {
|
||||
// Nuwa is reachable on host network on port 8877; the GUI is same-origin to
|
||||
// presa-mano (8890). Use the upstream host directly.
|
||||
const host = window.location.hostname;
|
||||
return `http://${host}:8877/api/rgb?t=${Date.now()}`;
|
||||
}
|
||||
|
||||
function startRgbLoop() {
|
||||
const tick = () => { $('wpImg').src = rgbUrl(); };
|
||||
tick();
|
||||
setInterval(tick, RGB_REFRESH_MS);
|
||||
}
|
||||
|
||||
// ---------------- ROI drawing on the RGB image ----------------
|
||||
(function setupRoiDrawing() {
|
||||
const viewport = $('wpViewport');
|
||||
const img = $('wpImg');
|
||||
const overlay = $('wpOverlay');
|
||||
let drawing = null;
|
||||
|
||||
function imgPixelFromEvent(e) {
|
||||
const rect = img.getBoundingClientRect();
|
||||
const sx = img.naturalWidth / rect.width;
|
||||
const sy = img.naturalHeight / rect.height;
|
||||
return {
|
||||
u: Math.round((e.clientX - rect.left) * sx),
|
||||
v: Math.round((e.clientY - rect.top) * sy),
|
||||
};
|
||||
}
|
||||
|
||||
function syncOverlaySize() {
|
||||
overlay.width = img.naturalWidth || 640;
|
||||
overlay.height = img.naturalHeight || 480;
|
||||
overlay.style.width = img.clientWidth + 'px';
|
||||
overlay.style.height = img.clientHeight + 'px';
|
||||
}
|
||||
img.addEventListener('load', syncOverlaySize);
|
||||
|
||||
function drawOverlay() {
|
||||
const ctx = overlay.getContext('2d');
|
||||
ctx.clearRect(0, 0, overlay.width, overlay.height);
|
||||
if (roi) {
|
||||
ctx.strokeStyle = '#9b6dff';
|
||||
ctx.lineWidth = 2;
|
||||
ctx.strokeRect(roi.u0, roi.v0, roi.u1 - roi.u0, roi.v1 - roi.v0);
|
||||
}
|
||||
}
|
||||
|
||||
viewport.addEventListener('mousedown', (e) => {
|
||||
if (!e.shiftKey) return; // shift+drag = ROI; plain drag = pan (no-op here)
|
||||
const p = imgPixelFromEvent(e);
|
||||
drawing = { u0: p.u, v0: p.v, u1: p.u, v1: p.v };
|
||||
e.preventDefault();
|
||||
});
|
||||
viewport.addEventListener('mousemove', (e) => {
|
||||
if (!drawing) return;
|
||||
const p = imgPixelFromEvent(e);
|
||||
drawing.u1 = p.u;
|
||||
drawing.v1 = p.v;
|
||||
roi = {
|
||||
u0: Math.min(drawing.u0, drawing.u1),
|
||||
v0: Math.min(drawing.v0, drawing.v1),
|
||||
u1: Math.max(drawing.u0, drawing.u1),
|
||||
v1: Math.max(drawing.v0, drawing.v1),
|
||||
};
|
||||
drawOverlay();
|
||||
setRoiInfo();
|
||||
updateButtonStates();
|
||||
});
|
||||
viewport.addEventListener('mouseup', () => { drawing = null; });
|
||||
viewport.addEventListener('mouseleave', () => { drawing = null; });
|
||||
})();
|
||||
|
||||
// ---------------- Three.js 3D viewer ----------------
|
||||
const three = (function setupThree() {
|
||||
const canvas = $('wp3dCanvas');
|
||||
const scene = new THREE.Scene();
|
||||
const camera = new THREE.PerspectiveCamera(45, canvas.width / canvas.height, 0.01, 20);
|
||||
camera.position.set(0.5, -0.5, -1.2);
|
||||
camera.lookAt(0, 0, 1);
|
||||
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
renderer.setSize(canvas.width, canvas.height);
|
||||
const controls = new OrbitControls(camera, canvas);
|
||||
scene.add(new THREE.AxesHelper(0.1));
|
||||
const planeMesh = new THREE.Mesh(
|
||||
new THREE.PlaneGeometry(0.4, 0.4),
|
||||
new THREE.MeshBasicMaterial({ color: 0x9b6dff, transparent: true, opacity: 0.3, side: THREE.DoubleSide }),
|
||||
);
|
||||
planeMesh.visible = false;
|
||||
scene.add(planeMesh);
|
||||
let scatter = null;
|
||||
|
||||
function render() { renderer.render(scene, camera); }
|
||||
(function loop() { controls.update(); render(); requestAnimationFrame(loop); })();
|
||||
|
||||
function setPlane(plane) {
|
||||
if (!plane) { planeMesh.visible = false; return; }
|
||||
const c = plane.centroid;
|
||||
const n = plane.normal;
|
||||
planeMesh.position.set(c[0], c[1], c[2]);
|
||||
// Align plane to its normal.
|
||||
planeMesh.lookAt(c[0] + n[0], c[1] + n[1], c[2] + n[2]);
|
||||
planeMesh.visible = true;
|
||||
}
|
||||
|
||||
function setScatter(pointsCv, plane, offsetMm) {
|
||||
if (scatter) { scene.remove(scatter); scatter.geometry.dispose(); scatter.material.dispose(); scatter = null; }
|
||||
if (!pointsCv || pointsCv.length === 0) return;
|
||||
const n = pointsCv.length;
|
||||
const positions = new Float32Array(n * 3);
|
||||
const colors = new Float32Array(n * 3);
|
||||
const nx = plane ? plane.normal[0] : 0;
|
||||
const ny = plane ? plane.normal[1] : 0;
|
||||
const nz = plane ? plane.normal[2] : 0;
|
||||
const d = plane ? plane.offset : 0;
|
||||
const offsetM = (offsetMm || 0) / 1000.0;
|
||||
const rmsM = plane ? (plane.fit_rms_mm / 1000.0) * 2.0 : 0.01;
|
||||
for (let i = 0; i < n; i++) {
|
||||
const x = pointsCv[i][0], y = pointsCv[i][1], z = pointsCv[i][2];
|
||||
positions[3 * i] = x; positions[3 * i + 1] = y; positions[3 * i + 2] = z;
|
||||
const signed = plane ? (nx * x + ny * y + nz * z + d) : 0;
|
||||
let r = 0.5, g = 0.5, b = 0.5;
|
||||
if (plane) {
|
||||
if (signed > offsetM) { r = 1.0; g = 0.83; b = 0.31; } // yellow: above plane+offset
|
||||
else if (signed < -offsetM) { r = 0.93; g = 0.27; b = 0.27; } // red: below plane-offset
|
||||
else if (Math.abs(signed) <= rmsM) { r = 0.30; g = 0.78; b = 0.40; } // green: inlier
|
||||
else { r = 0.55; g = 0.55; b = 0.55; } // grey: buffer
|
||||
}
|
||||
colors[3 * i] = r; colors[3 * i + 1] = g; colors[3 * i + 2] = b;
|
||||
}
|
||||
const geom = new THREE.BufferGeometry();
|
||||
geom.setAttribute('position', new THREE.BufferAttribute(positions, 3));
|
||||
geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
|
||||
const mat = new THREE.PointsMaterial({ size: 0.005, vertexColors: true });
|
||||
scatter = new THREE.Points(geom, mat);
|
||||
scene.add(scatter);
|
||||
}
|
||||
|
||||
return { setPlane, setScatter };
|
||||
})();
|
||||
|
||||
// ---------------- State + button handlers ----------------
|
||||
async function fetchSaved() {
|
||||
try {
|
||||
const r = await fetch('/api/work_plane').then((r) => r.json());
|
||||
savedPlane = r.plane || null;
|
||||
} catch (e) {
|
||||
savedPlane = null;
|
||||
}
|
||||
if (savedPlane && !previewPlane) {
|
||||
three.setPlane(savedPlane);
|
||||
renderDetails(savedPlane);
|
||||
$('wpStateBlock').innerHTML = `piano salvato · RMS ${savedPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${savedPlane.n_inliers} · offset ${savedPlane.offset_mm} mm`;
|
||||
setChip('piano salvato', 'ok');
|
||||
} else if (!savedPlane && !previewPlane) {
|
||||
$('wpStateBlock').innerHTML = '<div class="dim">nessun piano</div>';
|
||||
setChip('nessun piano');
|
||||
}
|
||||
updateButtonStates();
|
||||
}
|
||||
|
||||
function renderDetails(plane) {
|
||||
$('wpDetails').textContent = JSON.stringify(plane, null, 2);
|
||||
}
|
||||
|
||||
async function doFit() {
|
||||
if (!roi) return;
|
||||
setChip('fit…');
|
||||
$('wpFitBtn').disabled = true;
|
||||
try {
|
||||
const body = {
|
||||
u_min: roi.u0, v_min: roi.v0, u_max: roi.u1, v_max: roi.v1,
|
||||
offset_mm: parseFloat($('wpOffset').value || '5'),
|
||||
};
|
||||
const r = await fetch('/api/work_plane/fit', {
|
||||
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(body),
|
||||
}).then((r) => r.json());
|
||||
if (r.detail) throw new Error(r.detail);
|
||||
previewPlane = r.plane;
|
||||
previewPoints = r.points_camera;
|
||||
three.setPlane(previewPlane);
|
||||
three.setScatter(previewPoints, previewPlane, previewPlane.offset_mm);
|
||||
renderDetails(previewPlane);
|
||||
$('wpStateBlock').innerHTML = `<strong>preview</strong> · RMS ${previewPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${previewPlane.n_inliers} · offset ${previewPlane.offset_mm} mm · <span class="warn">non salvato</span>`;
|
||||
setChip('preview pronto', 'ok');
|
||||
} catch (e) {
|
||||
setChip('fit failed: ' + e.message, 'err');
|
||||
} finally {
|
||||
updateButtonStates();
|
||||
}
|
||||
}
|
||||
|
||||
async function doSave() {
|
||||
if (!previewPlane) return;
|
||||
setChip('save…');
|
||||
try {
|
||||
const r = await fetch('/api/work_plane/save', {
|
||||
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify({ plane: previewPlane }),
|
||||
}).then((r) => r.json());
|
||||
if (r.detail) throw new Error(r.detail);
|
||||
savedPlane = r.plane;
|
||||
previewPlane = null;
|
||||
$('wpStateBlock').innerHTML = `piano salvato · RMS ${savedPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${savedPlane.n_inliers} · offset ${savedPlane.offset_mm} mm`;
|
||||
renderDetails(savedPlane);
|
||||
setChip('piano salvato', 'ok');
|
||||
} catch (e) {
|
||||
setChip('save failed: ' + e.message, 'err');
|
||||
} finally {
|
||||
updateButtonStates();
|
||||
}
|
||||
}
|
||||
|
||||
async function doClear() {
|
||||
if (!confirm('Rimuovere il piano salvato?')) return;
|
||||
setChip('clear…');
|
||||
try {
|
||||
await fetch('/api/work_plane', { method: 'DELETE' }).then((r) => r.json());
|
||||
savedPlane = null;
|
||||
previewPlane = null;
|
||||
previewPoints = null;
|
||||
three.setPlane(null);
|
||||
three.setScatter(null);
|
||||
$('wpStateBlock').innerHTML = '<div class="dim">nessun piano</div>';
|
||||
$('wpDetails').textContent = '—';
|
||||
setChip('nessun piano');
|
||||
} catch (e) {
|
||||
setChip('clear failed: ' + e.message, 'err');
|
||||
} finally {
|
||||
updateButtonStates();
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------- Offset slider: re-color in real time ----------------
|
||||
$('wpOffset').addEventListener('input', () => {
|
||||
if (previewPlane && previewPoints) {
|
||||
const off = parseFloat($('wpOffset').value || '0');
|
||||
previewPlane.offset_mm = off;
|
||||
three.setScatter(previewPoints, previewPlane, off);
|
||||
$('wpStateBlock').innerHTML = `<strong>preview</strong> · RMS ${previewPlane.fit_rms_mm.toFixed(1)} mm · n_inliers ${previewPlane.n_inliers} · offset ${previewPlane.offset_mm.toFixed(1)} mm · <span class="warn">non salvato</span>`;
|
||||
}
|
||||
});
|
||||
|
||||
$('wpFitBtn').addEventListener('click', doFit);
|
||||
$('wpSaveBtn').addEventListener('click', doSave);
|
||||
$('wpClearBtn').addEventListener('click', doClear);
|
||||
|
||||
// Initial load.
|
||||
startRgbLoop();
|
||||
fetchSaved();
|
||||
Reference in new issue
Block a user