feat: initial commit — orchestratore presa-mano con AI assistant + dual-robot

Pipeline completa per calibrazione hand-eye e grasp planning su robot
Unitree G1/H1 con camera depth Nuwa-HP60C. Esposta come FastAPI + GUI vanilla
JS, con assistente AI (OpenRouter + tool calling curato) per pilotare il
sistema in linguaggio naturale.

Architettura:
- FastAPI orchestrator (porta da .env, default 8890) che coordina 4
  microservizi upstream: Nuwa-HP60C, marker-finder, YOLO, robot bridge.
- Dual-robot: services.g1 e services.h1 in config.yaml; switchabili a
  runtime via PUT /api/home/robot.
- Calibrazione hand-eye eye-to-hand split per-side (right/left).
- Marker pose: marker-finder riceve gli intrinseci RGB della Nuwa al boot,
  PnP server-side con quei dati. Nessun re-PnP locale richiesto.
- Depth-to-RGB: registrazione iterativa IR↔RGB per back-projection 3D
  corretta (Nuwa /api/measure usa coords IR; il nostro /api/perception/
  measure_rgb prende pixel RGB e ritorna 3D nel frame RGB OpenCV).
- Correzione affine sul depth bias (calibrabile via marker come
  ground-truth) — riduce errore Z da ~6% a ~1.5% nei test.
- AI assistant: 16 tool curati (move_wrist_*, hand_action, get_marker[_3d],
  detect_object, calibration_*, set_active_robot…), system prompt
  dinamico che riflette robot attivo + marker config + addendum
  customizzabile da GUI. Temperature di default 0.1 per evitare invenzioni.

GUI (7 tab):
- Home: stato comunicazione device + selettore robot attivo G1/H1
- Assistente AI: chat con OpenRouter, history persistita lato browser,
  task riutilizzabili (prompt salvati), trace tool calls
- Impostazioni AI: modello da combo curata, temperature, max iterations,
  prompt addendum, anteprima system prompt
- Calibrazione, Auto-Calibrazione, TrovaUtensile, Servo Visuale, Vista 3D,
  Movimento EE — invariate dalle versioni precedenti

Setup:
- Variabili in presa-mano/.env (template in .env.example)
- OPENROUTER_API_KEY richiesta solo per il tab Assistente AI
- PRESA_MANO_PORT configurabile

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

src/presa_mano/grasp/__init__.py

@@ -0,0 +1,4 @@
+from .axis import ObjectAxes, compute_axes
+from .planner import GraspPlanner, GraspPlan
+
+__all__ = ["ObjectAxes", "compute_axes", "GraspPlanner", "GraspPlan"]

src/presa_mano/grasp/axis.py

