Adriano/Presa-Mano
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor: remove visual-servo feature

Browse Source 
The marker-relative servo (endpoints /api/grasp/visual_servo/{plan,step,
plane/*}, helpers in grasp_routes.py, the bulk of grasp/visual_servo.py,
web/servo.js, and the Servo Visuale GUI tab) is gone. Plane fitting now
lives in /api/work_plane/* and perception/work_plane.py; Task 5 wires the
new Calibrazione Piano tab.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed 2026-05-11 17:04:00 +02:00
1 parent 565c3df2f4
commit eb53f30e23
5 files changed
+3 -1455
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

src/presa_mano/grasp/visual_servo.py
-504
View File
@@ -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,
)