@@ -0,0 +1,63 @@
+"""Object axis estimation via PCA.
+
+Given Nx3 object points (in any frame), compute:
+  - centroid
+  - 3 principal axes (unit vectors) with corresponding extents
+  - right-handed frame (axes columns form a rotation matrix)
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass
+class ObjectAxes:
+    centroid: np.ndarray              # (3,)
+    axes: np.ndarray                  # (3,3) columns = unit vectors, sorted largest extent first
+    extents: np.ndarray               # (3,) along each axis (meters)
+
+    @property
+    def long_axis(self) -> np.ndarray:
+        return self.axes[:, 0]
+
+    @property
+    def mid_axis(self) -> np.ndarray:
+        return self.axes[:, 1]
+
+    @property
+    def short_axis(self) -> np.ndarray:
+        return self.axes[:, 2]
+
+    def to_dict(self) -> dict:
+        return {
+            "centroid": self.centroid.tolist(),
+            "axes": self.axes.tolist(),
+            "extents_m": self.extents.tolist(),
+        }
+
+
+def compute_axes(points_xyz: np.ndarray) -> ObjectAxes:
+    pts = np.asarray(points_xyz, dtype=np.float64)
+    if pts.ndim != 2 or pts.shape[1] != 3 or len(pts) < 3:
+        raise ValueError(f"need at least 3 3D points, got shape {pts.shape}")
+
+    c = pts.mean(axis=0)
+    centered = pts - c
+    cov = np.cov(centered.T)
+    eigvals, eigvecs = np.linalg.eigh(cov)
+    # eigh returns ascending — reverse for largest-first
+    order = np.argsort(eigvals)[::-1]
+    eigvals = eigvals[order]
+    eigvecs = eigvecs[:, order]
+
+    # Ensure right-handed coordinate system
+    if np.linalg.det(eigvecs) < 0:
+        eigvecs[:, 2] = -eigvecs[:, 2]
+
+    # Extent along each axis = range of projected coordinates
+    proj = centered @ eigvecs  # Nx3
+    extents = proj.max(axis=0) - proj.min(axis=0)
+    return ObjectAxes(centroid=c, axes=eigvecs, extents=extents)

src/presa_mano/grasp/planner.py

@@ -0,0 +1,99 @@
+"""Grasp planner: from object axes (in base frame) to a wrist pose.
+
+Gripper convention (G1 right hand, approximate):
+  - X_hand: forward (approach direction — palm-forward)
+  - Y_hand: lateral (axis along which fingers close)
+  - Z_hand: up (along the fingers)
+
+Strategy:
+  - The fingers close along the *mid* axis of the object (perpendicular to its
+    long axis, largest pinch span).
+  - The approach direction points downward toward the object centroid from above
+    (so the wrist comes from +Z in base frame).
+  - We build R_wrist = [x_hand | y_hand | z_hand].
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any
+
+import numpy as np
+
+from ..geometry import R_to_quat_xyzw
+from .axis import ObjectAxes
+
+
+@dataclass
+class GraspPlan:
+    grasp_position: np.ndarray         # (3,) in base frame
+    grasp_quaternion: np.ndarray       # (4,) xyzw
+    pre_grasp_position: np.ndarray     # (3,) above the grasp by approach_offset
+    object_width_m: float              # expected span along closure axis
+    axes: ObjectAxes
+
+    def to_dict(self) -> dict[str, Any]:
+        return {
+            "grasp": {
+                "position": self.grasp_position.tolist(),
+                "quaternion_xyzw": self.grasp_quaternion.tolist(),
+            },
+            "pre_grasp": {
+                "position": self.pre_grasp_position.tolist(),
+                "quaternion_xyzw": self.grasp_quaternion.tolist(),
+            },
+            "object_width_m": float(self.object_width_m),
+            "axes": self.axes.to_dict(),
+        }
+
+
+class GraspPlanner:
+    def __init__(self, approach_offset_m: float = 0.10, max_object_width_m: float = 0.08):
+        self.approach_offset_m = approach_offset_m
+        self.max_object_width_m = max_object_width_m
+
+    def plan(self, axes: ObjectAxes) -> GraspPlan:
+        long_axis = _unit(axes.long_axis)
+        # Closure axis = component of the mid axis orthogonal to long_axis, then horizontalize.
+        closure = _unit(axes.mid_axis - np.dot(axes.mid_axis, long_axis) * long_axis)
+
+        # Approach comes from above in base frame: z_base = +Z up.
+        z_base = np.array([0.0, 0.0, 1.0])
+        approach = -z_base  # hand approaches downward
+
+        # Build hand frame so that:
+        #   x_hand = approach direction (forward)
+        #   y_hand = closure direction (fingers close)
+        #   z_hand = x × y  (along fingers / wrist up)
+        x_hand = _unit(approach)
+        # Remove any component of closure along x_hand
+        y_hand = _unit(closure - np.dot(closure, x_hand) * x_hand)
+        if np.linalg.norm(y_hand) < 1e-6:
+            # Fall back: pick any horizontal axis perpendicular to approach
+            y_hand = _unit(np.cross(x_hand, np.array([1.0, 0.0, 0.0])))
+        z_hand = _unit(np.cross(x_hand, y_hand))
+        y_hand = _unit(np.cross(z_hand, x_hand))
+
+        R_wrist = np.column_stack([x_hand, y_hand, z_hand])
+        quat = R_to_quat_xyzw(R_wrist)
+
+        grasp_pos = axes.centroid.copy()
+        pre_grasp_pos = grasp_pos + z_base * self.approach_offset_m
+
+        # Object width along mid axis (the pinch span)
+        object_width_m = float(axes.extents[1])
+
+        return GraspPlan(
+            grasp_position=grasp_pos,
+            grasp_quaternion=quat,
+            pre_grasp_position=pre_grasp_pos,
+            object_width_m=object_width_m,
+            axes=axes,
+        )
+
+
+def _unit(v: np.ndarray) -> np.ndarray:
+    n = np.linalg.norm(v)
+    if n < 1e-12:
+        return v
+    return v / n

src/presa_mano/grasp/visual_servo.py

@@ -0,0 +1,504 @@
+"""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,
+    )