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/__init__.py

@@ -0,0 +1,3 @@
+"""presa-mano: hand-eye calibration + object grasping microservice."""
+
+__version__ = "0.1.0"

src/presa_mano/agent/__init__.py

@@ -0,0 +1,10 @@
+"""Conversational AI assistant for presa-mano.
+
+Talks to OpenRouter (OpenAI-compatible chat-completions API) and orchestrates
+robot/perception actions via a curated set of tools defined in `tools.py`. The
+LLM never sees the raw upstream services — it only sees high-level functions
+that wrap our existing FastAPI endpoints, with safety guards (default dry-run
+on motion, side validation, calibration-side awareness)."""
+
+from .runner import AgentRunner  # noqa: F401
+from .tools import TOOL_DEFINITIONS, dispatch_tool  # noqa: F401

src/presa_mano/agent/prompt.py

@@ -0,0 +1,107 @@
+"""System prompt for the presa-mano AI assistant.
+
+Built dynamically so it always reflects the currently-active robot and the
+real list of tools the model can call."""
+
+from __future__ import annotations
+
+from typing import Any
+
+
+def build_system_prompt() -> str:
+    # Late imports to avoid the circular dependency:
+    # api/__init__.py imports agent_routes -> agent.runner -> agent.prompt,
+    # so agent.prompt must NOT import from `..api` at module load time.
+    from ..config import get_config
+    from ..api import _deps
+
+    cfg = get_config()
+    active = _deps.active_robot_key()
+    default_side = _deps.default_side()
+    aruco = cfg.get("aruco", {}).get("wrist_marker", {})
+    min_samples = int(cfg.get("calibration", {}).get("min_samples", 5))
+
+    return f"""Sei l'assistente di **presa-mano**, un orchestratore FastAPI che pilota
+un robot umanoide (Unitree G1 o H1) con una camera depth Nuwa-HP60C fissa.
+
+## Stile di risposta — REGOLE OBBLIGATORIE
+- Rispondi in italiano, **diretto e conciso**: niente preamboli ("ecco i risultati…"),
+  niente paragrafi di chiusura ("se vuoi posso…"), niente emoji.
+- Riporta SOLO i fatti che derivano dai tool eseguiti. **NON inventare valori
+  numerici, posizioni, dimensioni, spiegazioni di errori che il tool non ha
+  ritornato.** Se un tool ritorna `ok=false`, riporta letteralmente l'errore.
+- Se manca un'informazione, dillo invece di indovinare. Non aggiungere
+  "possibili cause" speculative se non sono nel risultato del tool.
+- Niente disclaimers o suggerimenti non richiesti. L'operatore ti dirà se
+  vuole un consiglio.
+- Per dati numerici usa tabelle compatte; per istruzioni passi numerati brevi.
+- Per tutto ciò che è azione su robot/camera/calibrazione DEVI usare i tool.
+  **Vietato simulare risultati.**
+
+## Stato corrente
+- Robot attivo: **{active.upper()}** (puoi cambiarlo con `set_active_robot`).
+- Lato di default per le operazioni di braccio: **{default_side}**. Se l'utente
+  non specifica `right`/`left`, usa questo default.
+- Marker ArUco al polso: id={aruco.get('id', 10)}, lato={aruco.get('size_mm', 40)} mm.
+
+## Convenzioni
+- Le posizioni sono in **metri** nel frame base del robot, gli orientamenti
+  in quaternioni `[x, y, z, w]`. Le rotazioni "in gradi" che l'utente cita
+  vanno convertite tu (le tool accettano sempre quaternioni).
+- Movimenti **assoluti**: `move_wrist_absolute(side, position, quaternion)`.
+- Movimenti **relativi**: `move_wrist_relative(side, delta_position)` —
+  delta in metri, frame base. Se l'utente dice "muovi di 200 mm in X+", passa
+  delta=[0.2, 0, 0]. Conferma sempre il risultato leggendo la nuova posa.
+- Default `dry_run=true` su tutte le operazioni di motion: NON fai mai partire
+  un movimento reale senza che l'utente l'abbia chiesto esplicitamente.
+- Le mani: `hand_action(side, action)` con `action ∈
+  {{LEFT_OPEN, LEFT_CLOSE, RIGHT_OPEN, RIGHT_CLOSE}}`.
+
+## Calibrazione hand-eye (eye-to-hand)
+La calibrazione lega `T_base_camera`. È **specifica per braccio**: dx e sx
+hanno file separati. Procedura completa quando l'utente dice "calibra":
+1. Determina `side` (chiedi se ambiguo).
+2. `calibration_clear(side)` per partire da zero (chiedi prima conferma).
+3. Ripeti almeno {min_samples} volte (consigliato 8–15):
+   a. `move_wrist_relative` o `move_wrist_absolute` per portare il polso in
+      una posa nuova ben visibile dalla camera (varia X, Y, Z e l'orientamento
+      del marker — più diversità = miglior calibrazione).
+   b. Aspetta l'assestamento (tool `sleep` non disponibile: passa al sample;
+      `move_wrist_*` di default attende).
+   c. `calibration_add_sample(side)` — questa cattura RGB+pose+marker.
+   d. Se il marker non è stato trovato (errore 422), riprova con una posa
+      diversa.
+4. `calibration_solve(side, method='TSAI')` — richiede ≥{min_samples} sample.
+5. Riporta all'utente i residui (`residual_translation_mm`, `residual_rotation_deg`).
+   Se i residui sono > 5 mm o > 1° suggerisci più sample.
+
+## Percezione (per planning grasp)
+- `detect_object(class_name, confidence)` → bbox + score.
+- `get_point_cloud_roi(bbox)` → statistiche nuvola 3D (mean/min/max in **frame camera Nuwa**: X-right, Y-up, Z-back; Z<0 = davanti).
+- `get_marker(marker_id, size_mm)` → posa marker **da PnP** nel **frame camera OpenCV** (X-right, Y-down, Z-forward; Z>0 = davanti). PnP eseguita da marker-finder con gli intrinseci veri della Nuwa. È il metodo più affidabile per **posizione assoluta** quando size_mm è dichiarata correttamente.
+- `get_marker_3d(marker_id, size_mm)` → posa marker **dal sensore depth** (no PnP). Restituisce anche `measured_side_mm`. ATTENZIONE: se la correzione depth (affine, vedi sotto) è attiva, le dimensioni geometriche misurate vengono distorte — `measured_side_mm` può differire significativamente da `declared_size_mm` anche se la size dichiarata è corretta. **NON usare measured_side_mm come ground truth della size dichiarata.**
+- Per validare se `size_mm` dichiarato è corretto, confronta `get_marker` Z con la distanza vera misurata col metro: il PnP scala linearmente con size_mm dichiarata.
+- Per stimare la posa **nel frame base del robot** dopo la calibrazione, considera che la matrice `T_base_camera` salvata da `calibration_solve` è quella che mappa il frame camera OpenCV nel frame base.
+
+## Sensore depth e correzione
+Il sensore depth Nuwa ha un bias che è stato calibrato come trasformazione affine (`depth_corrected = (raw - offset) / scale`). Quando attiva, migliora l'accuratezza della **distanza** ma distorce le **dimensioni** misurate dai 4 corner 3D del marker. Lo stato della correzione è leggibile da `GET /api/perception/depth_correction` (non esposto come tool: chiedi all'utente se serve verificarla).
+
+## Sicurezza
+- Prima di QUALSIASI movimento reale (dry_run=false), conferma con l'utente
+  l'azione e il valore numerico.
+- Non chiamare mai `emergency_stop` o `unlock` se non te lo chiede l'utente.
+- Se l'utente ti dà un task vago ("muovi il braccio") chiedi precisazioni.
+
+Comincia ascoltando la richiesta dell'operatore e usa i tool quando servono.
+Se rispondi con testo soltanto (nessun tool call), assicurati che l'utente
+abbia tutte le informazioni che ha chiesto.
+""" + _addendum(cfg)
+
+
+def _addendum(cfg: dict[str, Any]) -> str:
+    """Optional extra instructions that the operator can edit from the GUI
+    without recompiling the base prompt. Stored in chat.openrouter.user_prompt_addendum."""
+    txt = (cfg.get("chat", {}).get("openrouter", {}).get("user_prompt_addendum") or "").strip()
+    if not txt:
+        return ""
+    return f"\n\n## Istruzioni aggiuntive dell'operatore\n{txt}\n"

src/presa_mano/agent/runner.py

@@ -0,0 +1,174 @@
+"""OpenRouter chat runner with multi-turn tool calling.
+
+OpenRouter speaks the OpenAI chat-completions API verbatim, so we use
+`httpx` directly and re-implement the tool-call loop. We don't take a hard
+dep on the openai SDK — the protocol is small enough to inline and this
+keeps Docker layers slim.
+"""
+
+from __future__ import annotations
+
+import json
+import logging
+import os
+import time
+from dataclasses import dataclass, field
+from typing import Any
+
+import httpx
+
+from ..config import get_config
+from .prompt import build_system_prompt
+from .tools import TOOL_DEFINITIONS, dispatch_tool
+
+log = logging.getLogger(__name__)
+
+
+class OpenRouterError(RuntimeError):
+    pass
+
+
+@dataclass
+class ToolCallTrace:
+    name: str
+    arguments: dict[str, Any]
+    result: dict[str, Any]
+    latency_ms: float
+
+
+@dataclass
+class AgentResult:
+    reply: str
+    iterations: int
+    tool_calls: list[ToolCallTrace] = field(default_factory=list)
+    finish_reason: str | None = None
+    model: str | None = None
+    usage: dict[str, Any] | None = None
+
+
+class AgentRunner:
+    """Stateless: one runner per request. Caller is responsible for keeping
+    the conversation history and passing it back in via `messages`.
+
+    The run loop: send messages → if tool_calls in response, execute each,
+    append the assistant message + tool messages, repeat. Up to
+    `max_tool_iterations` rounds (configurable in chat.openrouter)."""
+
+    def __init__(self) -> None:
+        cfg = get_config().get("chat", {}).get("openrouter") or {}
+        self.base_url = str(cfg.get("base_url") or "https://openrouter.ai/api/v1").rstrip("/")
+        self.model = str(cfg.get("model") or "anthropic/claude-sonnet-4.5")
+        self.timeout_s = float(cfg.get("timeout_s") or 60.0)
+        self.max_tool_iterations = int(cfg.get("max_tool_iterations") or 8)
+        # Default low so the model doesn't invent values when summarising
+        # tool results. Override in chat.openrouter.temperature if needed.
+        self.temperature = float(cfg.get("temperature") if cfg.get("temperature") is not None else 0.1)
+        self.referer = cfg.get("referer") or "http://localhost:8890"
+        self.title = cfg.get("title") or "presa-mano"
+        env_var = str(cfg.get("api_key_env") or "OPENROUTER_API_KEY")
+        self.api_key = os.environ.get(env_var, "")
+        self.api_key_env = env_var
+
+    def has_credentials(self) -> bool:
+        return bool(self.api_key)
+
+    # ------------------------------------------------------------------
+
+    def run(self, user_message: str,
+            history: list[dict[str, Any]] | None = None,
+            ) -> tuple[AgentResult, list[dict[str, Any]]]:
+        """Returns (result, updated_history). `history` is the full message
+        list as it appears in the OpenAI API (excluding the system prompt;
+        we always inject it ourselves so it reflects current state)."""
+        if not self.has_credentials():
+            raise OpenRouterError(f"OpenRouter API key non impostata (env var {self.api_key_env}).")
+
+        system = {"role": "system", "content": build_system_prompt()}
+        messages: list[dict[str, Any]] = [system]
+        if history:
+            messages.extend(m for m in history if m.get("role") != "system")
+        messages.append({"role": "user", "content": user_message})
+
+        traces: list[ToolCallTrace] = []
+        iterations = 0
+        finish_reason: str | None = None
+        usage: dict[str, Any] | None = None
+
+        while iterations < self.max_tool_iterations:
+            iterations += 1
+            resp = self._chat_completion(messages)
+            usage = resp.get("usage") or usage
+            choices = resp.get("choices") or []
+            if not choices:
+                raise OpenRouterError(f"empty choices in response: {resp}")
+            msg = choices[0].get("message") or {}
+            finish_reason = choices[0].get("finish_reason")
+            messages.append(msg)
+            tool_calls = msg.get("tool_calls") or []
+            if not tool_calls:
+                # Final assistant turn — no further tools requested.
+                reply = msg.get("content") or ""
+                history_out = [m for m in messages if m.get("role") != "system"]
+                return AgentResult(reply=reply, iterations=iterations,
+                                   tool_calls=traces, finish_reason=finish_reason,
+                                   model=resp.get("model"), usage=usage), history_out
+
+            # Execute each tool call and append the result back.
+            for call in tool_calls:
+                fn = (call.get("function") or {})
+                name = fn.get("name") or "?"
+                raw_args = fn.get("arguments") or "{}"
+                try:
+                    args = json.loads(raw_args) if isinstance(raw_args, str) else (raw_args or {})
+                except json.JSONDecodeError:
+                    args = {}
+                t0 = time.perf_counter()
+                result = dispatch_tool(name, args)
+                latency = round((time.perf_counter() - t0) * 1000.0, 1)
+                traces.append(ToolCallTrace(name=name, arguments=args,
+                                            result=result, latency_ms=latency))
+                messages.append({
+                    "role": "tool",
+                    "tool_call_id": call.get("id"),
+                    "name": name,
+                    "content": json.dumps(result, ensure_ascii=False, default=str),
+                })
+
+        # Hit the iteration cap with tool calls still pending — degrade gracefully.
+        history_out = [m for m in messages if m.get("role") != "system"]
+        return AgentResult(
+            reply="Limite di iterazioni raggiunto: rivedi il task o chiedimi di continuare.",
+            iterations=iterations, tool_calls=traces, finish_reason="iteration_cap",
+            model=self.model, usage=usage,
+        ), history_out
+
+    # ------------------------------------------------------------------
+
+    def _chat_completion(self, messages: list[dict[str, Any]]) -> dict[str, Any]:
+        url = f"{self.base_url}/chat/completions"
+        body = {
+            "model": self.model,
+            "messages": messages,
+            "tools": TOOL_DEFINITIONS,
+            "tool_choice": "auto",
+            "temperature": self.temperature,
+        }
+        headers = {
+            "Authorization": f"Bearer {self.api_key}",
+            "Content-Type": "application/json",
+            # OpenRouter analytics — optional but recommended.
+            "HTTP-Referer": self.referer,
+            "X-Title": self.title,
+        }
+        try:
+            with httpx.Client(timeout=self.timeout_s) as c:
+                r = c.post(url, json=body, headers=headers)
+        except httpx.HTTPError as e:
+            raise OpenRouterError(f"OpenRouter unreachable: {e}") from e
+        if r.status_code >= 400:
+            try:
+                detail = r.json()
+            except Exception:
+                detail = r.text
+            raise OpenRouterError(f"OpenRouter HTTP {r.status_code}: {detail}")
+        return r.json()

src/presa_mano/agent/tools.py

@@ -0,0 +1,684 @@
+"""Tools the AI assistant can invoke.
+
+Each tool is defined twice:
+1. As a JSON-schema entry in `TOOL_DEFINITIONS` (sent to OpenRouter / the LLM
+   so it knows the function signature).
+2. As a Python handler keyed by tool name in `_HANDLERS` that wraps existing
+   internal endpoints/clients.
+
+Handlers are intentionally thin and well-validated: they enforce side ∈
+{right,left}, default dry_run=True for every motion call, and reuse the
+existing per-request client construction (so a `set_active_robot` call mid-
+conversation actually changes which robot subsequent tool calls hit).
+
+Tool results are returned as dicts and serialised to JSON before being sent
+back to the model. Errors are reported as `{"error": "..."}` rather than
+raising, so the model can recover (e.g. retry with a different pose).
+"""
+
+from __future__ import annotations
+
+import logging
+import time
+from dataclasses import asdict
+from typing import Any, Callable
+
+import numpy as np
+
+from ..calibration import CalibrationSampler, CalibrationStore, HandEyeSolver, SamplesStore
+from ..clients import G1StatusError, G1Unavailable
+from ..config import get_config, update_config
+from ..perception import ObjectDetector
+
+log = logging.getLogger(__name__)
+
+
+# Lazy proxy for `..api._deps` to break the import cycle:
+# api/__init__.py loads agent_routes which loads agent.runner which loads us.
+# Importing `..api._deps` at module load would trigger api/__init__.py while
+# it's already mid-init. Attribute access via this proxy defers the import
+# until the first tool actually runs, by which time everything is loaded.
+class _LazyDeps:
+    def __getattr__(self, name: str) -> Any:
+        from ..api import _deps as _m
+        return getattr(_m, name)
+
+
+_deps = _LazyDeps()
+
+
+# ----- helpers --------------------------------------------------------------
+
+def _ok(**kw: Any) -> dict[str, Any]:
+    out = {"ok": True}
+    out.update(kw)
+    return out
+
+
+def _err(msg: str, **kw: Any) -> dict[str, Any]:
+    out = {"ok": False, "error": msg}
+    out.update(kw)
+    return out
+
+
+def _validate_side(side: str | None) -> str:
+    s = (side or _deps.default_side()).lower()
+    if s not in {"right", "left"}:
+        raise ValueError(f"side must be 'right' or 'left', got: {side!r}")
+    return s
+
+
+# ----- tool: home / robot selection ----------------------------------------
+
+def _h_get_active_robot(_: dict[str, Any]) -> dict[str, Any]:
+    return _ok(active=_deps.active_robot_key(),
+               default_side=_deps.default_side())
+
+
+def _h_set_active_robot(args: dict[str, Any]) -> dict[str, Any]:
+    target = str(args.get("robot", "")).lower()
+    if target not in {"g1", "h1"}:
+        return _err(f"robot must be 'g1' or 'h1', got: {target!r}")
+    services = (get_config().get("services") or {})
+    if not (services.get(target) or {}).get("base_url"):
+        return _err(f"services.{target}.base_url not configured")
+    update_config({"robot": {"active": target}})
+    return _ok(active=_deps.active_robot_key())
+
+
+def _h_device_status(_: dict[str, Any]) -> dict[str, Any]:
+    """Reuses the home_routes implementation to avoid duplicating the ping logic."""
+    from ..api.home_routes import status as _status
+    return _status()
+
+
+# ----- tool: wrist read/move ------------------------------------------------
+
+def _h_get_wrist_pose(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        data = _deps.g1().wrist()
+        body = data.get("data") if isinstance(data.get("data"), dict) else data
+        pose = body.get(f"{side}_wrist") or body.get(side) or {}
+        return _ok(side=side,
+                   position=pose.get("position"),
+                   quaternion=pose.get("quaternion"))
+    except (G1Unavailable, G1StatusError) as e:
+        return _err(f"robot non raggiungibile: {e}")
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_move_wrist_absolute(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        position = list(args.get("position") or [])
+        quaternion = list(args.get("quaternion") or [])
+        if len(position) != 3 or len(quaternion) != 4:
+            return _err("position deve avere 3 float (m), quaternion 4 float (xyzw)")
+        dry_run = bool(args.get("dry_run", True))
+        timeout_s = float(args.get("timeout_s", 5.0))
+        result = _deps.g1(dry_run=dry_run).wrist_absolute(
+            side=side, position=position, quaternion=quaternion,
+            wait=True, timeout_s=timeout_s,
+        )
+        return _ok(dry_run=dry_run, side=side, response=result)
+    except (G1Unavailable, G1StatusError) as e:
+        return _err(f"motion failed: {e}")
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_move_wrist_relative(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        dp = args.get("delta_position")
+        dq = args.get("delta_quaternion")
+        if dp is not None and len(list(dp)) != 3:
+            return _err("delta_position deve avere 3 float (m) o essere null")
+        if dq is not None and len(list(dq)) != 4:
+            return _err("delta_quaternion deve avere 4 float (xyzw) o essere null")
+        dry_run = bool(args.get("dry_run", True))
+        timeout_s = float(args.get("timeout_s", 5.0))
+        result = _deps.g1(dry_run=dry_run).wrist_relative(
+            side=side,
+            delta_position=list(dp) if dp is not None else None,
+            delta_quaternion=list(dq) if dq is not None else None,
+            wait=True, timeout_s=timeout_s,
+        )
+        return _ok(dry_run=dry_run, side=side, response=result)
+    except (G1Unavailable, G1StatusError) as e:
+        return _err(f"motion failed: {e}")
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_hand_action(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        action = str(args.get("action", "")).upper()
+        valid = {"LEFT_OPEN", "LEFT_CLOSE", "RIGHT_OPEN", "RIGHT_CLOSE"}
+        if action not in valid:
+            return _err(f"action deve essere uno di {sorted(valid)}")
+        dry_run = bool(args.get("dry_run", True))
+        result = _deps.g1(dry_run=dry_run).hand_action(action)
+        return _ok(dry_run=dry_run, response=result)
+    except (G1Unavailable, G1StatusError) as e:
+        return _err(f"hand action failed: {e}")
+    except Exception as e:
+        return _err(str(e))
+
+
+# ----- tool: perception -----------------------------------------------------
+
+def _h_get_marker(args: dict[str, Any]) -> dict[str, Any]:
+    """Detect ArUco marker and return its pose in the camera frame.
+
+    Da quando marker-finder accetta intrinseci OpenCV via API e presa-mano
+    gli pusha quelli veri della Nuwa al boot, possiamo fidarci direttamente
+    di `position_m`/`quaternion_xyzw` ritornati dal servizio. Niente più
+    re-PnP locale qui: per la diagnostica di ambiguità c'è il sampler
+    (CalibrationSampler) che la calcola sui sample reali."""
+    try:
+        cfg = get_config()
+        marker_id = int(args.get("marker_id") or cfg["aruco"]["wrist_marker"]["id"])
+        size_mm = float(args.get("size_mm") or cfg["aruco"]["wrist_marker"]["size_mm"])
+        result = _deps.marker().detect_url(
+            markers=[{"id": marker_id, "size_mm": size_mm}],
+            image_url=_deps.nuwa().rgb_url(),
+        )
+        found = result.get("found", [])
+        entry = next((m for m in found if int(m["id"]) == marker_id), None)
+        if entry is None:
+            return _err(f"marker id={marker_id} non trovato",
+                        not_found=result.get("not_found"))
+
+        img_pts = np.asarray(entry["corners"], dtype=np.float64).reshape(-1, 2)
+        tvec = np.asarray(entry.get("position_m") or [0, 0, 0], dtype=np.float64).flatten()
+        return _ok(
+            marker_id=marker_id,
+            position_m=tvec.tolist(),
+            distance_m=float(np.linalg.norm(tvec)),
+            quaternion_xyzw=entry.get("quaternion_xyzw"),
+            rotation_matrix=entry.get("rotation_matrix"),
+            corners=entry.get("corners"),
+            center_px=img_pts.mean(axis=0).tolist(),
+        )
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_detect_object(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        nuwa = _deps.nuwa()
+        yolo = _deps.yolo()
+        detector = ObjectDetector(yolo, nuwa)
+        conf = args.get("confidence")
+        conf = float(conf) if conf is not None else None
+        class_name = args.get("class_name")
+        dets = detector.detect(conf=conf)
+        if class_name:
+            dets = [d for d in dets if str(d.get("class_name", "")).lower() == str(class_name).lower()]
+        if not dets:
+            return _ok(found=False, detections=[])
+        return _ok(found=True, detections=dets[:5])
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_get_marker_3d(args: dict[str, Any]) -> dict[str, Any]:
+    """Pose marker da DEPTH (no PnP).
+
+    Detection 2D dei corner dal marker-finder, poi back-projection di
+    ciascun corner usando la depth con registrazione RGB↔IR. Centro = media
+    dei 4 corner 3D; orientamento = base ortonormale costruita dai lati del
+    marker. Più affidabile di PnP quando si conosce bene la depth e meno
+    sensibile a errori di size_mm dichiarata."""
+    try:
+        from ..perception.depth_to_rgb import (
+            CameraRegistration, back_project_rgb_pixel, fetch_depth_mm,
+        )
+        from ..perception.depth_correction import load as _load_dc_state
+        cfg = get_config()
+        marker_id = int(args.get("marker_id") or cfg["aruco"]["wrist_marker"]["id"])
+        size_mm = float(args.get("size_mm") or cfg["aruco"]["wrist_marker"]["size_mm"])
+
+        nuwa = _deps.nuwa()
+        result = _deps.marker().detect_url(
+            markers=[{"id": marker_id, "size_mm": size_mm}],
+            image_url=nuwa.rgb_url(),
+        )
+        entry = next((m for m in result.get("found", []) if int(m["id"]) == marker_id), None)
+        if entry is None:
+            return _err(f"marker id={marker_id} non trovato",
+                        not_found=result.get("not_found"))
+
+        reg = CameraRegistration.from_nuwa(nuwa.intrinsics())
+        depth = fetch_depth_mm(nuwa)
+        dc_state = _load_dc_state(_deps.depth_correction_path())
+        corners_2d = np.asarray(entry["corners"], dtype=np.float64).reshape(-1, 2)
+
+        corners_3d = []
+        per_corner = []
+        for u, v in corners_2d:
+            r = back_project_rgb_pixel(u, v, depth, reg, sample_radius=4, n_iters=2,
+                                       depth_correction=dc_state)
+            per_corner.append({"rgb_pixel": r["rgb_pixel"], "ok": r["ok"],
+                               "ir_pixel": r.get("ir_pixel"),
+                               "depth_mm": r.get("depth_mm"),
+                               "point_rgb_m": r.get("point_rgb_m"),
+                               **({"error": r["error"]} if "error" in r else {})})
+            if r["ok"]:
+                corners_3d.append(r["point_rgb_m"])
+
+        if len(corners_3d) < 3:
+            return _err(f"depth mancante su {4 - len(corners_3d)} corner: marker fuori range o riflesso",
+                        per_corner=per_corner)
+
+        pts = np.asarray(corners_3d, dtype=np.float64)
+        center = pts.mean(axis=0)
+
+        # Build an orthonormal frame from the marker plane:
+        # x-axis = (TR - TL) (top edge), y-axis = (BL - TL) (left edge),
+        # z-axis = x × y (outward normal).
+        out_pose: dict[str, Any] = {"center_m": center.tolist()}
+        if len(corners_3d) == 4:
+            tl, tr, br, bl = pts
+            x_axis = tr - tl
+            y_axis = bl - tl
+            x_axis /= (np.linalg.norm(x_axis) + 1e-12)
+            # Re-orthogonalize y against x:
+            y_axis = y_axis - (y_axis @ x_axis) * x_axis
+            y_axis /= (np.linalg.norm(y_axis) + 1e-12)
+            z_axis = np.cross(x_axis, y_axis)
+            R = np.column_stack([x_axis, y_axis, z_axis])
+            from ..geometry import R_to_quat_xyzw
+            out_pose["rotation_matrix"] = R.tolist()
+            out_pose["quaternion_xyzw"] = R_to_quat_xyzw(R).tolist()
+            # Side measured vs declared — sanity check
+            measured_side_mm = float(np.linalg.norm(tr - tl) * 1000)
+            out_pose["measured_side_mm"] = round(measured_side_mm, 1)
+            out_pose["declared_size_mm"] = size_mm
+
+        return _ok(
+            marker_id=marker_id,
+            distance_m=float(np.linalg.norm(center)),
+            n_corners_with_depth=len(corners_3d),
+            pose=out_pose,
+            per_corner=per_corner,
+            corners_2d=corners_2d.tolist(),
+        )
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_get_point_cloud_roi(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        bbox = args.get("bbox")
+        if not bbox or len(list(bbox)) != 4:
+            return _err("bbox deve essere [x1,y1,x2,y2] in pixel")
+        x1, y1, x2, y2 = (int(v) for v in bbox)
+        nuwa = _deps.nuwa()
+        cloud = nuwa.pointcloud_roi(x1=x1, y1=y1, x2=x2, y2=y2)
+        # Cloud is potentially huge — return only summary stats so we don't
+        # blow up the LLM context.
+        pts = np.asarray(cloud, dtype=np.float64).reshape(-1, 6) if len(cloud) else np.zeros((0, 6))
+        if pts.size == 0:
+            return _ok(n_points=0, summary=None)
+        xyz = pts[:, :3]
+        return _ok(
+            n_points=int(pts.shape[0]),
+            mean=xyz.mean(axis=0).tolist(),
+            min=xyz.min(axis=0).tolist(),
+            max=xyz.max(axis=0).tolist(),
+            std_mm=(1000.0 * xyz.std(axis=0)).tolist(),
+        )
+    except Exception as e:
+        return _err(str(e))
+
+
+# ----- tool: calibration ---------------------------------------------------
+
+def _h_calibration_add_sample(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        cfg = get_config()
+        marker_id = int(args.get("marker_id") or cfg["aruco"]["wrist_marker"]["id"])
+        size_mm = float(args.get("size_mm") or cfg["aruco"]["wrist_marker"]["size_mm"])
+        sampler = CalibrationSampler(_deps.nuwa(), _deps.marker(), _deps.g1())
+        sample = sampler.acquire(marker_id=marker_id, size_mm=size_mm,
+                                 side=side, note=str(args.get("note", "")))
+        store = SamplesStore(_deps.samples_file_path(side))
+        items = store.append(sample)
+        return _ok(side=side, total=len(items), sample=asdict(sample))
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_calibration_solve(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        cfg = get_config()
+        method = str(args.get("method") or cfg.get("calibration", {}).get("method") or "TSAI").upper()
+        min_n = int(cfg.get("calibration", {}).get("min_samples", 5))
+        samples = SamplesStore(_deps.samples_file_path(side)).load()
+        if len(samples) < min_n:
+            return _err(f"servono almeno {min_n} sample per side='{side}', ce ne sono {len(samples)}")
+        result = HandEyeSolver.solve(samples, method=method)
+        CalibrationStore(_deps.calibration_file_path(side)).save(result)
+        # Strip the heavy matrix from what we return to the model — keep summary.
+        slim = {
+            "side": side,
+            "method": result.get("method"),
+            "n_samples": result.get("n_samples"),
+            "residual_translation_mm": result.get("residual_translation_mm"),
+            "residual_rotation_deg": result.get("residual_rotation_deg"),
+        }
+        return _ok(**slim, saved=True)
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_calibration_clear(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        SamplesStore(_deps.samples_file_path(side)).clear()
+        return _ok(side=side, total=0)
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_calibration_get_result(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        doc = CalibrationStore(_deps.calibration_file_path(side)).load()
+        if doc is None:
+            return _err(f"nessuna calibrazione salvata per side='{side}'")
+        return _ok(side=side,
+                   saved_at=doc.get("saved_at"),
+                   method=doc.get("method"),
+                   n_samples=doc.get("n_samples"),
+                   residual_translation_mm=doc.get("residual_translation_mm"),
+                   residual_rotation_deg=doc.get("residual_rotation_deg"))
+    except Exception as e:
+        return _err(str(e))
+
+
+def _h_calibration_count_samples(args: dict[str, Any]) -> dict[str, Any]:
+    try:
+        side = _validate_side(args.get("side"))
+        samples = SamplesStore(_deps.samples_file_path(side)).load()
+        return _ok(side=side, total=len(samples))
+    except Exception as e:
+        return _err(str(e))
+
+
+# ----- registry -------------------------------------------------------------
+
+_HANDLERS: dict[str, Callable[[dict[str, Any]], dict[str, Any]]] = {
+    "get_active_robot": _h_get_active_robot,
+    "set_active_robot": _h_set_active_robot,
+    "device_status": _h_device_status,
+    "get_wrist_pose": _h_get_wrist_pose,
+    "move_wrist_absolute": _h_move_wrist_absolute,
+    "move_wrist_relative": _h_move_wrist_relative,
+    "hand_action": _h_hand_action,
+    "get_marker": _h_get_marker,
+    "get_marker_3d": _h_get_marker_3d,
+    "detect_object": _h_detect_object,
+    "get_point_cloud_roi": _h_get_point_cloud_roi,
+    "calibration_add_sample": _h_calibration_add_sample,
+    "calibration_solve": _h_calibration_solve,
+    "calibration_clear": _h_calibration_clear,
+    "calibration_get_result": _h_calibration_get_result,
+    "calibration_count_samples": _h_calibration_count_samples,
+}
+
+
+# OpenRouter / OpenAI tool-calling format. Each entry is a 'function' tool.
+TOOL_DEFINITIONS: list[dict[str, Any]] = [
+    {
+        "type": "function",
+        "function": {
+            "name": "get_active_robot",
+            "description": "Quale robot è attualmente collegato (g1 o h1) e quale lato è il default.",
+            "parameters": {"type": "object", "properties": {}},
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "set_active_robot",
+            "description": "Cambia il robot attivo (g1 o h1). Tutti i comandi successivi useranno questo robot.",
+            "parameters": {
+                "type": "object",
+                "required": ["robot"],
+                "properties": {"robot": {"type": "string", "enum": ["g1", "h1"]}},
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "device_status",
+            "description": "Stato di comunicazione (online/offline + latenza) dei microservizi: Nuwa, marker-finder, YOLO, robot.",
+            "parameters": {"type": "object", "properties": {}},
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "get_wrist_pose",
+            "description": "Posa attuale del polso (position xyz in metri, quaternion xyzw) nel frame base.",
+            "parameters": {
+                "type": "object",
+                "properties": {"side": {"type": "string", "enum": ["right", "left"]}},
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "move_wrist_absolute",
+            "description": "Sposta il polso a una posa assoluta nel frame base. Default dry_run=true: il robot NON si muove davvero finché dry_run=false.",
+            "parameters": {
+                "type": "object",
+                "required": ["side", "position", "quaternion"],
+                "properties": {
+                    "side": {"type": "string", "enum": ["right", "left"]},
+                    "position": {"type": "array", "items": {"type": "number"}, "minItems": 3, "maxItems": 3,
+                                  "description": "x,y,z in metri (frame base)"},
+                    "quaternion": {"type": "array", "items": {"type": "number"}, "minItems": 4, "maxItems": 4,
+                                    "description": "x,y,z,w"},
+                    "dry_run": {"type": "boolean", "default": True},
+                    "timeout_s": {"type": "number", "default": 5.0},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "move_wrist_relative",
+            "description": "Sposta il polso di un delta rispetto alla posa attuale. delta_position in metri (frame base), delta_quaternion opzionale. Default dry_run=true.",
+            "parameters": {
+                "type": "object",
+                "required": ["side"],
+                "properties": {
+                    "side": {"type": "string", "enum": ["right", "left"]},
+                    "delta_position": {"type": "array", "items": {"type": "number"}, "minItems": 3, "maxItems": 3},
+                    "delta_quaternion": {"type": "array", "items": {"type": "number"}, "minItems": 4, "maxItems": 4},
+                    "dry_run": {"type": "boolean", "default": True},
+                    "timeout_s": {"type": "number", "default": 5.0},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "hand_action",
+            "description": "Apre o chiude una mano. action ∈ {LEFT_OPEN, LEFT_CLOSE, RIGHT_OPEN, RIGHT_CLOSE}. Default dry_run=true.",
+            "parameters": {
+                "type": "object",
+                "required": ["action"],
+                "properties": {
+                    "action": {"type": "string", "enum": ["LEFT_OPEN", "LEFT_CLOSE", "RIGHT_OPEN", "RIGHT_CLOSE"]},
+                    "dry_run": {"type": "boolean", "default": True},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "get_marker",
+            "description": (
+                "Rileva il marker ArUco nell'RGB corrente di Nuwa e ne ritorna la "
+                "posa nel frame camera OpenCV (X-right, Y-down, Z-forward, Z>0=davanti). "
+                "Restituisce position_m, quaternion_xyzw, rotation_matrix, distance_m, "
+                "corners 2D e center_px. La PnP è eseguita da marker-finder con gli "
+                "intrinseci veri della Nuwa (pushati al boot di presa-mano)."
+            ),
+            "parameters": {
+                "type": "object",
+                "properties": {
+                    "marker_id": {"type": "integer", "description": "default = id del marker polso da config"},
+                    "size_mm": {"type": "number", "description": "default = size del marker polso da config"},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "get_marker_3d",
+            "description": (
+                "Posa marker calcolata dal sensore depth (no PnP). Detection "
+                "2D dei 4 corner via marker-finder, poi per ogni corner "
+                "back-projection corretta usando depth IR + extrinsics IR↔RGB. "
+                "Centro = media 3D dei corner; orientamento da base ortonormale "
+                "del piano marker. Restituisce anche `measured_side_mm` (utile "
+                "per verificare che size_mm dichiarata sia giusta) e una entry "
+                "per_corner con qualità della depth a ciascuno dei 4 corner."
+            ),
+            "parameters": {
+                "type": "object",
+                "properties": {
+                    "marker_id": {"type": "integer"},
+                    "size_mm": {"type": "number"},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "detect_object",
+            "description": "Rileva oggetti via YOLO sull'RGB di Nuwa. class_name opzionale per filtrare; restituisce massimo 5 detection.",
+            "parameters": {
+                "type": "object",
+                "properties": {
+                    "class_name": {"type": "string"},
+                    "confidence": {"type": "number"},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "get_point_cloud_roi",
+            "description": "Statistiche della nuvola punti (frame camera) all'interno di un bbox in pixel. Restituisce conteggio + media/min/max/std, NON i punti raw.",
+            "parameters": {
+                "type": "object",
+                "required": ["bbox"],
+                "properties": {
+                    "bbox": {"type": "array", "items": {"type": "number"}, "minItems": 4, "maxItems": 4,
+                             "description": "[x1,y1,x2,y2] in pixel"},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "calibration_add_sample",
+            "description": "Cattura un sample di calibrazione hand-eye per il braccio indicato (RGB + posa polso + posa marker). Richiede il marker visibile.",
+            "parameters": {
+                "type": "object",
+                "properties": {
+                    "side": {"type": "string", "enum": ["right", "left"]},
+                    "marker_id": {"type": "integer"},
+                    "size_mm": {"type": "number"},
+                    "note": {"type": "string"},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "calibration_solve",
+            "description": "Risolve hand-eye calibration per il braccio dato e SALVA la matrice T_base_camera_<side>.yaml. Riporta residui.",
+            "parameters": {
+                "type": "object",
+                "properties": {
+                    "side": {"type": "string", "enum": ["right", "left"]},
+                    "method": {"type": "string", "enum": ["TSAI", "PARK", "HORAUD", "ANDREFF", "DANIILIDIS"]},
+                },
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "calibration_clear",
+            "description": "Cancella tutti i sample di calibrazione per il braccio indicato.",
+            "parameters": {
+                "type": "object",
+                "properties": {"side": {"type": "string", "enum": ["right", "left"]}},
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "calibration_get_result",
+            "description": "Riporta la calibrazione salvata per il braccio indicato (residui + saved_at).",
+            "parameters": {
+                "type": "object",
+                "properties": {"side": {"type": "string", "enum": ["right", "left"]}},
+            },
+        },
+    },
+    {
+        "type": "function",
+        "function": {
+            "name": "calibration_count_samples",
+            "description": "Numero di sample attualmente memorizzati per il braccio indicato.",
+            "parameters": {
+                "type": "object",
+                "properties": {"side": {"type": "string", "enum": ["right", "left"]}},
+            },
+        },
+    },
+]
+
+
+def dispatch_tool(name: str, args: dict[str, Any]) -> dict[str, Any]:
+    """Run the named tool with the provided arguments. Always returns a dict.
+
+    The runner serialises this to JSON and feeds it back to the model as a
+    `tool` role message. Any exception is converted to {"ok": false, ...}.
+    """
+    h = _HANDLERS.get(name)
+    if h is None:
+        return _err(f"tool sconosciuto: {name}")
+    t0 = time.perf_counter()
+    try:
+        out = h(args or {})
+    except Exception as e:
+        log.exception("tool %s raised", name)
+        out = _err(f"{type(e).__name__}: {e}")
+    out["_latency_ms"] = round((time.perf_counter() - t0) * 1000.0, 1)
+    return out

src/presa_mano/api/__init__.py

@@ -0,0 +1,27 @@
+# Load `_deps` first so it is registered as an attribute of this package
+# before any of the route modules (which reach into the agent module which
+# in turn imports `from ..api import _deps`) start executing.
+from . import _deps  # noqa: F401
+from . import (
+    agent_routes,
+    calibration_routes,
+    config_routes,
+    grasp_routes,
+    home_routes,
+    perception_routes,
+    posture_routes,
+    robot_routes,
+    wrist_routes,
+)
+
+__all__ = [
+    "agent_routes",
+    "calibration_routes",
+    "config_routes",
+    "grasp_routes",
+    "home_routes",
+    "perception_routes",
+    "posture_routes",
+    "robot_routes",
+    "wrist_routes",
+]

src/presa_mano/api/_deps.py

@@ -0,0 +1,96 @@
+"""Build clients & helpers from current config — called per-request so runtime
+updates (PUT /api/config) take effect without restart."""
+
+from __future__ import annotations
+
+from pathlib import Path
+from typing import Any
+
+from ..clients import G1Client, MarkerClient, NuwaClient, YoloClient
+from ..config import data_dir, get_config
+
+
+def _svc(cfg: dict[str, Any], key: str) -> dict[str, Any]:
+    return cfg.get("services", {}).get(key, {})
+
+
+def nuwa() -> NuwaClient:
+    c = _svc(get_config(), "nuwa")
+    return NuwaClient(c["base_url"], timeout_s=c.get("timeout_s", 5.0), api_key=c.get("api_key"))
+
+
+def marker() -> MarkerClient:
+    c = _svc(get_config(), "marker_finder")
+    return MarkerClient(c["base_url"], timeout_s=c.get("timeout_s", 5.0))
+
+
+def yolo() -> YoloClient:
+    c = _svc(get_config(), "yolo")
+    return YoloClient(c["base_url"], timeout_s=c.get("timeout_s", 10.0))
+
+
+def active_robot_key() -> str:
+    """Which robot the orchestrator is currently bound to ('g1' or 'h1').
+    Read from `robot.active` in config; defaults to 'g1' for backward compat."""
+    return str(get_config().get("robot", {}).get("active", "g1")).lower()
+
+
+def g1(dry_run: bool | None = None) -> G1Client:
+    """Returns the client for the currently-active robot service. Despite the
+    historical name, this picks `services.g1` or `services.h1` based on
+    `robot.active`. Both robots speak the same HTTP API (G1_API_Server)."""
+    cfg = get_config()
+    key = active_robot_key()
+    c = _svc(cfg, key)
+    if not c:
+        # Fallback to legacy g1 block if the active key is missing — keeps the
+        # orchestrator usable on configs that predate the dual-robot split.
+        c = _svc(cfg, "g1")
+    if dry_run is None:
+        dry_run = bool(cfg.get("robot", {}).get("dry_run", False))
+    return G1Client(
+        c["base_url"],
+        timeout_s=c.get("timeout_s", 5.0),
+        write_timeout_s=c.get("write_timeout_s"),
+        dry_run=dry_run,
+    )
+
+
+def _resolve_data_path(rel: str) -> Path:
+    p = Path(rel)
+    if p.is_absolute():
+        return p
+    return data_dir() / Path(rel).name if rel.startswith("data/") else data_dir() / rel
+
+
+def _per_side(p: Path, side: str) -> Path:
+    """Return <stem>_<side><suffix> in the same directory as p."""
+    return p.with_name(f"{p.stem}_{side}{p.suffix}")
+
+
+def default_side() -> str:
+    return str(get_config().get("robot", {}).get("wrist_side", "right")).lower()
+
+
+def calibration_file_path(side: str | None = None) -> Path:
+    """Calibration matrix file for the given side. The hand-eye matrix is
+    arm-specific (the camera is fixed but each arm has its own kinematic chain),
+    so we keep right/left in separate files (calibration_right.yaml / _left.yaml)."""
+    cfg = get_config()
+    rel = cfg.get("calibration", {}).get("file", "data/calibration.yaml")
+    base = _resolve_data_path(rel)
+    s = (side or default_side()).lower()
+    return _per_side(base, s)
+
+
+def samples_file_path(side: str | None = None) -> Path:
+    cfg = get_config()
+    rel = cfg.get("calibration", {}).get("samples_file", "data/samples.json")
+    base = _resolve_data_path(rel)
+    s = (side or default_side()).lower()
+    return _per_side(base, s)
+
+
+def depth_correction_path() -> Path:
+    """Where the depth-bias correction state (params + samples) is persisted."""
+    return data_dir() / "depth_correction.json"

src/presa_mano/api/agent_routes.py

@@ -0,0 +1,203 @@
+"""AI assistant routes.
+
+- `POST /api/agent/chat`  — send a user message, get back the assistant reply
+  and the trace of every tool the model invoked. Stateless on the server side:
+  the GUI keeps the rolling history and posts it back in `history`.
+
+- `GET  /api/agent/tools` — list of tools exposed to the model (debug aid).
+
+- `GET/POST/DELETE /api/agent/tasks` — persistent registry of named prompt
+  templates so frequently-used tasks ("calibra braccio dx", "sposta a home
+  position", …) can be re-launched in one click without retyping the prompt.
+"""
+
+from __future__ import annotations
+
+import json
+import re
+import time
+from dataclasses import asdict
+from typing import Any, Optional
+
+from fastapi import APIRouter, HTTPException
+from pydantic import BaseModel, Field
+
+# Submodule imports (not `from ..agent import …`) to break a circular import:
+# api/__init__.py is loaded first by main.py and pulls in agent_routes; if we
+# imported via the agent package, agent/__init__.py would try to load
+# agent.runner → agent.tools → ..api._deps while ..api is still mid-init.
+from ..agent.runner import AgentRunner, OpenRouterError
+from ..agent.tools import TOOL_DEFINITIONS
+from ..config import data_dir, get_config
+
+router = APIRouter()
+
+
+# ---------- chat ----------
+
+class ChatMessage(BaseModel):
+    role: str
+    content: Optional[str] = None
+    # Pass-through fields used by the OpenAI tool-calling protocol.
+    tool_calls: Optional[list[dict[str, Any]]] = None
+    tool_call_id: Optional[str] = None
+    name: Optional[str] = None
+
+
+class ChatReq(BaseModel):
+    message: str = Field(..., min_length=1, max_length=4000)
+    history: list[ChatMessage] = Field(default_factory=list)
+
+
+@router.get("/tools")
+def list_tools() -> dict[str, Any]:
+    """Tools exposed to the LLM. Useful for debugging the system prompt."""
+    return {"tools": TOOL_DEFINITIONS}
+
+
+@router.get("/health")
+def agent_health() -> dict[str, Any]:
+    """Reports whether OpenRouter credentials are available and which model
+    is configured. The frontend uses this to grey-out the chat input when
+    the env var is missing."""
+    runner = AgentRunner()
+    return {
+        "configured": runner.has_credentials(),
+        "model": runner.model,
+        "api_key_env": runner.api_key_env,
+        "base_url": runner.base_url,
+        "temperature": runner.temperature,
+        "max_tool_iterations": runner.max_tool_iterations,
+        "timeout_s": runner.timeout_s,
+    }
+
+
+@router.get("/system_prompt_preview")
+def system_prompt_preview() -> dict[str, Any]:
+    """Returns the exact system prompt the model will receive at the next
+    chat. Useful for the AI Settings tab so the operator can preview the
+    effect of editing chat.openrouter.user_prompt_addendum or any other
+    state that influences the prompt (active robot, marker config…)."""
+    from ..agent.prompt import build_system_prompt
+    return {"system_prompt": build_system_prompt()}
+
+
+# Curated list shown in the AI Settings model selector. Plus a free-text
+# field on the GUI side for anything not in the list. Keep the list short
+# and focused on models that we've verified support OpenRouter's tool-use.
+_KNOWN_MODELS = [
+    {"id": "anthropic/claude-sonnet-4.5",        "label": "Claude Sonnet 4.5 (consigliato)"},
+    {"id": "anthropic/claude-opus-4.1",          "label": "Claude Opus 4.1 (più capace, più caro)"},
+    {"id": "anthropic/claude-haiku-4.5",         "label": "Claude Haiku 4.5 (veloce, economico)"},
+    {"id": "openai/gpt-4o",                       "label": "GPT-4o"},
+    {"id": "openai/gpt-4o-mini",                  "label": "GPT-4o mini"},
+    {"id": "google/gemini-2.5-pro",              "label": "Gemini 2.5 Pro"},
+    {"id": "google/gemini-2.5-flash",            "label": "Gemini 2.5 Flash"},
+    {"id": "meta-llama/llama-3.3-70b-instruct",  "label": "Llama 3.3 70B"},
+    {"id": "mistralai/mistral-large-2411",        "label": "Mistral Large 2411"},
+]
+
+
+@router.get("/known_models")
+def known_models() -> dict[str, Any]:
+    """Curated short-list of OpenRouter models that work with our tool-calling.
+    The GUI also offers a free-text input for anything not in this list."""
+    return {"models": _KNOWN_MODELS}
+
+
+@router.post("/chat")
+def chat(req: ChatReq) -> dict[str, Any]:
+    runner = AgentRunner()
+    if not runner.has_credentials():
+        raise HTTPException(status_code=412,
+                            detail=f"OpenRouter API key non impostata (env var {runner.api_key_env}).")
+    history = [m.model_dump(exclude_none=True) for m in req.history]
+    try:
+        result, updated_history = runner.run(req.message, history=history)
+    except OpenRouterError as e:
+        raise HTTPException(status_code=502, detail=str(e))
+    return {
+        "reply": result.reply,
+        "iterations": result.iterations,
+        "finish_reason": result.finish_reason,
+        "model": result.model,
+        "usage": result.usage,
+        "tool_calls": [asdict(t) for t in result.tool_calls],
+        "history": updated_history,
+    }
+
+
+# ---------- saved task templates ----------
+
+_NAME_RE = re.compile(r"^[a-zA-Z0-9 _\-àèéìòù]{1,80}$")
+
+
+class TaskItem(BaseModel):
+    name: str = Field(..., min_length=1, max_length=80)
+    prompt: str = Field(..., min_length=1, max_length=4000)
+    description: Optional[str] = Field(None, max_length=400)
+
+
+def _tasks_path():
+    return data_dir() / "agent_tasks.json"
+
+
+def _load_tasks() -> list[dict[str, Any]]:
+    p = _tasks_path()
+    if not p.exists():
+        return []
+    try:
+        with open(p, "r", encoding="utf-8") as f:
+            data = json.load(f)
+        return data if isinstance(data, list) else []
+    except json.JSONDecodeError:
+        return []
+
+
+def _save_tasks(items: list[dict[str, Any]]) -> None:
+    p = _tasks_path()
+    p.parent.mkdir(parents=True, exist_ok=True)
+    with open(p, "w", encoding="utf-8") as f:
+        json.dump(items, f, ensure_ascii=False, indent=2)
+
+
+@router.get("/tasks")
+def list_tasks() -> dict[str, Any]:
+    return {"tasks": _load_tasks()}
+
+
+@router.post("/tasks")
+def save_task(req: TaskItem) -> dict[str, Any]:
+    if not _NAME_RE.match(req.name):
+        raise HTTPException(status_code=422, detail="nome non valido (max 80 char, alfanumerici/spazi/_-)")
+    items = _load_tasks()
+    now = time.time()
+    # Replace by name if already exists, otherwise append.
+    found = False
+    for it in items:
+        if it.get("name") == req.name:
+            it["prompt"] = req.prompt
+            it["description"] = req.description
+            it["updated_at"] = now
+            found = True
+            break
+    if not found:
+        items.append({
+            "name": req.name,
+            "prompt": req.prompt,
+            "description": req.description,
+            "created_at": now,
+            "updated_at": now,
+        })
+    _save_tasks(items)
+    return {"ok": True, "total": len(items), "name": req.name}
+
+
+@router.delete("/tasks/{name}")
+def delete_task(name: str) -> dict[str, Any]:
+    items = _load_tasks()
+    new_items = [it for it in items if it.get("name") != name]
+    if len(new_items) == len(items):
+        raise HTTPException(status_code=404, detail=f"task '{name}' non trovata")
+    _save_tasks(new_items)
+    return {"ok": True, "total": len(new_items)}

src/presa_mano/api/calibration_routes.py

@@ -0,0 +1,474 @@
+"""Calibration endpoints."""
+
+from __future__ import annotations
+
+import logging
+import time
+from dataclasses import asdict
+from typing import Any, Optional
+
+import numpy as np
+from fastapi import APIRouter, HTTPException, Query, Response
+from pydantic import BaseModel, Field
+
+from ..calibration import CalibrationSampler, CalibrationStore, HandEyeSolver, SamplesStore
+from ..calibration.auto_job import AutoRunner, build_grid, get_job as _auto_job
+from ..clients import G1Unavailable
+from ..config import get_config
+from ..timing import Stopwatch
+from . import _deps
+
+log = logging.getLogger(__name__)
+
+router = APIRouter()
+
+
+class SampleReq(BaseModel):
+    marker_id: Optional[int] = None
+    size_mm: Optional[float] = None
+    side: Optional[str] = None
+    note: str = ""
+
+
+class SolveReq(BaseModel):
+    method: Optional[str] = Field(None, description="TSAI | PARK | HORAUD | ANDREFF | DANIILIDIS")
+    side: Optional[str] = Field(None, description="right|left (default: robot.wrist_side)")
+    sanity_check: bool = Field(True, description="drop samples whose pair-angle or T_wrist_marker are outliers")
+    pair_angle_tol_deg: float = Field(8.0, ge=1.0, le=45.0)
+    wm_translation_mad_factor: float = Field(3.5, ge=1.0, le=10.0)
+
+
+def _samples_store(side: str | None = None) -> SamplesStore:
+    return SamplesStore(_deps.samples_file_path(side))
+
+
+def _calib_store(side: str | None = None) -> CalibrationStore:
+    return CalibrationStore(_deps.calibration_file_path(side))
+
+
+@router.post("/sample")
+def acquire_sample(req: SampleReq) -> dict[str, Any]:
+    sw = Stopwatch()
+    cfg = get_config()
+    marker_id = req.marker_id if req.marker_id is not None else cfg["aruco"]["wrist_marker"]["id"]
+    size_mm = req.size_mm if req.size_mm is not None else cfg["aruco"]["wrist_marker"]["size_mm"]
+    side = req.side or cfg["robot"]["wrist_side"]
+
+    sampler = CalibrationSampler(_deps.nuwa(), _deps.marker(), _deps.g1())
+    try:
+        sample = sampler.acquire(marker_id=marker_id, size_mm=size_mm, side=side, note=req.note)
+    except Exception as e:
+        raise HTTPException(status_code=422, detail=str(e))
+    with sw.step("persist"):
+        items = _samples_store(side).append(sample)
+    return {"sample": asdict(sample), "total": len(items), "side": side,
+            "timings_ms": {**(sample.timings_ms or {}), **sw.as_dict()}}
+
+
+@router.get("/samples")
+def list_samples(side: Optional[str] = Query(None, description="right|left (default: robot.wrist_side)")) -> dict[str, Any]:
+    side = side or _deps.default_side()
+    items = _samples_store(side).load()
+    return {"total": len(items), "side": side, "samples": items}
+
+
+@router.delete("/samples")
+def clear_samples(side: Optional[str] = Query(None)) -> dict[str, Any]:
+    side = side or _deps.default_side()
+    _samples_store(side).clear()
+    return {"ok": True, "total": 0, "side": side}
+
+
+@router.post("/solve")
+def solve(req: SolveReq) -> dict[str, Any]:
+    sw = Stopwatch()
+    cfg = get_config()
+    method = (req.method or cfg.get("calibration", {}).get("method") or "TSAI").upper()
+    side = (req.side or _deps.default_side()).lower()
+    min_samples = int(cfg.get("calibration", {}).get("min_samples", 5))
+    with sw.step("load_samples"):
+        samples = _samples_store(side).load()
+    if len(samples) < min_samples:
+        raise HTTPException(status_code=422,
+                            detail=f"need at least {min_samples} samples for side='{side}', have {len(samples)}")
+    try:
+        with sw.step("hand_eye"):
+            result = HandEyeSolver.solve(
+                samples, method=method,
+                sanity_check=req.sanity_check,
+                pair_angle_tol_deg=req.pair_angle_tol_deg,
+                wm_translation_mad_factor=req.wm_translation_mad_factor,
+            )
+    except Exception as e:
+        raise HTTPException(status_code=422, detail=f"hand-eye solve failed: {e}")
+    with sw.step("persist"):
+        _calib_store(side).save(result)
+    result["side"] = side
+    result["timings_ms"] = sw.as_dict()
+    return result
+
+
+@router.get("/result")
+def current_calibration(side: Optional[str] = Query(None)) -> dict[str, Any]:
+    side = side or _deps.default_side()
+    doc = _calib_store(side).load()
+    if doc is None:
+        raise HTTPException(status_code=404, detail=f"no calibration saved yet for side='{side}'")
+    doc["side"] = side
+    return doc
+
+
+# --- Live preview helpers (RGB proxy + marker overlay) --------------------
+
+@router.get("/rgb.jpg")
+def rgb_proxy() -> Response:
+    """Proxy Nuwa RGB stream to avoid CORS issues in the GUI."""
+    nuwa = _deps.nuwa()
+    try:
+        jpeg = nuwa.rgb_jpeg()
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"Nuwa RGB failed: {e}")
+    return Response(content=jpeg, media_type="image/jpeg",
+                    headers={"Cache-Control": "no-store"})
+
+
+class PreviewReq(BaseModel):
+    marker_id: Optional[int] = None
+    size_mm: Optional[float] = None
+
+
+@router.post("/preview")
+def preview_marker(req: PreviewReq) -> dict[str, Any]:
+    """Detect the wrist marker on the current RGB frame and return its corners
+    (for drawing overlays in the GUI) + pose in camera frame.
+
+    Marker-finder runs the PnP server-side using the Nuwa intrinsics that
+    presa-mano pushes at startup, so we just relay its result."""
+    sw = Stopwatch()
+    cfg = get_config()
+    marker_id = req.marker_id if req.marker_id is not None else cfg["aruco"]["wrist_marker"]["id"]
+    size_mm = req.size_mm if req.size_mm is not None else cfg["aruco"]["wrist_marker"]["size_mm"]
+
+    try:
+        with sw.step("marker_detect"):
+            result = _deps.marker().detect_url(
+                markers=[{"id": int(marker_id), "size_mm": float(size_mm)}],
+                image_url=_deps.nuwa().rgb_url(),
+            )
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"marker-finder failed: {e}")
+
+    found = result.get("found", [])
+    entry = next((m for m in found if int(m["id"]) == int(marker_id)), None)
+    if entry is None:
+        return {"found": False, "marker_id": marker_id,
+                "not_found": result.get("not_found", []),
+                "timings_ms": sw.as_dict()}
+
+    img_pts = np.asarray(entry["corners"], dtype=np.float64).reshape(-1, 2)
+    tvec = np.asarray(entry.get("position_m") or [0, 0, 0], dtype=np.float64)
+    return {
+        "found": True,
+        "marker_id": marker_id,
+        "corners": entry["corners"],
+        "center_px": img_pts.mean(axis=0).tolist(),
+        "pose_camera": {
+            "rvec": entry.get("rvec"),
+            "tvec": tvec.tolist(),
+            "distance_m": float(np.linalg.norm(tvec)),
+            "quaternion_xyzw": entry.get("quaternion_xyzw"),
+            "source": "marker_finder",
+        },
+        "timings_ms": sw.as_dict(),
+    }
+
+
+# --- Future: scripted grid acquisition --------------------------------------
+
+class AutoGridReq(BaseModel):
+    """Automated calibration by visiting a 3D grid of wrist poses.
+
+    Default: 3x3 (X,Y) at 3 heights = 27 waypoints.
+    Z is clamped so it never drops below `min_z_m` to avoid hitting the table.
+    """
+    start_position: list[float] = Field(..., min_length=3, max_length=3)
+    orientation_quaternion_xyzw: list[float] = Field(..., min_length=4, max_length=4)
+    step_x_m: float = 0.05
+    step_y_m: float = 0.05
+    step_z_m: float = 0.05
+    n_x: int = 3
+    n_y: int = 3
+    n_z: int = 3
+    min_z_m: float = Field(..., description="floor/table safety — z is clamped to >= this value")
+    settle_s: float = 1.0
+    side: Optional[str] = None
+    dry_run: bool = True
+    auto_solve: bool = True
+
+
+@router.post("/auto_grid")
+def auto_grid(req: AutoGridReq) -> dict[str, Any]:
+    """Move the wrist through a grid of waypoints, acquiring a sample at each.
+
+    While the G1 DDS is unstable, keep dry_run=True to just preview the waypoints.
+    """
+    cfg = get_config()
+    side = req.side or cfg["robot"]["wrist_side"]
+    g1 = _deps.g1(dry_run=req.dry_run)
+    sampler = CalibrationSampler(_deps.nuwa(), _deps.marker(), _deps.g1(dry_run=False))
+    store = _samples_store(side)
+
+    x0, y0, z0 = req.start_position
+    waypoints: list[list[float]] = []
+    for iz in range(req.n_z):
+        for iy in range(req.n_y):
+            for ix in range(req.n_x):
+                x = x0 + (ix - (req.n_x - 1) / 2.0) * req.step_x_m
+                y = y0 + (iy - (req.n_y - 1) / 2.0) * req.step_y_m
+                z = max(req.min_z_m, z0 + iz * req.step_z_m)
+                waypoints.append([x, y, z])
+
+    results: list[dict[str, Any]] = []
+    for i, pos in enumerate(waypoints):
+        try:
+            move = g1.wrist_absolute(side=side, position=pos,
+                                     quaternion=req.orientation_quaternion_xyzw,
+                                     wait=True, timeout_s=req.settle_s * 5)
+        except G1Unavailable as e:
+            results.append({"idx": i, "position": pos, "error": f"G1 move failed: {e}"})
+            continue
+        time.sleep(req.settle_s)
+        if req.dry_run:
+            results.append({"idx": i, "position": pos, "move": move, "sample": None, "dry_run": True})
+            continue
+        try:
+            sample = sampler.acquire(
+                marker_id=cfg["aruco"]["wrist_marker"]["id"],
+                size_mm=cfg["aruco"]["wrist_marker"]["size_mm"],
+                side=side,
+                note=f"auto_grid[{i}]",
+            )
+            store.append(sample)
+            results.append({"idx": i, "position": pos, "move": move, "sample": asdict(sample)})
+        except Exception as e:
+            log.warning("auto_grid sample %d failed: %s", i, e)
+            results.append({"idx": i, "position": pos, "move": move, "error": str(e)})
+
+    out: dict[str, Any] = {"waypoints": waypoints, "results": results, "dry_run": req.dry_run}
+
+    if req.auto_solve and not req.dry_run:
+        samples = store.load()
+        if len(samples) >= int(cfg.get("calibration", {}).get("min_samples", 5)):
+            try:
+                method = (cfg.get("calibration", {}).get("method") or "TSAI").upper()
+                solve_result = HandEyeSolver.solve(samples, method=method)
+                _calib_store(side).save(solve_result)
+                out["calibration"] = solve_result
+            except Exception as e:
+                out["solve_error"] = str(e)
+    out["side"] = side
+    return out
+
+
+# --- Automated calibration procedure ---------------------------------------
+# Grid + wrist rotation search, background job with polled status. Frontend
+# (tab "Auto-Calibrazione") renders the waypoints on top of the 3D URDF.
+
+
+class AutoStartReq(BaseModel):
+    # Starting wrist pose: taken from G1 live if omitted.
+    start_position: Optional[list[float]] = Field(None, min_length=3, max_length=3)
+    start_quaternion_xyzw: Optional[list[float]] = Field(None, min_length=4, max_length=4)
+    # Grid span (mm) and number of divisions per axis.
+    delta_x_mm: float = Field(..., ge=0.0, le=500.0)
+    delta_y_mm: float = Field(..., ge=0.0, le=500.0)
+    delta_z_mm: float = Field(..., ge=0.0, le=500.0)
+    # Offset (mm) applied to the live wrist pose before centring the grid.
+    # Lets the operator displace the grid centre from the current pose
+    # without physically moving the wrist first.
+    offset_x_mm: float = Field(0.0, ge=-500.0, le=500.0)
+    offset_y_mm: float = Field(0.0, ge=-500.0, le=500.0)
+    offset_z_mm: float = Field(0.0, ge=-500.0, le=500.0)
+    n_x: int = Field(3, ge=1, le=7)
+    n_y: int = Field(3, ge=1, le=7)
+    n_z: int = Field(3, ge=1, le=7)
+    # Wrist rotation search range (degrees) when marker not found at nominal pose.
+    wrist_rot_delta_deg: float = Field(10.0, ge=0.0, le=45.0)
+    min_z_m: float = 0.05
+    side: Optional[str] = None
+    settle_s: float = Field(1.0, ge=0.1, le=5.0)
+    move_timeout_s: float = Field(8.0, ge=1.0, le=30.0)
+    auto_solve: bool = True
+    dry_run: bool = False
+    order: str = Field("serpentine", pattern="^(nested|serpentine)$")
+
+
+class AutoFeasibilityReq(AutoStartReq):
+    """Same shape as start, used only to preview the grid."""
+    pass
+
+
+class AutoVerifyReq(AutoStartReq):
+    """Real reachability verification — same grid, robot moves to each point
+    but does NOT acquire samples. Faster than full procedure, definitive."""
+    pass
+
+
+def _resolve_start_pose(req: AutoStartReq, side: str) -> tuple[list[float], list[float]]:
+    """If caller did not supply a start pose, read the current wrist pose from G1."""
+    if req.start_position and req.start_quaternion_xyzw:
+        return list(req.start_position), list(req.start_quaternion_xyzw)
+    try:
+        w = _deps.g1().wrist()
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"cannot read wrist pose: {e}")
+    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 HTTPException(status_code=422, detail=f"G1 /wrist missing '{side}' pose")
+    return list(pose["position"]), list(pose["quaternion"])
+
+
+def _grid_from_req(req: AutoStartReq, side: str) -> tuple[list, list[float], list[float], list[float]]:
+    """Returns (grid, home_position, home_quaternion, center_position).
+
+    home_pose is the actual live wrist pose — used for return-to-home on stop.
+    center_position is home_position + (offset_x, offset_y, offset_z), which
+    is what the grid is actually centred on."""
+    home_p, home_q = _resolve_start_pose(req, side)
+    center_p = [
+        home_p[0] + req.offset_x_mm / 1000.0,
+        home_p[1] + req.offset_y_mm / 1000.0,
+        home_p[2] + req.offset_z_mm / 1000.0,
+    ]
+    grid = build_grid(
+        start_position=np.asarray(center_p),
+        start_quaternion=np.asarray(home_q),
+        delta_m=(req.delta_x_mm / 1000.0, req.delta_y_mm / 1000.0, req.delta_z_mm / 1000.0),
+        divisions=(req.n_x, req.n_y, req.n_z),
+        min_z_m=req.min_z_m,
+        order=req.order,
+    )
+    return grid, home_p, home_q, center_p
+
+
+@router.post("/auto/feasibility")
+def auto_feasibility(req: AutoFeasibilityReq) -> dict[str, Any]:
+    """Build the grid and return per-point reachability — no motion."""
+    from dataclasses import asdict as _asdict
+    cfg = get_config()
+    side = req.side or cfg["robot"]["wrist_side"]
+    grid, home_p, home_q, center_p = _grid_from_req(req, side)
+    return {
+        "side": side,
+        "start_position": home_p,
+        "start_quaternion_xyzw": home_q,
+        "center_position": center_p,
+        "offset_mm": [req.offset_x_mm, req.offset_y_mm, req.offset_z_mm],
+        "wrist_rot_delta_deg": req.wrist_rot_delta_deg,
+        "waypoints": [_asdict(w) for w in grid],
+        "n_reachable": sum(1 for w in grid if w.reachable),
+        "n_unreachable": sum(1 for w in grid if not w.reachable),
+    }
+
+
+def _make_runner(req: AutoStartReq, side: str, *, feasibility_only: bool):
+    cfg = get_config()
+    return AutoRunner(
+        side=side,
+        marker_id=int(cfg["aruco"]["wrist_marker"]["id"]),
+        size_mm=float(cfg["aruco"]["wrist_marker"]["size_mm"]),
+        wrist_rot_delta_rad=float(np.deg2rad(req.wrist_rot_delta_deg)),
+        settle_s=req.settle_s,
+        move_timeout_s=req.move_timeout_s,
+        g1_move=_deps.g1(dry_run=req.dry_run).wrist_absolute,
+        sampler=CalibrationSampler(_deps.nuwa(), _deps.marker(), _deps.g1(dry_run=False)),
+        store=_samples_store(side),
+        feasibility_only=feasibility_only,
+    )
+
+
+@router.post("/auto/start")
+def auto_start(req: AutoStartReq) -> dict[str, Any]:
+    job = _auto_job()
+    if job.is_running():
+        raise HTTPException(status_code=409, detail="another auto job is already running")
+    cfg = get_config()
+    side = req.side or cfg["robot"]["wrist_side"]
+    grid, home_p, home_q, center_p = _grid_from_req(req, side)
+    runner = _make_runner(req, side, feasibility_only=False)
+
+    def on_finish(state) -> None:
+        if not req.auto_solve:
+            return
+        samples = _samples_store(side).load()
+        min_n = int(cfg.get("calibration", {}).get("min_samples", 5))
+        if len(samples) < min_n:
+            return
+        try:
+            method = (cfg.get("calibration", {}).get("method") or "TSAI").upper()
+            result = HandEyeSolver.solve(samples, method=method)
+            _calib_store(side).save(result)
+            state.calibration = result
+        except Exception as e:
+            log.warning("auto_solve after auto-calibration failed: %s", e)
+
+    params = {
+        "side": side,
+        "start_position": home_p,
+        "start_quaternion_xyzw": home_q,
+        "center_position": center_p,
+        "offset_mm": [req.offset_x_mm, req.offset_y_mm, req.offset_z_mm],
+        "delta_mm": [req.delta_x_mm, req.delta_y_mm, req.delta_z_mm],
+        "divisions": [req.n_x, req.n_y, req.n_z],
+        "wrist_rot_delta_deg": req.wrist_rot_delta_deg,
+        "min_z_m": req.min_z_m,
+        "dry_run": req.dry_run,
+        "auto_solve": req.auto_solve,
+        "order": req.order,
+        "mode": "calibration",
+    }
+    job.start(waypoints=grid, runner=runner, on_finish=on_finish, params=params,
+              home_pose=(home_p, home_q))
+    return {"ok": True, "status": "running", "n_waypoints": len(grid), "params": params}
+
+
+@router.post("/auto/verify_reachability")
+def auto_verify_reachability(req: AutoVerifyReq) -> dict[str, Any]:
+    """Run the robot through the grid WITHOUT acquiring samples. The reached_target
+    flag from each move is the ground truth for reachability — slow but definitive."""
+    job = _auto_job()
+    if job.is_running():
+        raise HTTPException(status_code=409, detail="another auto job is already running")
+    cfg = get_config()
+    side = req.side or cfg["robot"]["wrist_side"]
+    grid, home_p, home_q, center_p = _grid_from_req(req, side)
+    runner = _make_runner(req, side, feasibility_only=True)
+    params = {
+        "side": side,
+        "start_position": home_p,
+        "start_quaternion_xyzw": home_q,
+        "center_position": center_p,
+        "offset_mm": [req.offset_x_mm, req.offset_y_mm, req.offset_z_mm],
+        "delta_mm": [req.delta_x_mm, req.delta_y_mm, req.delta_z_mm],
+        "divisions": [req.n_x, req.n_y, req.n_z],
+        "order": req.order,
+        "dry_run": req.dry_run,
+        "mode": "verify_reachability",
+    }
+    job.start(waypoints=grid, runner=runner, params=params,
+              home_pose=(home_p, home_q))
+    return {"ok": True, "status": "running", "n_waypoints": len(grid), "params": params}
+
+
+@router.get("/auto/status")
+def auto_status() -> dict[str, Any]:
+    return _auto_job().snapshot()
+
+
+@router.post("/auto/stop")
+def auto_stop() -> dict[str, Any]:
+    """Stop the current job. If the job was started with a home_pose, the
+    worker returns the wrist there before finishing."""
+    stopped = _auto_job().stop()
+    return {"ok": True, "was_running": stopped}

src/presa_mano/api/config_routes.py

@@ -0,0 +1,21 @@
+"""GET/PUT /api/config — read the merged config and apply runtime overrides."""
+
+from __future__ import annotations
+
+from typing import Any
+
+from fastapi import APIRouter, Body
+
+from ..config import get_config, update_config
+
+router = APIRouter()
+
+
+@router.get("")
+def read_config() -> dict[str, Any]:
+    return get_config()
+
+
+@router.put("")
+def write_config(patch: dict[str, Any] = Body(...)) -> dict[str, Any]:
+    return update_config(patch)

src/presa_mano/api/grasp_routes.py

@@ -0,0 +1,319 @@
+"""Grasp planning + execution endpoints."""
+
+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 ..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_routes import _bbox_to_roi
+from ..timing import Stopwatch
+from . import _deps
+
+router = APIRouter()
+
+
+class PlanReq(BaseModel):
+    conf: Optional[float] = None
+    class_name: Optional[str] = None
+
+
+class ExecuteReq(PlanReq):
+    dry_run: bool = True
+
+
+def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopwatch
+                           ) -> tuple[np.ndarray, dict[str, Any]]:
+    cfg = get_config()
+    nuwa = _deps.nuwa()
+    yolo = _deps.yolo()
+
+    side = cfg.get("robot", {}).get("wrist_side", "right")
+    T_base_cam = CalibrationStore(_deps.calibration_file_path(side)).T_base_camera()
+    if T_base_cam is None:
+        raise HTTPException(status_code=412, detail=f"calibration missing for side='{side}': run /api/calibration/solve first")
+
+    detector = ObjectDetector(yolo, nuwa)
+    try:
+        with sw.step("yolo"):
+            dets = detector.detect(conf=conf)
+    except HTTPException:
+        raise
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}")
+    if class_name:
+        dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()]
+    if not dets:
+        raise HTTPException(status_code=404, detail="no object detected")
+    dets.sort(key=lambda d: d["confidence"], reverse=True)
+    top = dets[0]
+    b = top["bbox"]
+
+    u_min, v_min, u_max, v_max = _bbox_to_roi(b)
+    try:
+        with sw.step("pointcloud_roi"):
+            pc, _meta = nuwa.pointcloud_roi(
+                u_min, v_min, u_max, v_max,
+                step=int(cfg.get("grasp", {}).get("pointcloud_step", 1)),
+                max_depth_mm=int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000)),
+            )
+    except Exception as e:
+        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)}")
+
+    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, {"detection": top, "T_base_camera": T_base_cam.tolist(), "n_points": len(pts_base)}
+
+
+@router.post("/plan")
+def plan(req: PlanReq) -> dict[str, Any]:
+    sw = Stopwatch()
+    cfg = get_config()
+    pts_base, meta = _object_points_in_base(req.conf, req.class_name, sw)
+    with sw.step("pca_axes"):
+        axes = compute_axes(pts_base)
+    planner = GraspPlanner(
+        approach_offset_m=float(cfg["grasp"]["approach_offset_m"]),
+        max_object_width_m=float(cfg["grasp"]["max_object_width_m"]),
+    )
+    with sw.step("plan"):
+        g = planner.plan(axes)
+    return {"plan": g.to_dict(), "timings_ms": sw.as_dict(), **meta}
+
+
+@router.post("/execute")
+def execute(req: ExecuteReq) -> dict[str, Any]:
+    sw = Stopwatch()
+    cfg = get_config()
+    side = cfg["robot"]["wrist_side"]
+    pts_base, meta = _object_points_in_base(req.conf, req.class_name, sw)
+    with sw.step("pca_axes"):
+        axes = compute_axes(pts_base)
+    planner = GraspPlanner(
+        approach_offset_m=float(cfg["grasp"]["approach_offset_m"]),
+        max_object_width_m=float(cfg["grasp"]["max_object_width_m"]),
+    )
+    with sw.step("plan"):
+        g = planner.plan(axes)
+
+    open_action = "RIGHT_OPEN" if side == "right" else "LEFT_OPEN"
+    close_action = "RIGHT_CLOSE" if side == "right" else "LEFT_CLOSE"
+
+    g1 = _deps.g1(dry_run=req.dry_run)
+    steps: list[dict[str, Any]] = []
+    try:
+        with sw.step("hand_open"):
+            steps.append({"step": "hand_open", "result": g1.hand_action(open_action)})
+        with sw.step("pre_grasp"):
+            steps.append({"step": "pre_grasp", "result": g1.wrist_absolute(
+                side=side,
+                position=g.pre_grasp_position.tolist(),
+                quaternion=g.grasp_quaternion.tolist(),
+            )})
+        with sw.step("grasp_approach"):
+            steps.append({"step": "grasp_approach", "result": g1.wrist_absolute(
+                side=side,
+                position=g.grasp_position.tolist(),
+                quaternion=g.grasp_quaternion.tolist(),
+            )})
+        with sw.step("hand_close"):
+            steps.append({"step": "hand_close", "result": g1.hand_action(close_action)})
+    except G1Unavailable as e:
+        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}

src/presa_mano/api/home_routes.py

@@ -0,0 +1,122 @@
+"""Home dashboard endpoints.
+
+Aggregates communication health for every upstream microservice
+(Nuwa, marker-finder, YOLO, and the currently-active robot bridge — G1 or H1)
+and exposes the active-robot selector used by the Home tab in the GUI.
+
+The orchestrator has no hardware of its own, so "is the system healthy" reduces
+to "are the four upstream services reachable". This endpoint pings each one
+and reports status + round-trip latency in a single payload so the GUI can
+render it without making four parallel calls itself.
+"""
+
+from __future__ import annotations
+
+import time
+from typing import Any
+
+import httpx
+from fastapi import APIRouter, HTTPException
+from pydantic import BaseModel, Field
+
+from ..config import get_config, update_config
+from . import _deps
+
+router = APIRouter()
+
+
+def _ping(name: str, base_url: str, path: str, timeout_s: float) -> dict[str, Any]:
+    """Best-effort GET ping. Always returns a record, never raises."""
+    rec: dict[str, Any] = {
+        "name": name,
+        "url": base_url.rstrip("/") + path,
+        "ok": False,
+        "status_code": None,
+        "latency_ms": None,
+        "error": None,
+    }
+    t0 = time.perf_counter()
+    try:
+        with httpx.Client(timeout=timeout_s) as c:
+            r = c.get(rec["url"])
+        rec["latency_ms"] = round((time.perf_counter() - t0) * 1000.0, 1)
+        rec["status_code"] = r.status_code
+        rec["ok"] = r.is_success
+        if not r.is_success:
+            rec["error"] = (r.text or "")[:200]
+    except (httpx.HTTPError, OSError) as e:
+        rec["latency_ms"] = round((time.perf_counter() - t0) * 1000.0, 1)
+        rec["error"] = str(e)[:200]
+    return rec
+
+
+@router.get("/status")
+def status() -> dict[str, Any]:
+    """Aggregate health of every configured upstream microservice."""
+    cfg = get_config()
+    services = cfg.get("services", {})
+    active = _deps.active_robot_key()
+
+    devices: list[dict[str, Any]] = []
+
+    nuwa = services.get("nuwa") or {}
+    if nuwa.get("base_url"):
+        devices.append({
+            "key": "nuwa",
+            "label": "Nuwa-HP60C (camera)",
+            **_ping("nuwa", nuwa["base_url"], "/api/health", float(nuwa.get("timeout_s", 5.0))),
+        })
+
+    marker = services.get("marker_finder") or {}
+    if marker.get("base_url"):
+        # marker-finder has no /health; fall back to the root which returns a
+        # response (typically 404/405) — what matters for "is it reachable" is
+        # that we got *any* HTTP response, so we treat 4xx as reachable too.
+        rec = _ping("marker_finder", marker["base_url"], "/", float(marker.get("timeout_s", 5.0)))
+        if rec["status_code"] is not None:
+            rec["ok"] = True
+            rec["error"] = None
+        devices.append({"key": "marker_finder", "label": "marker-finder (ArUco)", **rec})
+
+    yolo = services.get("yolo") or {}
+    if yolo.get("base_url"):
+        rec = _ping("yolo", yolo["base_url"], "/classes", float(yolo.get("timeout_s", 10.0)))
+        devices.append({"key": "yolo", "label": "YOLO server", **rec})
+
+    for robot_key in ("g1", "h1"):
+        rcfg = services.get(robot_key) or {}
+        if not rcfg.get("base_url"):
+            continue
+        rec = _ping(robot_key, rcfg["base_url"], "/health", float(rcfg.get("timeout_s", 5.0)))
+        devices.append({
+            "key": robot_key,
+            "label": f"Robot {robot_key.upper()}" + (" (attivo)" if robot_key == active else ""),
+            "active": robot_key == active,
+            **rec,
+        })
+
+    overall = "ok" if all(d["ok"] for d in devices) else "degraded" if any(d["ok"] for d in devices) else "down"
+    return {"overall": overall, "active_robot": active, "devices": devices}
+
+
+class ActiveRobotReq(BaseModel):
+    active: str = Field(..., pattern="^(g1|h1)$")
+
+
+@router.get("/robot")
+def get_active_robot() -> dict[str, Any]:
+    cfg = get_config()
+    services = cfg.get("services", {}) or {}
+    return {
+        "active": _deps.active_robot_key(),
+        "available": [k for k in ("g1", "h1") if (services.get(k) or {}).get("base_url")],
+    }
+
+
+@router.put("/robot")
+def set_active_robot(req: ActiveRobotReq) -> dict[str, Any]:
+    services = (get_config().get("services") or {})
+    if not (services.get(req.active) or {}).get("base_url"):
+        raise HTTPException(status_code=400, detail=f"services.{req.active}.base_url not configured")
+    update_config({"robot": {"active": req.active}})
+    return {"active": _deps.active_robot_key()}

src/presa_mano/api/perception_routes.py

@@ -0,0 +1,441 @@
+"""Perception endpoints: YOLO detect + object point cloud in base frame."""
+
+from __future__ import annotations
+
+from typing import Any, Optional
+
+import numpy as np
+from fastapi import APIRouter, HTTPException, Query
+from pydantic import BaseModel
+
+from ..config import get_config
+from ..geometry import transform_points
+from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame
+from ..timing import Stopwatch
+from . import _deps
+
+router = APIRouter()
+
+
+class DetectReq(BaseModel):
+    conf: Optional[float] = None
+    class_name: Optional[str] = None
+    max_points: int = 20000
+
+
+class ScanReq(BaseModel):
+    conf: Optional[float] = None
+    class_name: Optional[str] = None  # if set, point cloud is computed for this class
+    max_points: int = 20000
+    include_points: bool = True       # set false to skip the ROI pointcloud (faster)
+
+
+@router.post("/detect")
+def detect_objects(req: DetectReq) -> dict[str, Any]:
+    sw = Stopwatch()
+    yolo = _deps.yolo()
+    nuwa = _deps.nuwa()
+    detector = ObjectDetector(yolo, nuwa)
+    try:
+        with sw.step("yolo"):
+            dets = detector.detect(conf=req.conf)
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}")
+    if req.class_name:
+        dets = [d for d in dets if (d.get("class") or "").lower() == req.class_name.lower()]
+    return {"detections": dets, "timings_ms": sw.as_dict()}
+
+
+@router.get("/classes")
+def list_classes() -> dict[str, Any]:
+    """Class names exposed by the YOLO model. Empty list if yolo-server is older
+    and does not serve /classes yet — the UI should still work by deriving the
+    classes from /scan detections."""
+    try:
+        items = _deps.yolo().classes()
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"YOLO classes failed: {e}")
+    return {"classes": items}
+
+
+@router.post("/scan")
+def scan(req: ScanReq) -> dict[str, Any]:
+    """One YOLO call → all detections (sorted by confidence) + optional point
+    cloud for the selected class. If class_name is provided the point cloud is
+    computed for its highest-confidence detection; otherwise for the top overall
+    detection."""
+    sw = Stopwatch()
+    yolo = _deps.yolo()
+    nuwa = _deps.nuwa()
+    detector = ObjectDetector(yolo, nuwa)
+    try:
+        with sw.step("yolo"):
+            all_dets = detector.detect(conf=req.conf)
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}")
+
+    detections = sorted(
+        (d for d in all_dets if (d.get("class") or "").strip()),
+        key=lambda d: d["confidence"],
+        reverse=True,
+    )
+
+    out: dict[str, Any] = {
+        "detections": detections,
+        "selected": None,
+        "n_points": 0,
+        "timings_ms": sw.as_dict(),
+    }
+    if not req.include_points or not detections:
+        out["timings_ms"] = sw.as_dict()
+        return out
+
+    target: dict[str, Any] | None = None
+    if req.class_name:
+        target = next(
+            (d for d in detections if (d.get("class") or "").lower() == req.class_name.lower()),
+            None,
+        )
+    if target is None and not req.class_name:
+        target = detections[0]
+    if target is None:
+        # requested class not detected in this frame
+        out["timings_ms"] = sw.as_dict()
+        return out
+
+    bbox = target["bbox"]
+    cfg = get_config()
+    step = int(cfg.get("grasp", {}).get("pointcloud_step", 1))
+    max_depth = int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000))
+    u_min, v_min, u_max, v_max = _bbox_to_roi(bbox)
+    try:
+        with sw.step("pointcloud_roi"):
+            pc, _meta = nuwa.pointcloud_roi(u_min, v_min, u_max, v_max,
+                                            step=step, max_depth_mm=max_depth)
+    except HTTPException:
+        raise
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"Nuwa pointcloud ROI failed: {e}")
+
+    with sw.step("filter_outliers"):
+        cropped = filter_outliers(pc)
+    if len(cropped) > req.max_points:
+        idx = np.random.choice(len(cropped), req.max_points, replace=False)
+        cropped = cropped[idx]
+
+    out["selected"] = target
+    out["n_points"] = int(len(cropped))
+    out["points_camera"] = cropped[:, :3].tolist() if len(cropped) else []
+
+    T_base_cam = _load_calibration()
+    if T_base_cam is not None and len(cropped) > 0:
+        with sw.step("to_base_frame"):
+            pts_cv = nuwa_to_cv_frame(cropped[:, :3])
+            pts_base = transform_points(T_base_cam, pts_cv)
+        out["points_base"] = pts_base.tolist()
+        out["T_base_camera"] = T_base_cam.tolist()
+    out["timings_ms"] = sw.as_dict()
+    return out
+
+
+@router.post("/object_points")
+def object_points(req: DetectReq) -> dict[str, Any]:
+    """Runs YOLO, takes the top detection (or class-filtered), returns its point cloud
+    both in camera frame and in base frame (if calibration is available)."""
+    sw = Stopwatch()
+    yolo = _deps.yolo()
+    nuwa = _deps.nuwa()
+    detector = ObjectDetector(yolo, nuwa)
+    try:
+        with sw.step("yolo"):
+            dets = detector.detect(conf=req.conf)
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}")
+    if req.class_name:
+        dets = [d for d in dets if (d.get("class") or "").lower() == req.class_name.lower()]
+    if not dets:
+        raise HTTPException(status_code=404, detail="no object detected")
+    dets.sort(key=lambda d: d["confidence"], reverse=True)
+    top = dets[0]
+    bbox = top["bbox"]
+
+    cfg = get_config()
+    step = int(cfg.get("grasp", {}).get("pointcloud_step", 1))
+    max_depth = int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000))
+    u_min, v_min, u_max, v_max = _bbox_to_roi(bbox)
+    try:
+        with sw.step("pointcloud_roi"):
+            pc, _meta = nuwa.pointcloud_roi(u_min, v_min, u_max, v_max,
+                                            step=step, max_depth_mm=max_depth)
+    except HTTPException:
+        raise
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"Nuwa pointcloud ROI failed: {e}")
+
+    with sw.step("filter_outliers"):
+        cropped = filter_outliers(pc)
+    if len(cropped) > req.max_points:
+        idx = np.random.choice(len(cropped), req.max_points, replace=False)
+        cropped = cropped[idx]
+
+    out: dict[str, Any] = {
+        "detection": top,
+        "n_points": int(len(cropped)),
+        "points_camera": cropped[:, :3].tolist(),
+    }
+
+    T_base_cam = _load_calibration()
+    if T_base_cam is not None and len(cropped) > 0:
+        with sw.step("to_base_frame"):
+            pts_cv = nuwa_to_cv_frame(cropped[:, :3])
+            pts_base = transform_points(T_base_cam, pts_cv)
+        out["points_base"] = pts_base.tolist()
+        out["T_base_camera"] = T_base_cam.tolist()
+    out["timings_ms"] = sw.as_dict()
+    return out
+
+
+def _load_calibration() -> np.ndarray | None:
+    from ..calibration import CalibrationStore
+    return CalibrationStore(_deps.calibration_file_path(_deps.default_side())).T_base_camera()
+
+
+@router.get("/measure")
+def measure(u: int = Query(..., ge=0), v: int = Query(..., ge=0),
+            radius: int = Query(6, ge=0, le=50)) -> dict[str, Any]:
+    """Back-project a pixel to 3D (camera frame, and base frame if calibrated)."""
+    sw = Stopwatch()
+    try:
+        with sw.step("nuwa_measure"):
+            res = _deps.nuwa().measure(u=u, v=v, radius=radius)
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"Nuwa measure failed: {e}")
+    out: dict[str, Any] = {"camera": res}
+    T_base_cam = _load_calibration()
+    pt = _extract_xyz(res)
+    if T_base_cam is not None and pt is not None:
+        pt_cv = nuwa_to_cv_frame(np.asarray([pt]))[0]
+        base_xyz = (T_base_cam[:3, :3] @ pt_cv + T_base_cam[:3, 3]).tolist()
+        out["base"] = {"x": base_xyz[0], "y": base_xyz[1], "z": base_xyz[2]}
+    out["timings_ms"] = sw.as_dict()
+    return out
+
+
+# ---------- depth-bias correction ----------
+# Sensor depth has a non-trivial offset (see /api/perception/measure_rgb vs
+# PnP on a known marker). We model it as: depth_raw = scale * depth_true +
+# offset_mm and let the user calibrate `scale, offset` from samples.
+# State + samples persisted in data/depth_correction.json.
+
+class DepthCorrectionParams(BaseModel):
+    enabled: Optional[bool] = None
+    scale: Optional[float] = None
+    offset_mm: Optional[float] = None
+
+
+class DepthCorrectionSampleReq(BaseModel):
+    """Add a (raw, true) sample. Either supply both directly, or set
+    `from_marker=True` to acquire one automatically from the current
+    marker (PnP Z taken as ground truth, raw read from depth sensor at
+    the marker centre)."""
+    from_marker: bool = False
+    marker_id: Optional[int] = None
+    size_mm: Optional[float] = None
+    depth_raw_mm: Optional[int] = None
+    depth_true_mm: Optional[int] = None
+    note: str = ""
+
+
+def _load_dc():
+    from ..perception.depth_correction import load
+    return load(_deps.depth_correction_path())
+
+
+def _save_dc(state):
+    from ..perception.depth_correction import save
+    save(_deps.depth_correction_path(), state)
+
+
+@router.get("/depth_correction")
+def get_depth_correction() -> dict[str, Any]:
+    return _load_dc()
+
+
+@router.put("/depth_correction")
+def set_depth_correction(req: DepthCorrectionParams) -> dict[str, Any]:
+    state = _load_dc()
+    if req.enabled is not None:
+        state["enabled"] = bool(req.enabled)
+    if req.scale is not None:
+        state["scale"] = float(req.scale)
+    if req.offset_mm is not None:
+        state["offset_mm"] = float(req.offset_mm)
+    _save_dc(state)
+    return state
+
+
+@router.delete("/depth_correction/samples")
+def clear_dc_samples() -> dict[str, Any]:
+    from ..perception.depth_correction import clear_samples
+    state = clear_samples(_load_dc())
+    _save_dc(state)
+    return state
+
+
+@router.post("/depth_correction/sample")
+def add_dc_sample(req: DepthCorrectionSampleReq) -> dict[str, Any]:
+    """Add a sample to the depth-correction calibration set.
+
+    `from_marker=True`: detect the configured marker, take its PnP-derived
+    Z (in the camera frame) as ground truth, and read the raw sensor depth
+    at the marker centre via the same registration pipeline. The marker
+    must be in view.
+    """
+    from ..perception.depth_correction import add_sample
+    cfg = get_config()
+    state = _load_dc()
+
+    if req.from_marker:
+        from ..perception.depth_to_rgb import (
+            CameraRegistration, back_project_rgb_pixel, fetch_depth_mm,
+        )
+        marker_id = int(req.marker_id or cfg["aruco"]["wrist_marker"]["id"])
+        size_mm = float(req.size_mm or cfg["aruco"]["wrist_marker"]["size_mm"])
+        try:
+            mres = _deps.marker().detect_url(
+                markers=[{"id": marker_id, "size_mm": size_mm}],
+                image_url=_deps.nuwa().rgb_url(),
+            )
+        except Exception as e:
+            raise HTTPException(status_code=502, detail=f"marker-finder failed: {e}")
+        entry = next((m for m in mres.get("found", []) if int(m["id"]) == marker_id), None)
+        if entry is None:
+            raise HTTPException(status_code=422, detail=f"marker id={marker_id} non in vista")
+        pnp_z_m = float(np.asarray(entry["position_m"], dtype=np.float64)[2])
+        # Sample the raw depth at the centre pixel via the registration pipeline
+        # WITHOUT correction (we want the uncorrected sensor reading).
+        center_uv = np.asarray(entry["corners"], dtype=np.float64).reshape(-1, 2).mean(axis=0)
+        nuwa = _deps.nuwa()
+        reg = CameraRegistration.from_nuwa(nuwa.intrinsics())
+        depth = fetch_depth_mm(nuwa)
+        r = back_project_rgb_pixel(center_uv[0], center_uv[1], depth, reg,
+                                   sample_radius=4, depth_correction=None)
+        if not r.get("ok"):
+            raise HTTPException(status_code=422,
+                                detail=f"depth non disponibile al marker: {r.get('error')}")
+        depth_raw = int(r["depth_mm_raw"])
+        depth_true = int(round(pnp_z_m * 1000.0))
+        note = req.note or f"from_marker id={marker_id} size={size_mm}mm pnp_z={depth_true}mm"
+    else:
+        if req.depth_raw_mm is None or req.depth_true_mm is None:
+            raise HTTPException(status_code=422,
+                                detail="senza from_marker servono depth_raw_mm e depth_true_mm")
+        depth_raw = int(req.depth_raw_mm)
+        depth_true = int(req.depth_true_mm)
+        note = req.note
+
+    state = add_sample(state, depth_raw_mm=depth_raw, depth_true_mm=depth_true, note=note)
+    _save_dc(state)
+    return {"ok": True, "n_samples": len(state["samples"]),
+            "added": {"depth_raw_mm": depth_raw, "depth_true_mm": depth_true,
+                      "delta_mm": depth_raw - depth_true, "note": note}}
+
+
+@router.post("/depth_correction/fit")
+def fit_dc() -> dict[str, Any]:
+    """Fit the affine model from collected samples and (optionally) enable it.
+    Updates `scale` and `offset_mm` in the persisted state. Does not change
+    `enabled` — call PUT /depth_correction with `enabled=true` to activate."""
+    from ..perception.depth_correction import fit_affine
+    state = _load_dc()
+    samples = state.get("samples") or []
+    try:
+        fit = fit_affine(samples)
+    except ValueError as e:
+        raise HTTPException(status_code=422, detail=str(e))
+    state["scale"] = fit["scale"]
+    state["offset_mm"] = fit["offset_mm"]
+    state["fit_meta"] = fit
+    _save_dc(state)
+    return state
+
+
+@router.get("/measure_rgb")
+def measure_rgb(u: int = Query(..., ge=0, description="pixel column in the RGB image"),
+                v: int = Query(..., ge=0, description="pixel row in the RGB image"),
+                radius: int = Query(4, ge=0, le=50)) -> dict[str, Any]:
+    """Back-project an **RGB** pixel to 3D, properly handling IR↔RGB
+    registration.
+
+    `Nuwa /api/measure` interprets `(u,v)` as IR-image pixel coords and
+    back-projects with IR intrinsics; for an RGB pixel that yields a wrong
+    point. This endpoint:
+      1. samples depth at the corresponding IR pixel (computed iteratively
+         from the RGB ray + extrinsics);
+      2. back-projects with IR intrinsics;
+      3. transforms to the RGB camera frame.
+
+    Output is in the **OpenCV-convention RGB camera frame** (X-right, Y-down,
+    Z-forward), the same frame the hand-eye matrix `T_base_camera` expects.
+    """
+    sw = Stopwatch()
+    nuwa = _deps.nuwa()
+    try:
+        from ..perception.depth_to_rgb import (
+            CameraRegistration, back_project_rgb_pixel, fetch_depth_mm,
+        )
+        with sw.step("intrinsics"):
+            reg = CameraRegistration.from_nuwa(nuwa.intrinsics())
+        with sw.step("depth_fetch"):
+            depth = fetch_depth_mm(nuwa)
+        with sw.step("back_project"):
+            r = back_project_rgb_pixel(u, v, depth, reg, sample_radius=radius,
+                                       depth_correction=_load_dc())
+    except Exception as e:
+        raise HTTPException(status_code=502, detail=f"measure_rgb failed: {e}")
+    if not r["ok"]:
+        return {"found": False, "rgb_pixel": [u, v], "error": r.get("error"),
+                "timings_ms": sw.as_dict()}
+
+    pt_rgb_m = r["point_rgb_m"]
+    out: dict[str, Any] = {
+        "found": True,
+        "rgb_pixel": r["rgb_pixel"],
+        "ir_pixel": r["ir_pixel"],
+        "depth_mm": r["depth_mm"],
+        "point_rgb_m": {"x": pt_rgb_m[0], "y": pt_rgb_m[1], "z": pt_rgb_m[2]},
+        "point_ir_m":  {"x": r["point_ir_m"][0], "y": r["point_ir_m"][1], "z": r["point_ir_m"][2]},
+        "distance_m": float(np.linalg.norm(pt_rgb_m)),
+    }
+    T_base_cam = _load_calibration()
+    if T_base_cam is not None:
+        pt_arr = np.asarray(pt_rgb_m)
+        base_xyz = (T_base_cam[:3, :3] @ pt_arr + T_base_cam[:3, 3]).tolist()
+        out["point_base_m"] = {"x": base_xyz[0], "y": base_xyz[1], "z": base_xyz[2]}
+    out["timings_ms"] = sw.as_dict()
+    return out
+
+
+def _bbox_to_roi(bbox: dict[str, Any]) -> tuple[int, int, int, int]:
+    """Convert a YOLO bbox {x1,y1,x2,y2} to integer ROI (u_min,v_min,u_max,v_max)."""
+    u_min = max(0, int(np.floor(bbox["x1"])))
+    v_min = max(0, int(np.floor(bbox["y1"])))
+    u_max = int(np.ceil(bbox["x2"]))
+    v_max = int(np.ceil(bbox["y2"]))
+    if u_max <= u_min:
+        u_max = u_min + 1
+    if v_max <= v_min:
+        v_max = v_min + 1
+    return u_min, v_min, u_max, v_max
+
+
+def _extract_xyz(res: Any) -> list[float] | None:
+    if not isinstance(res, dict):
+        return None
+    for key in ("point", "xyz", "camera_xyz"):
+        v = res.get(key)
+        if isinstance(v, (list, tuple)) and len(v) >= 3:
+            return [float(v[0]), float(v[1]), float(v[2])]
+    if all(k in res for k in ("x", "y", "z")):
+        return [float(res["x"]), float(res["y"]), float(res["z"])]
+    return None

src/presa_mano/api/posture_routes.py

@@ -0,0 +1,112 @@
+"""Posture & state endpoints: full live state, hand control, server-side presets.
+
+Thin proxy over G1 endpoints not covered by `wrist_routes.py`. Mutating calls
+honor `dry_run` (handled inside `G1Client`).
+"""
+
+from __future__ import annotations
+
+from typing import Any, Optional
+
+from fastapi import APIRouter, HTTPException
+from pydantic import BaseModel, Field
+
+from ..clients import G1Unavailable
+from . import _deps
+
+router = APIRouter()
+
+
+# ---------- live state ----------
+
+@router.get("/state")
+def get_state() -> dict[str, Any]:
+    try:
+        return _deps.g1().state()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.get("/hand")
+def get_hand() -> dict[str, Any]:
+    try:
+        return _deps.g1().hand()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+# ---------- per-finger control ----------
+
+class FingerValues(BaseModel):
+    pinky: Optional[float] = Field(None, ge=0.0, le=1.0)
+    ring: Optional[float] = Field(None, ge=0.0, le=1.0)
+    middle: Optional[float] = Field(None, ge=0.0, le=1.0)
+    index: Optional[float] = Field(None, ge=0.0, le=1.0)
+    thumb_bend: Optional[float] = Field(None, ge=0.0, le=1.0)
+    thumb_rotation: Optional[float] = Field(None, ge=0.0, le=1.0)
+
+
+class HandControlReq(BaseModel):
+    side: str = Field(..., pattern="^(left|right)$")
+    fingers: FingerValues
+    dry_run: bool = False
+
+
+@router.post("/hand_control")
+def post_hand_control(req: HandControlReq) -> dict[str, Any]:
+    fingers = {k: v for k, v in req.fingers.model_dump().items() if v is not None}
+    try:
+        return _deps.g1(dry_run=req.dry_run).hand_control(side=req.side, fingers=fingers)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+# ---------- presets ----------
+
+class PresetSaveReq(BaseModel):
+    name: str = Field(..., min_length=1, max_length=64, pattern=r"^[a-zA-Z0-9_-]+$")
+    dry_run: bool = False
+
+
+class PresetExecuteReq(BaseModel):
+    dry_run: bool = False
+
+
+@router.get("/presets")
+def list_presets() -> dict[str, Any]:
+    try:
+        return _deps.g1().presets()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.get("/presets/{name}")
+def get_preset(name: str) -> dict[str, Any]:
+    try:
+        return _deps.g1().preset_get(name)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/presets/save")
+def save_preset(req: PresetSaveReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).preset_save(name=req.name)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/presets/{name}/execute")
+def execute_preset(name: str, req: PresetExecuteReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).preset_execute(name=name)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.delete("/presets/{name}")
+def delete_preset(name: str, dry_run: bool = False) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).preset_delete(name=name)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))

src/presa_mano/api/robot_routes.py

@@ -0,0 +1,155 @@
+"""Robot FSM, arm-mode and locomotion endpoints (proxied to G1).
+
+Thin proxy over `/robot/*` and `/arms/*` on the G1 API server. Mutating calls
+honor `dry_run` (handled inside `G1Client`).
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+from fastapi import APIRouter, HTTPException, Query
+from pydantic import BaseModel, Field
+
+from ..clients import G1StatusError, G1Unavailable
+from . import _deps
+
+router = APIRouter()
+
+
+# ---------- FSM state ----------
+
+class StateReq(BaseModel):
+    state: str = Field(..., min_length=1, max_length=32)
+    dry_run: bool = False
+
+
+@router.get("/state")
+def get_state() -> dict[str, Any]:
+    try:
+        return _deps.g1().robot_state_get()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/state")
+def set_state(req: StateReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).robot_state_set(req.state)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+# ---------- arm mode ----------
+
+class ArmModeReq(BaseModel):
+    mode: str = Field(..., min_length=1, max_length=32)
+    dry_run: bool = False
+
+
+@router.post("/arm_mode")
+def set_arm_mode(req: ArmModeReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).robot_arm_mode(req.mode)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+# ---------- stand / move ----------
+
+class StandReq(BaseModel):
+    mode: str = Field("high", min_length=1, max_length=32)
+    dry_run: bool = False
+
+
+class MoveReq(BaseModel):
+    vx: float = Field(..., ge=-2.0, le=2.0)
+    vy: float = Field(..., ge=-1.0, le=1.0)
+    vyaw: float = Field(..., ge=-2.0, le=2.0)
+    continuous: bool = False
+    dry_run: bool = False
+
+
+@router.post("/stand")
+def post_stand(req: StandReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).robot_stand(req.mode)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/move")
+def post_move(req: MoveReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).robot_move(
+            vx=req.vx, vy=req.vy, vyaw=req.vyaw, continuous=req.continuous,
+        )
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/stopmove")
+def post_stopmove(dry_run: bool = Query(False)) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).robot_stopmove()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+# ---------- arms (compliance + helpers) ----------
+
+@router.get("/arms/compliance")
+def get_compliance() -> dict[str, Any]:
+    try:
+        return _deps.g1().arms_compliance()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/arms/compliance/start")
+def start_compliance(dry_run: bool = Query(False)) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).arms_compliance_start()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/arms/compliance/stop")
+def stop_compliance(dry_run: bool = Query(False)) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).arms_compliance_stop()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/arms/go_neutral")
+def go_neutral(dry_run: bool = Query(False)) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).arms_go_neutral()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/arms/loco_reset")
+def loco_reset(dry_run: bool = Query(False)) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).arms_loco_reset()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.get("/diagnostics")
+def diagnostics() -> dict[str, Any]:
+    """Proxy for G1 /diagnostics (motor temps, fault codes). Falls back to
+    {"available": false} when the upstream version doesn't expose it — the
+    frontend uses this to hide the thermal UI gracefully."""
+    try:
+        data = _deps.g1().diagnostics()
+        data = data if isinstance(data, dict) else {"data": data}
+        return {"available": True, **data}
+    except G1StatusError as e:
+        if e.status == 404:
+            return {"available": False, "reason": "G1 API does not expose /diagnostics yet"}
+        raise HTTPException(status_code=e.status, detail=str(e.detail))
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))

src/presa_mano/api/wrist_routes.py

@@ -0,0 +1,92 @@
+"""Direct end-effector control endpoints (proxied to G1)."""
+
+from __future__ import annotations
+
+from typing import Any, Optional
+
+from fastapi import APIRouter, HTTPException, Query
+from pydantic import BaseModel, Field
+
+from ..clients import G1Unavailable
+from . import _deps
+
+router = APIRouter()
+
+
+class AbsoluteReq(BaseModel):
+    side: str = Field(..., pattern="^(left|right|both)$")
+    position: list[float] = Field(..., min_length=3, max_length=3)
+    quaternion: list[float] = Field(..., min_length=4, max_length=4)
+    wait: bool = True
+    timeout_s: float = 5.0
+    precise: bool = False
+    dry_run: bool = False
+
+
+class RelativeReq(BaseModel):
+    side: str = Field(..., pattern="^(left|right|both)$")
+    delta_position: Optional[list[float]] = None
+    delta_quaternion: Optional[list[float]] = None
+    wait: bool = True
+    timeout_s: float = 5.0
+    dry_run: bool = False
+
+
+class HandActionReq(BaseModel):
+    action: str = Field(..., pattern="^(LEFT_OPEN|LEFT_CLOSE|RIGHT_OPEN|RIGHT_CLOSE)$")
+    dry_run: bool = False
+
+
+@router.get("")
+def get_wrist() -> dict[str, Any]:
+    try:
+        return _deps.g1().wrist()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/absolute")
+def post_absolute(req: AbsoluteReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).wrist_absolute(
+            side=req.side, position=req.position, quaternion=req.quaternion,
+            wait=req.wait, timeout_s=req.timeout_s, precise=req.precise,
+        )
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/relative")
+def post_relative(req: RelativeReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).wrist_relative(
+            side=req.side, delta_position=req.delta_position,
+            delta_quaternion=req.delta_quaternion,
+            wait=req.wait, timeout_s=req.timeout_s,
+        )
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/hand")
+def post_hand(req: HandActionReq) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=req.dry_run).hand_action(req.action)
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/emergency_stop")
+def post_estop(dry_run: bool = Query(False)) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).emergency_stop()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))
+
+
+@router.post("/unlock")
+def post_unlock(dry_run: bool = Query(False)) -> dict[str, Any]:
+    try:
+        return _deps.g1(dry_run=dry_run).unlock()
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=str(e))

src/presa_mano/calibration/__init__.py

@@ -0,0 +1,5 @@
+from .handeye import HandEyeSolver
+from .sampler import CalibrationSampler, Sample
+from .store import CalibrationStore, SamplesStore
+
+__all__ = ["HandEyeSolver", "CalibrationSampler", "Sample", "CalibrationStore", "SamplesStore"]

src/presa_mano/calibration/auto_job.py

@@ -0,0 +1,340 @@
+"""Automated hand-eye calibration procedure.
+
+Operator defines a 3D grid of wrist waypoints around the current pose plus a
+wrist rotation search delta. The job moves to each waypoint, tries to detect
+the ArUco marker, and if needed sweeps a 3x3 grid of wrist-frame rotations
+within the delta. Designed for a single in-flight job (global singleton) and
+polled status — the frontend renders the waypoints as spheres over the 3D
+model and updates their colours as the job progresses.
+"""
+
+from __future__ import annotations
+
+import logging
+import math
+import threading
+import time
+from dataclasses import asdict, dataclass, field
+from typing import Any, Callable
+
+import cv2
+import numpy as np
+
+from ..clients import G1StatusError, G1Unavailable
+from ..geometry import R_to_quat_xyzw, quat_xyzw_to_R
+from .sampler import CalibrationSampler, Sample
+from .store import SamplesStore
+
+log = logging.getLogger(__name__)
+
+
+@dataclass
+class Waypoint:
+    idx: int
+    position: list[float]                # base-frame xyz (m)
+    quaternion: list[float]              # xyzw
+    reachable: bool = True
+    reason: str | None = None            # why not reachable, if applicable
+    state: str = "pending"               # pending | current | done | failed | skipped
+    attempts: int = 0
+    error: str | None = None
+    sample_total: int | None = None      # store length at the moment a sample was acquired
+
+
+@dataclass
+class JobState:
+    status: str = "idle"                 # idle | running | done | stopped | error
+    waypoints: list[Waypoint] = field(default_factory=list)
+    current_index: int = -1
+    started_at: float | None = None
+    finished_at: float | None = None
+    error: str | None = None
+    calibration: dict[str, Any] | None = None
+    stats: dict[str, int] = field(default_factory=lambda: {
+        "done": 0, "failed": 0, "skipped": 0,
+    })
+    # Echoed params so the UI can regenerate visuals from status alone.
+    params: dict[str, Any] = field(default_factory=dict)
+
+
+def _frac(i: int, n: int) -> float:
+    """Return a [-0.5, +0.5] grid fraction for index i out of n divisions,
+    or 0.0 when n == 1 (single slice in that axis)."""
+    if n <= 1:
+        return 0.0
+    return (i / (n - 1)) - 0.5
+
+
+def build_grid(start_position: np.ndarray, start_quaternion: np.ndarray,
+               delta_m: tuple[float, float, float],
+               divisions: tuple[int, int, int],
+               min_z_m: float,
+               order: str = "serpentine") -> list[Waypoint]:
+    """Axis-aligned grid centred on `start_position`. delta_m is the full span
+    per axis (grid covers start ± delta/2). divisions >= 1 per axis; 1 means
+    "just the starting value on this axis".
+
+    order: "nested" = simple ix/iy/iz lexicographic; "serpentine" = boustrophedon,
+    alternates x-direction per y-row and y-direction per z-layer to minimise
+    total wrist travel between adjacent waypoints.
+    """
+    waypoints: list[Waypoint] = []
+    idx = 0
+    nx, ny, nz = (max(1, d) for d in divisions)
+    for iz in range(nz):
+        iys = range(ny) if (order != "serpentine" or iz % 2 == 0) else range(ny - 1, -1, -1)
+        for iy in iys:
+            ixs = range(nx) if (order != "serpentine" or (iy + iz) % 2 == 0) else range(nx - 1, -1, -1)
+            for ix in ixs:
+                fx = _frac(ix, nx)
+                fy = _frac(iy, ny)
+                fz = _frac(iz, nz)
+                x = float(start_position[0] + fx * delta_m[0])
+                y = float(start_position[1] + fy * delta_m[1])
+                z = float(start_position[2] + fz * delta_m[2])
+                # Keep the original (unclamped) position so the operator can
+                # see *where* the failing points actually fall — stacking them
+                # all on the floor hides the problem (N z-layers collapsing
+                # to 1 → apparent 9 points instead of 27).
+                reason = None
+                reachable = True
+                if z < min_z_m:
+                    reason = f"sotto z_min ({z*100:.1f} < {min_z_m*100:.1f} cm)"
+                    reachable = False
+                w = Waypoint(
+                    idx=idx,
+                    position=[x, y, z],
+                    quaternion=list(map(float, start_quaternion)),
+                    reachable=reachable,
+                    reason=reason,
+                )
+                waypoints.append(w)
+                idx += 1
+    return waypoints
+
+
+def perturb_quat(q_xyzw: list[float], drx: float, dry: float) -> list[float]:
+    """Apply a small rotation (drx around local X, dry around local Y) to the
+    wrist-frame orientation described by q_xyzw, returning a new xyzw quat."""
+    R_base = quat_xyzw_to_R(np.asarray(q_xyzw, dtype=np.float64))
+    Rx, _ = cv2.Rodrigues(np.array([drx, 0.0, 0.0]))
+    Ry, _ = cv2.Rodrigues(np.array([0.0, dry, 0.0]))
+    R_new = R_base @ Ry @ Rx
+    return R_to_quat_xyzw(R_new).tolist()
+
+
+def _rotation_search_order(delta_rad: float) -> list[tuple[float, float]]:
+    """Return (drx, dry) pairs to try in order of increasing |rotation|.
+    Excludes (0, 0) since that's the nominal pose already attempted."""
+    d = delta_rad
+    if d <= 0:
+        return []
+    offsets = [(drx, dry) for drx in (-d, 0.0, d) for dry in (-d, 0.0, d)
+               if (drx, dry) != (0.0, 0.0)]
+    offsets.sort(key=lambda v: math.hypot(v[0], v[1]))
+    return offsets
+
+
+class AutoRunner:
+    """Per-waypoint executor. One instance per job run, stateless between
+    waypoints. Captures dependencies (G1/Nuwa clients, sampler, store)."""
+
+    def __init__(self, *, side: str, marker_id: int, size_mm: float,
+                 wrist_rot_delta_rad: float, settle_s: float,
+                 move_timeout_s: float,
+                 g1_move, sampler: CalibrationSampler, store: SamplesStore,
+                 feasibility_only: bool = False):
+        self.side = side
+        self.marker_id = marker_id
+        self.size_mm = size_mm
+        self.wrist_rot_delta_rad = wrist_rot_delta_rad
+        self.settle_s = settle_s
+        self.move_timeout_s = move_timeout_s
+        self.g1_move = g1_move         # bound method: wrist_absolute(...)
+        self.sampler = sampler
+        self.store = store
+        self.feasibility_only = feasibility_only
+
+    def _try_sample(self, note: str) -> tuple[bool, str | None]:
+        try:
+            sample = self.sampler.acquire(
+                marker_id=self.marker_id,
+                size_mm=self.size_mm,
+                side=self.side,
+                note=note,
+            )
+            self.store.append(sample)
+            return True, None
+        except Exception as e:
+            return False, str(e)
+
+    def _move(self, position: list[float], quaternion: list[float],
+              timeout_s: float | None = None) -> tuple[bool, str | None]:
+        t = timeout_s if timeout_s is not None else self.move_timeout_s
+        try:
+            r = self.g1_move(
+                side=self.side, position=position, quaternion=quaternion,
+                wait=True, timeout_s=t,
+            )
+            if isinstance(r, dict) and r.get("reached_target") is False:
+                return False, f"reached_target=false (pos_err_mm={r.get('position_error_mm')})"
+            return True, None
+        except (G1Unavailable, G1StatusError) as e:
+            return False, str(e)
+        except Exception as e:
+            return False, str(e)
+
+    def run(self, wp: Waypoint) -> dict[str, Any]:
+        attempts = 1
+        ok, err = self._move(wp.position, wp.quaternion)
+        # Retry once with doubled timeout: reached_target=false with pos_err a
+        # few cm away is usually solved by giving the controller more time to
+        # settle, especially after big jumps between waypoints.
+        if not ok:
+            attempts = 2
+            ok, err = self._move(wp.position, wp.quaternion,
+                                 timeout_s=self.move_timeout_s * 2)
+        if not ok:
+            return {"ok": False, "attempts": attempts, "error": f"move failed: {err}"}
+        # In feasibility-only mode we skip the settle+detect — reachability has
+        # already been answered by the move itself (reached_target flag).
+        if self.feasibility_only:
+            return {"ok": True, "attempts": attempts, "sample_total": None}
+
+        time.sleep(self.settle_s)
+        acquired, err = self._try_sample(note=f"auto[{wp.idx}]")
+        if acquired:
+            return {"ok": True, "attempts": attempts, "sample_total": len(self.store.load())}
+
+        # Marker not found → rotation search within ±delta.
+        offsets = _rotation_search_order(self.wrist_rot_delta_rad)
+        for k, (drx, dry) in enumerate(offsets, start=1):
+            att = attempts + k
+            q = perturb_quat(wp.quaternion, drx, dry)
+            ok, move_err = self._move(wp.position, q)
+            if not ok:
+                continue
+            time.sleep(self.settle_s)
+            acquired, err = self._try_sample(
+                note=f"auto[{wp.idx}]+rx{math.degrees(drx):+.0f}ry{math.degrees(dry):+.0f}")
+            if acquired:
+                return {"ok": True, "attempts": att, "sample_total": len(self.store.load())}
+
+        return {"ok": False, "attempts": attempts + len(offsets),
+                "error": err or "marker non trovato dopo ricerca rotazione"}
+
+
+class AutoCalibrationJob:
+    def __init__(self) -> None:
+        self._lock = threading.Lock()
+        self._thread: threading.Thread | None = None
+        self._stop_event = threading.Event()
+        self.state = JobState()
+
+    def is_running(self) -> bool:
+        return self._thread is not None and self._thread.is_alive()
+
+    def snapshot(self) -> dict[str, Any]:
+        with self._lock:
+            return {
+                "status": self.state.status,
+                "current_index": self.state.current_index,
+                "waypoints": [asdict(w) for w in self.state.waypoints],
+                "started_at": self.state.started_at,
+                "finished_at": self.state.finished_at,
+                "error": self.state.error,
+                "calibration": self.state.calibration,
+                "stats": dict(self.state.stats),
+                "params": dict(self.state.params),
+            }
+
+    def stop(self) -> bool:
+        if not self.is_running():
+            return False
+        self._stop_event.set()
+        return True
+
+    def start(self, *, waypoints: list[Waypoint], runner: AutoRunner,
+              on_finish: Callable[[JobState], None] | None = None,
+              params: dict[str, Any] | None = None,
+              home_pose: tuple[list[float], list[float]] | None = None) -> bool:
+        if self.is_running():
+            return False
+        with self._lock:
+            self.state = JobState(
+                status="running", waypoints=waypoints,
+                started_at=time.time(),
+                params=params or {},
+            )
+            self._stop_event.clear()
+
+        def worker():
+            try:
+                stopped = False
+                for wp in waypoints:
+                    if self._stop_event.is_set():
+                        with self._lock:
+                            self.state.status = "stopped"
+                        stopped = True
+                        break
+                    if not wp.reachable:
+                        with self._lock:
+                            wp.state = "skipped"
+                            self.state.stats["skipped"] += 1
+                        continue
+                    with self._lock:
+                        wp.state = "current"
+                        self.state.current_index = wp.idx
+                    result = runner.run(wp)
+                    with self._lock:
+                        wp.attempts = int(result.get("attempts", 1))
+                        wp.sample_total = result.get("sample_total")
+                        if result.get("ok"):
+                            wp.state = "done"
+                            self.state.stats["done"] += 1
+                        else:
+                            wp.state = "failed"
+                            wp.error = result.get("error")
+                            self.state.stats["failed"] += 1
+                # On stop, try to return to the home pose so the robot isn't
+                # stranded at an arbitrary waypoint. Best-effort: errors here
+                # are logged but don't change the job status.
+                if stopped and home_pose is not None:
+                    try:
+                        with self._lock:
+                            self.state.error = "returning to home…"
+                        runner._move(home_pose[0], home_pose[1])
+                        with self._lock:
+                            self.state.error = None
+                    except Exception as e:
+                        log.warning("return-to-home failed: %s", e)
+                        with self._lock:
+                            self.state.error = f"return-to-home failed: {e}"
+                with self._lock:
+                    if self.state.status == "running":
+                        self.state.status = "done"
+                    self.state.finished_at = time.time()
+                    self.state.current_index = -1
+                if on_finish:
+                    try:
+                        on_finish(self.state)
+                    except Exception:
+                        log.exception("on_finish callback failed")
+            except Exception as e:
+                log.exception("auto-calibration worker crashed")
+                with self._lock:
+                    self.state.status = "error"
+                    self.state.error = str(e)
+                    self.state.finished_at = time.time()
+
+        self._thread = threading.Thread(target=worker, daemon=True, name="auto-calib")
+        self._thread.start()
+        return True
+
+
+# Module-level singleton — FastAPI routes access it via get_job().
+_JOB = AutoCalibrationJob()
+
+
+def get_job() -> AutoCalibrationJob:
+    return _JOB

src/presa_mano/calibration/handeye.py

@@ -0,0 +1,299 @@
+"""Eye-to-hand calibration: camera fixed, ArUco mounted on the wrist.
+
+Given N samples (T_base_wrist_i, T_cam_marker_i), find T_base_camera (4x4).
+
+OpenCV's `cv2.calibrateHandEye` is documented for the eye-in-hand case.
+For eye-to-hand (our case) we feed the *inverse* of the gripper->base transform,
+which turns the solver output into T_camera->base (camera-to-base).
+
+See: https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html
+"""
+
+from __future__ import annotations
+
+import logging
+from typing import Any
+
+import cv2
+import numpy as np
+
+from ..geometry import invert_T
+
+log = logging.getLogger(__name__)
+
+_METHODS = {
+    "TSAI":       cv2.CALIB_HAND_EYE_TSAI,
+    "PARK":       cv2.CALIB_HAND_EYE_PARK,
+    "HORAUD":     cv2.CALIB_HAND_EYE_HORAUD,
+    "ANDREFF":    cv2.CALIB_HAND_EYE_ANDREFF,
+    "DANIILIDIS": cv2.CALIB_HAND_EYE_DANIILIDIS,
+}
+
+
+def _rot_angle(R: np.ndarray) -> float:
+    """Rotation angle (radians) of a 3x3 rotation matrix."""
+    cos = (np.trace(R) - 1.0) / 2.0
+    return float(np.arccos(np.clip(cos, -1.0, 1.0)))
+
+
+class HandEyeSolver:
+    @staticmethod
+    def solve(samples: list[dict[str, Any]], method: str = "TSAI",
+              sanity_check: bool = True,
+              pair_angle_tol_deg: float = 8.0,
+              wm_translation_mad_factor: float = 3.5,
+              max_reprojection_px: float = 3.0,
+              max_ambiguity_ratio: float = 0.90,
+              max_drop_frac: float = 0.5) -> dict[str, Any]:
+        """Run cv2.calibrateHandEye on `samples`.
+
+        When sanity_check=True (default), apply two filters:
+        * pair-angle check: pre-solver, independent of the unknown extrinsic.
+          For any two samples i,j, the rotation angles |R_wrist_ij| and
+          |R_cam_ij| must match (consequence of AX=XB). Samples whose median
+          per-pair angle discrepancy exceeds `pair_angle_tol_deg` are dropped.
+          Catches the ArUco planar-pose 180° flip ambiguity, which is the
+          typical cause of residuals in the hundreds of mm / tens of degrees.
+        * post-solver T_wrist_marker cluster check: the marker is rigid on the
+          wrist, so T_wrist_marker should be constant across samples. Solve
+          once, then drop samples whose translation component is > MAD*k from
+          the median, then re-solve on the survivors.
+        """
+        if len(samples) < 3:
+            raise ValueError(f"need at least 3 samples, got {len(samples)}")
+
+        flag = _METHODS.get(method.upper(), cv2.CALIB_HAND_EYE_TSAI)
+
+        filter_report: dict[str, Any] = {"applied": bool(sanity_check)}
+
+        working = list(enumerate(samples))  # (original_idx, sample)
+
+        if sanity_check:
+            kept, dropped_pose = _filter_pose_quality(
+                working, max_reprojection_px=max_reprojection_px,
+                max_ambiguity_ratio=max_ambiguity_ratio,
+                max_drop_frac=max_drop_frac,
+            )
+            filter_report["max_reprojection_px"] = max_reprojection_px
+            filter_report["max_ambiguity_ratio"] = max_ambiguity_ratio
+            filter_report["dropped_by_pose_quality"] = dropped_pose
+            working = kept
+
+            kept, dropped_pair = _filter_pair_angle(
+                working, tol_rad=np.deg2rad(pair_angle_tol_deg),
+                max_drop_frac=max_drop_frac - len(dropped_pose) / max(1, len(samples)),
+            )
+            filter_report["pair_angle_tol_deg"] = pair_angle_tol_deg
+            filter_report["dropped_by_pair_angle"] = dropped_pair
+            working = kept
+
+        if len(working) < 3:
+            raise ValueError(
+                f"after pre-filter only {len(working)} samples remain (need ≥3). "
+                f"Dropped by pair-angle: {filter_report.get('dropped_by_pair_angle')}")
+
+        T_cam2base = _solve_once([s for _, s in working], flag)
+
+        if sanity_check:
+            kept, dropped_wm = _filter_wm_cluster(
+                working, T_cam2base, mad_factor=wm_translation_mad_factor,
+                max_drop_frac=max_drop_frac - len(filter_report.get("dropped_by_pair_angle", [])) / max(1, len(samples)),
+            )
+            filter_report["wm_mad_factor"] = wm_translation_mad_factor
+            filter_report["dropped_by_wm_cluster"] = dropped_wm
+            if kept != working and len(kept) >= 3:
+                working = kept
+                T_cam2base = _solve_once([s for _, s in working], flag)
+
+        residual_mm, residual_rad, per_sample = _residuals(
+            [s for _, s in working], T_cam2base
+        )
+
+        if sanity_check:
+            filter_report["n_input"] = len(samples)
+            filter_report["n_used"] = len(working)
+            filter_report["used_indices"] = [i for i, _ in working]
+
+        return {
+            "T_base_camera": T_cam2base.tolist(),
+            "method": method.upper(),
+            "n_samples": len(working),
+            "residual_translation_mm": residual_mm,
+            "residual_rotation_deg": float(np.degrees(residual_rad)),
+            "per_sample_residuals": per_sample,
+            "sanity_check": filter_report,
+        }
+
+
+def _solve_once(samples: list[dict[str, Any]], flag: int) -> np.ndarray:
+    R_b2g, t_b2g, R_t2c, t_t2c = [], [], [], []
+    for s in samples:
+        T_bw = np.asarray(s["T_base_wrist"], dtype=np.float64)
+        T_cm = np.asarray(s["T_cam_marker"], dtype=np.float64)
+        # eye-to-hand trick: feed the inverse of base->wrist.
+        T_wb = invert_T(T_bw)
+        R_b2g.append(T_wb[:3, :3])
+        t_b2g.append(T_wb[:3, 3])
+        R_t2c.append(T_cm[:3, :3])
+        t_t2c.append(T_cm[:3, 3])
+    R_cam2base, t_cam2base = cv2.calibrateHandEye(
+        R_b2g, t_b2g, R_t2c, t_t2c, method=flag
+    )
+    T = np.eye(4)
+    T[:3, :3] = R_cam2base
+    T[:3, 3] = t_cam2base.flatten()
+    return T
+
+
+def _filter_pose_quality(working: list[tuple[int, dict[str, Any]]],
+                         max_reprojection_px: float,
+                         max_ambiguity_ratio: float,
+                         max_drop_frac: float) -> tuple[list[tuple[int, dict[str, Any]]],
+                                                        list[dict[str, Any]]]:
+    """Drop samples whose local PnP was low-confidence: high reprojection
+    error (bad corner detection) or high ambiguity ratio (both planar
+    solutions nearly equivalent, pose underdetermined)."""
+    n = len(working)
+    if n < 3:
+        return working, []
+    max_drops = max(0, int(np.floor(n * max_drop_frac)))
+    scored: list[tuple[int, dict[str, Any], str, float]] = []
+    for local_i, (orig_i, s) in enumerate(working):
+        re = s.get("reprojection_error_px")
+        amb = s.get("ambiguity_ratio")
+        if re is not None and re > max_reprojection_px:
+            scored.append((local_i, s, "reprojection_px", float(re)))
+        elif amb is not None and amb > max_ambiguity_ratio:
+            scored.append((local_i, s, "ambiguity_ratio", float(amb)))
+    # Drop worst first, capped by max_drops.
+    scored.sort(key=lambda x: -x[3])
+    keep_mask = np.ones(n, dtype=bool)
+    dropped: list[dict[str, Any]] = []
+    for local_i, s, reason, val in scored:
+        if len(dropped) >= max_drops:
+            break
+        keep_mask[local_i] = False
+        orig_i = working[local_i][0]
+        dropped.append({"sample_index": orig_i, reason: val})
+    kept = [working[i] for i in range(n) if keep_mask[i]]
+    return kept, dropped
+
+
+def _filter_pair_angle(working: list[tuple[int, dict[str, Any]]],
+                       tol_rad: float,
+                       max_drop_frac: float) -> tuple[list[tuple[int, dict[str, Any]]],
+                                                      list[dict[str, Any]]]:
+    """AX=XB implies the rotation *angle* of the motion (wrist->wrist) equals
+    the rotation angle of the corresponding camera motion. Any sample that
+    consistently breaks this across its pairings is bad (e.g. ArUco flipped).
+    """
+    n = len(working)
+    if n < 3:
+        return working, []
+
+    R_w = [np.asarray(s["T_base_wrist"], dtype=np.float64)[:3, :3] for _, s in working]
+    R_c = [np.asarray(s["T_cam_marker"], dtype=np.float64)[:3, :3] for _, s in working]
+
+    scores = np.zeros(n)
+    for i in range(n):
+        diffs = []
+        for j in range(n):
+            if i == j:
+                continue
+            dR_w = R_w[j] @ R_w[i].T
+            dR_c = R_c[j] @ R_c[i].T
+            diffs.append(abs(_rot_angle(dR_w) - _rot_angle(dR_c)))
+        scores[i] = float(np.median(diffs)) if diffs else 0.0
+
+    dropped: list[dict[str, Any]] = []
+    max_drops = max(0, int(np.floor(n * max_drop_frac)))
+    # Drop in descending order of score while score > tol and under the cap.
+    order = np.argsort(-scores)
+    keep_mask = np.ones(n, dtype=bool)
+    for idx in order:
+        if len(dropped) >= max_drops:
+            break
+        if scores[idx] <= tol_rad:
+            break
+        keep_mask[idx] = False
+        orig_i = working[idx][0]
+        dropped.append({
+            "sample_index": orig_i,
+            "pair_angle_err_deg": float(np.degrees(scores[idx])),
+        })
+    kept = [working[i] for i in range(n) if keep_mask[i]]
+    return kept, dropped
+
+
+def _filter_wm_cluster(working: list[tuple[int, dict[str, Any]]],
+                       T_base_cam: np.ndarray,
+                       mad_factor: float,
+                       max_drop_frac: float) -> tuple[list[tuple[int, dict[str, Any]]],
+                                                      list[dict[str, Any]]]:
+    """T_wrist_marker is rigid (marker bolted to the wrist). Its translation
+    must be the same for every sample up to measurement noise. Drop outliers."""
+    n = len(working)
+    if n < 4:
+        return working, []
+
+    ts = np.zeros((n, 3))
+    for k, (_, s) in enumerate(working):
+        T_bw = np.asarray(s["T_base_wrist"], dtype=np.float64)
+        T_cm = np.asarray(s["T_cam_marker"], dtype=np.float64)
+        T_wm = invert_T(T_bw) @ T_base_cam @ T_cm
+        ts[k] = T_wm[:3, 3]
+
+    med = np.median(ts, axis=0)
+    dist = np.linalg.norm(ts - med, axis=1)
+    mad = float(np.median(np.abs(dist - np.median(dist))))
+    # If MAD is tiny (all samples extremely consistent), pick an absolute floor
+    # so we still drop gross outliers >5 cm without chasing numerical noise.
+    thr = max(mad * mad_factor, 0.02)  # metres
+
+    dropped: list[dict[str, Any]] = []
+    max_drops = max(0, int(np.floor(n * max_drop_frac)))
+    order = np.argsort(-dist)
+    keep_mask = np.ones(n, dtype=bool)
+    for idx in order:
+        if len(dropped) >= max_drops:
+            break
+        if dist[idx] <= thr:
+            break
+        keep_mask[idx] = False
+        orig_i = working[idx][0]
+        dropped.append({
+            "sample_index": orig_i,
+            "wm_translation_dev_mm": float(dist[idx] * 1000.0),
+        })
+    kept = [working[i] for i in range(n) if keep_mask[i]]
+    return kept, dropped
+
+
+def _residuals(samples: list[dict[str, Any]], T_base_cam: np.ndarray
+               ) -> tuple[float, float, list[dict[str, float]]]:
+    """Mean translation (mm) and rotation (rad) deviation of T_wrist_marker
+    from sample 0 across all samples. Also returns per-sample deviations so
+    the UI can highlight the worst offenders."""
+    T_wm_list: list[np.ndarray] = []
+    for s in samples:
+        T_bw = np.asarray(s["T_base_wrist"], dtype=np.float64)
+        T_cm = np.asarray(s["T_cam_marker"], dtype=np.float64)
+        T_wm_list.append(invert_T(T_bw) @ T_base_cam @ T_cm)
+
+    T_ref = T_wm_list[0]
+    per_sample: list[dict[str, float]] = []
+    t_err, r_err = [], []
+    for T in T_wm_list:
+        dt_mm = float(np.linalg.norm(T[:3, 3] - T_ref[:3, 3]) * 1000.0)
+        R = T[:3, :3] @ T_ref[:3, :3].T
+        dr_rad = _rot_angle(R)
+        per_sample.append({
+            "translation_err_mm": dt_mm,
+            "rotation_err_deg": float(np.degrees(dr_rad)),
+        })
+        if dt_mm > 0 or dr_rad > 0:  # skip the reference (always 0)
+            t_err.append(dt_mm)
+            r_err.append(dr_rad)
+    if not t_err:
+        return 0.0, 0.0, per_sample
+    return float(np.mean(t_err)), float(np.mean(r_err)), per_sample

src/presa_mano/calibration/sampler.py

@@ -0,0 +1,136 @@
+"""Acquire a calibration sample.
+
+One sample = (T_base_wrist from G1) + (T_cam_marker from ArUco detection).
+
+Pose computation: we call marker-finder only for the 2D `corners` (unaffected
+by its baked D435i intrinsics) and rerun solvePnP locally with the real Nuwa
+RGB intrinsics. This sidesteps what `CLAUDE.md` flags as the dominant error
+source in hand-eye calibration on this stack.
+"""
+
+from __future__ import annotations
+
+import logging
+import time
+from dataclasses import dataclass
+from typing import Any
+
+import numpy as np
+
+from ..clients import G1Client, MarkerClient, NuwaClient
+from ..geometry import T_from_pos_quat, make_T
+from ..perception.aruco_pose import nuwa_intrinsics_to_matrix, solve_square_marker
+from ..timing import Stopwatch
+
+log = logging.getLogger(__name__)
+
+
+@dataclass
+class Sample:
+    created_at: float
+    side: str
+    marker_id: int
+    T_base_wrist: list[list[float]]  # 4x4
+    T_cam_marker: list[list[float]]  # 4x4
+    corners: list[list[float]]
+    note: str = ""
+    timings_ms: dict[str, float] | None = None
+    # Diagnostics from the local pose solve (absent when we fell back to
+    # marker-finder's pose).
+    pose_source: str = "marker_finder"          # "nuwa_intrinsics" | "marker_finder"
+    reprojection_error_px: float | None = None
+    ambiguity_ratio: float | None = None
+
+    @classmethod
+    def make(cls, side: str, marker_id: int, T_base_wrist: np.ndarray,
+             T_cam_marker: np.ndarray, corners: np.ndarray, note: str = "",
+             timings_ms: dict[str, float] | None = None,
+             pose_source: str = "marker_finder",
+             reprojection_error_px: float | None = None,
+             ambiguity_ratio: float | None = None) -> "Sample":
+        return cls(
+            created_at=time.time(),
+            side=side,
+            marker_id=int(marker_id),
+            T_base_wrist=T_base_wrist.tolist(),
+            T_cam_marker=T_cam_marker.tolist(),
+            corners=np.asarray(corners).tolist(),
+            note=note,
+            timings_ms=timings_ms,
+            pose_source=pose_source,
+            reprojection_error_px=reprojection_error_px,
+            ambiguity_ratio=ambiguity_ratio,
+        )
+
+
+class SamplerError(RuntimeError):
+    pass
+
+
+class CalibrationSampler:
+    def __init__(self, nuwa: NuwaClient, marker: MarkerClient, g1: G1Client):
+        self.nuwa = nuwa
+        self.marker = marker
+        self.g1 = g1
+
+    def acquire(self, marker_id: int, size_mm: float, side: str, note: str = "") -> Sample:
+        sw = Stopwatch()
+        # 1) wrist pose from G1
+        with sw.step("g1_wrist"):
+            try:
+                wrist = self.g1.wrist()
+            except Exception as e:
+                raise SamplerError(f"cannot read wrist pose from G1: {e}") from e
+        # G1 returns {'left_wrist': {...}, 'right_wrist': {...}}; tolerate {'left'/'right'} and {'data': ...}.
+        body = wrist.get("data") if isinstance(wrist.get("data"), dict) else wrist
+        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 SamplerError(f"G1 /wrist response missing '{side}' pose: {wrist}")
+        T_base_wrist = T_from_pos_quat(np.asarray(pose["position"]), np.asarray(pose["quaternion"]))
+
+        # 2) ArUco corners from marker-finder (2D corners are intrinsics-free).
+        with sw.step("marker_detect"):
+            result = self.marker.detect_url(
+                markers=[{"id": int(marker_id), "size_mm": float(size_mm)}],
+                image_url=self.nuwa.rgb_url(),
+            )
+        found = result.get("found", [])
+        if not found:
+            raise SamplerError(f"marker id={marker_id} not detected (not_found={result.get('not_found')})")
+        entry = next((m for m in found if int(m["id"]) == int(marker_id)), None)
+        if entry is None:
+            raise SamplerError(f"marker id={marker_id} missing from 'found': {found}")
+        corners = np.asarray(entry["corners"], dtype=np.float64)
+
+        # 3) Pose: solve locally with Nuwa RGB intrinsics (primary path).
+        pose_source = "marker_finder"
+        reproj_err = None
+        ambiguity = None
+        T_cam_marker = None
+        try:
+            with sw.step("nuwa_intrinsics"):
+                intr = self.nuwa.intrinsics()
+            K, dist = nuwa_intrinsics_to_matrix(intr)
+            with sw.step("local_pnp"):
+                sol = solve_square_marker(corners, float(size_mm) / 1000.0, K, dist)
+            T_cam_marker = make_T(sol["R"], sol["t"])
+            pose_source = "nuwa_intrinsics"
+            reproj_err = sol["reprojection_error_px"]
+            ambiguity = sol["ambiguity_ratio"]
+        except Exception as e:
+            log.warning("local pose failed (%s) — falling back to marker-finder pose", e)
+
+        # Fallback: use marker-finder's baked-in pose if local solve failed.
+        if T_cam_marker is None:
+            R = np.asarray(entry["rotation_matrix"], dtype=np.float64).reshape(3, 3)
+            t = np.asarray(entry["position_m"], dtype=np.float64).flatten()
+            T_cam_marker = make_T(R, t)
+
+        return Sample.make(
+            side=side, marker_id=int(marker_id),
+            T_base_wrist=T_base_wrist, T_cam_marker=T_cam_marker,
+            corners=corners, note=note, timings_ms=sw.as_dict(),
+            pose_source=pose_source,
+            reprojection_error_px=reproj_err,
+            ambiguity_ratio=ambiguity,
+        )

src/presa_mano/calibration/store.py

@@ -0,0 +1,65 @@
+"""Persistence for calibration samples (JSON) and result (YAML)."""
+
+from __future__ import annotations
+
+import json
+import time
+from dataclasses import asdict
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+import yaml
+
+from .sampler import Sample
+
+
+class SamplesStore:
+    def __init__(self, path: Path):
+        self.path = Path(path)
+        self.path.parent.mkdir(parents=True, exist_ok=True)
+
+    def load(self) -> list[dict[str, Any]]:
+        if not self.path.exists():
+            return []
+        with open(self.path, "r") as f:
+            return json.load(f)
+
+    def save(self, samples: list[dict[str, Any]]) -> None:
+        with open(self.path, "w") as f:
+            json.dump(samples, f, indent=2)
+
+    def append(self, sample: Sample) -> list[dict[str, Any]]:
+        items = self.load()
+        items.append(asdict(sample))
+        self.save(items)
+        return items
+
+    def clear(self) -> None:
+        self.save([])
+
+
+class CalibrationStore:
+    def __init__(self, path: Path):
+        self.path = Path(path)
+        self.path.parent.mkdir(parents=True, exist_ok=True)
+
+    def save(self, result: dict[str, Any]) -> None:
+        doc = {
+            "saved_at": time.time(),
+            **result,
+        }
+        with open(self.path, "w") as f:
+            yaml.safe_dump(doc, f, sort_keys=False)
+
+    def load(self) -> dict[str, Any] | None:
+        if not self.path.exists():
+            return None
+        with open(self.path, "r") as f:
+            return yaml.safe_load(f)
+
+    def T_base_camera(self) -> np.ndarray | None:
+        doc = self.load()
+        if not doc:
+            return None
+        return np.asarray(doc["T_base_camera"], dtype=np.float64)

src/presa_mano/clients/__init__.py

@@ -0,0 +1,11 @@
+"""HTTP clients for upstream microservices."""
+
+from .g1 import G1Client, G1StatusError, G1Unavailable
+from .marker import MarkerClient
+from .nuwa import NuwaClient, NuwaError
+from .yolo import YoloClient
+
+__all__ = [
+    "G1Client", "G1StatusError", "G1Unavailable",
+    "MarkerClient", "NuwaClient", "NuwaError", "YoloClient",
+]

src/presa_mano/clients/g1.py

@@ -0,0 +1,197 @@
+"""Client for the G1 API Server (port 8765).
+
+Supports dry-run: when enabled, mutating calls return a mock response
+and the actual HTTP request is not performed. Useful while DDS is crashing.
+"""
+
+from __future__ import annotations
+
+import logging
+from typing import Any
+
+import httpx
+
+log = logging.getLogger(__name__)
+
+
+class G1Unavailable(RuntimeError):
+    """Upstream G1 bridge is unreachable or returned a non-actionable error.
+    Maps to 502 Bad Gateway by default."""
+    pass
+
+
+class G1StatusError(RuntimeError):
+    """Upstream G1 returned an HTTP error with a semantic status (e.g. 409
+    'arms already moving'). Preserve the code so the caller can forward it."""
+    def __init__(self, status: int, detail: str, path: str):
+        super().__init__(f"G1 {path}: {status} {detail}")
+        self.status = status
+        self.detail = detail
+        self.path = path
+
+
+class G1Client:
+    def __init__(self, base_url: str, timeout_s: float = 5.0, dry_run: bool = False,
+                 write_timeout_s: float | None = None):
+        """timeout_s applies to GETs (fast reads). write_timeout_s applies to
+        POST/DELETE where the G1 server blocks waiting for motion to complete
+        (FSM transitions, go_neutral, stand, etc.). Defaults to max(30, read)."""
+        self.base_url = base_url.rstrip("/")
+        self.timeout_s = timeout_s
+        self.write_timeout_s = write_timeout_s if write_timeout_s is not None else max(30.0, timeout_s)
+        self.dry_run = dry_run
+
+    # ----- read-only -----
+
+    def health(self) -> dict[str, Any]:
+        return self._get("/health")
+
+    def wrist(self) -> dict[str, Any]:
+        """Current wrist poses. Server shape: {'left_wrist': {'position','quaternion'}, 'right_wrist': {...}}."""
+        return self._get("/wrist")
+
+    def state(self) -> dict[str, Any]:
+        """Full live snapshot: arms (current+target+status), hands, locked, dds_ready."""
+        return self._get("/state")
+
+    def hand(self) -> dict[str, Any]:
+        """Hand finger state. Shape: {'left': {'fingers': {pinky,ring,middle,index,thumb_bend,thumb_rotation}, 'age_s'}, 'right': {...}}."""
+        return self._get("/hand")
+
+    # ----- mutating -----
+
+    def wrist_absolute(self, side: str, position: list[float], quaternion: list[float],
+                       wait: bool = True, timeout_s: float = 5.0, precise: bool = False) -> dict[str, Any]:
+        body = {
+            "side": side, "position": position, "quaternion": quaternion,
+            "wait": wait, "timeout_s": timeout_s, "precise": precise,
+        }
+        return self._post("/wrist/absolute", body)
+
+    def wrist_relative(self, side: str, delta_position: list[float] | None = None,
+                       delta_quaternion: list[float] | None = None, wait: bool = True,
+                       timeout_s: float = 5.0) -> dict[str, Any]:
+        body: dict[str, Any] = {"side": side, "wait": wait, "timeout_s": timeout_s}
+        if delta_position is not None:
+            body["delta_position"] = delta_position
+        if delta_quaternion is not None:
+            body["delta_quaternion"] = delta_quaternion
+        return self._post("/wrist/relative", body)
+
+    def hand_action(self, action: str) -> dict[str, Any]:
+        """action ∈ {LEFT_OPEN, LEFT_CLOSE, RIGHT_OPEN, RIGHT_CLOSE}."""
+        return self._post("/hand/action", {"action": action})
+
+    def hand_control(self, side: str, fingers: dict[str, float]) -> dict[str, Any]:
+        """Per-finger control (normalized 0..1). Unspecified fingers keep their last value."""
+        return self._post("/hand/control", {"side": side, "fingers": fingers})
+
+    def emergency_stop(self) -> dict[str, Any]:
+        return self._post("/emergency_stop", {})
+
+    def unlock(self) -> dict[str, Any]:
+        return self._post("/unlock", {})
+
+    # ----- robot FSM / locomotion -----
+
+    def robot_state_get(self) -> dict[str, Any]:
+        return self._get("/robot/state")
+
+    def robot_state_set(self, state: str) -> dict[str, Any]:
+        return self._post("/robot/state", {"state": state})
+
+    def robot_arm_mode(self, mode: str) -> dict[str, Any]:
+        return self._post("/robot/arm_mode", {"mode": mode})
+
+    def robot_stand(self, mode: str) -> dict[str, Any]:
+        return self._post("/robot/stand", {"mode": mode})
+
+    def robot_move(self, vx: float, vy: float, vyaw: float, continuous: bool = False) -> dict[str, Any]:
+        return self._post("/robot/move", {"vx": vx, "vy": vy, "vyaw": vyaw, "continuous": continuous})
+
+    def robot_stopmove(self) -> dict[str, Any]:
+        return self._post("/robot/stopmove", {})
+
+    def arms_compliance(self) -> dict[str, Any]:
+        return self._get("/arms/compliance")
+
+    def arms_compliance_start(self) -> dict[str, Any]:
+        return self._post("/arms/compliance/start", {})
+
+    def arms_compliance_stop(self) -> dict[str, Any]:
+        return self._post("/arms/compliance/stop", {})
+
+    def arms_go_neutral(self) -> dict[str, Any]:
+        return self._post("/arms/go_neutral", {})
+
+    def arms_loco_reset(self) -> dict[str, Any]:
+        return self._post("/arms/loco_reset", {})
+
+    def diagnostics(self) -> dict[str, Any]:
+        """Motor temperatures / fault codes / enabled flags extracted from the
+        G1 lowstate. Optional on the upstream — a 404 here means the G1 API
+        version in use does not expose diagnostics yet."""
+        return self._get("/diagnostics")
+
+    # ----- presets (server-side persistence of pose+fingers snapshots) -----
+
+    def presets(self) -> dict[str, Any]:
+        return self._get("/presets")
+
+    def preset_get(self, name: str) -> dict[str, Any]:
+        return self._get(f"/presets/{name}")
+
+    def preset_save(self, name: str) -> dict[str, Any]:
+        return self._post("/presets/save", {"name": name})
+
+    def preset_execute(self, name: str) -> dict[str, Any]:
+        return self._post(f"/presets/{name}/execute", {})
+
+    def preset_delete(self, name: str) -> dict[str, Any]:
+        return self._delete(f"/presets/{name}")
+
+    # ----- internals -----
+
+    def _check_status(self, r: httpx.Response, verb: str, path: str) -> None:
+        if r.is_success:
+            return
+        # Forward semantic client-side errors (409, 412, 422, …) with detail.
+        try:
+            body = r.json()
+            detail = body.get("detail", r.text) if isinstance(body, dict) else r.text
+        except Exception:
+            detail = r.text
+        raise G1StatusError(r.status_code, str(detail), f"{verb} {path}")
+
+    def _get(self, path: str) -> dict[str, Any]:
+        try:
+            with httpx.Client(timeout=self.timeout_s) as c:
+                r = c.get(f"{self.base_url}{path}")
+            self._check_status(r, "GET", path)
+            return r.json()
+        except (httpx.HTTPError, OSError) as e:
+            raise G1Unavailable(f"G1 GET {path} failed: {e}") from e
+
+    def _post(self, path: str, body: dict[str, Any]) -> dict[str, Any]:
+        if self.dry_run:
+            log.info("[DRY-RUN] G1 POST %s body=%s", path, body)
+            return {"ok": True, "dry_run": True, "path": path, "body": body}
+        try:
+            with httpx.Client(timeout=self.write_timeout_s) as c:
+                r = c.post(f"{self.base_url}{path}", json=body)
+            self._check_status(r, "POST", path)
+            return r.json()
+        except (httpx.HTTPError, OSError) as e:
+            raise G1Unavailable(f"G1 POST {path} failed: {e}") from e
+
+    def _delete(self, path: str) -> dict[str, Any]:
+        if self.dry_run:
+            log.info("[DRY-RUN] G1 DELETE %s", path)
+            return {"ok": True, "dry_run": True, "path": path}
+        try:
+            with httpx.Client(timeout=self.write_timeout_s) as c:
+                r = c.delete(f"{self.base_url}{path}")
+            self._check_status(r, "DELETE", path)
+            return r.json()
+        except (httpx.HTTPError, OSError) as e:
+            raise G1Unavailable(f"G1 DELETE {path} failed: {e}") from e

src/presa_mano/clients/marker.py

@@ -0,0 +1,68 @@
+"""Client for the marker-finder service (port 8082).
+
+Marker-finder accepts OpenCV intrinsics (`camera_matrix` 3x3 + `dist_coeffs`)
+either as a per-request `intrinsics` form field on `/detect`, or as a stored
+default via `POST /intrinsics`. We push the live Nuwa intrinsics at startup
+(see main.py) so every subsequent `/detect` produces correct poses with no
+extra work, and we still expose a per-call override for unusual cases.
+"""
+
+from __future__ import annotations
+
+import json
+from typing import Any
+
+import httpx
+
+
+class MarkerClient:
+    def __init__(self, base_url: str, timeout_s: float = 5.0):
+        self.base_url = base_url.rstrip("/")
+        self.timeout_s = timeout_s
+
+    def detect_url(self, markers: list[dict[str, Any]], image_url: str,
+                   intrinsics: dict[str, Any] | None = None) -> dict[str, Any]:
+        """Returns {'found': [...], 'not_found': [...]}.
+
+        Each found item includes 'corners' (Nx2), 'id', 'size_mm', 'position_m',
+        'rotation_matrix', 'quaternion_xyzw'. Pose is computed by marker-finder
+        with the active default intrinsics (or the per-request override).
+
+        Args:
+            intrinsics: optional per-call override, OpenCV format
+                `{"camera_matrix": [[fx,0,cx],[0,fy,cy],[0,0,1]],
+                  "dist_coeffs": [k1,k2,p1,p2,k3]}`
+                (mostly unused now that we push the Nuwa defaults at startup;
+                kept for tools that want to detect against a custom camera).
+        """
+        data = {"markers": json.dumps(markers), "image_url": image_url}
+        if intrinsics is not None:
+            data["intrinsics"] = json.dumps(intrinsics)
+        with httpx.Client(timeout=self.timeout_s) as c:
+            r = c.post(f"{self.base_url}/detect", data=data)
+        r.raise_for_status()
+        return r.json()
+
+    def get_intrinsics(self) -> dict[str, Any]:
+        """Currently-active default intrinsics on marker-finder."""
+        with httpx.Client(timeout=self.timeout_s) as c:
+            r = c.get(f"{self.base_url}/intrinsics")
+        r.raise_for_status()
+        return r.json()
+
+    def set_intrinsics(self, camera_matrix: list[list[float]],
+                       dist_coeffs: list[float],
+                       persist: bool = False) -> dict[str, Any]:
+        """Replace marker-finder's default intrinsics.
+
+        `persist=True` writes them into marker-finder's config.yaml so they
+        survive a restart. We default to in-memory only — presa-mano repushes
+        them at every startup, so persisting would just create config drift.
+        """
+        body = {"camera_matrix": camera_matrix, "dist_coeffs": dist_coeffs}
+        with httpx.Client(timeout=self.timeout_s) as c:
+            r = c.post(f"{self.base_url}/intrinsics",
+                       params={"persist": "true" if persist else "false"},
+                       json=body)
+        r.raise_for_status()
+        return r.json()

src/presa_mano/clients/nuwa.py

@@ -0,0 +1,105 @@
+"""Client for Nuwa-HP60C depth camera (port 8877)."""
+
+from __future__ import annotations
+
+import io
+from typing import Any
+
+import httpx
+import numpy as np
+
+
+class NuwaError(RuntimeError):
+    pass
+
+
+class NuwaClient:
+    def __init__(self, base_url: str, timeout_s: float = 5.0, api_key: str | None = None):
+        self.base_url = base_url.rstrip("/")
+        self.timeout_s = timeout_s
+        self._headers = {"x-api-key": api_key} if api_key else {}
+
+    def _get(self, path: str, **kwargs: Any) -> httpx.Response:
+        with httpx.Client(timeout=self.timeout_s, headers=self._headers) as c:
+            r = c.get(f"{self.base_url}{path}", **kwargs)
+        r.raise_for_status()
+        return r
+
+    def health(self) -> dict[str, Any]:
+        return self._get("/api/health").json()
+
+    def status(self) -> dict[str, Any]:
+        return self._get("/api/status").json()
+
+    def rgb_url(self) -> str:
+        """URL that upstream services (marker-finder, yolo) can use to pull the current RGB frame."""
+        return f"{self.base_url}/api/rgb"
+
+    def rgb_jpeg(self) -> bytes:
+        return self._get("/api/rgb").content
+
+    def intrinsics(self) -> dict[str, Any]:
+        """Returns {fx, fy, cx, cy, width, height, dist_coeffs?}."""
+        data = self._get("/api/intrinsics").json()
+        if isinstance(data, dict) and "data" in data:
+            payload = data.get("data") or {}
+            if not payload:
+                raise NuwaError(data.get("error") or "intrinsics unavailable")
+            return payload
+        return data
+
+    def depth_png(self) -> bytes:
+        """16-bit depth PNG (values in mm)."""
+        return self._get("/api/depth").content
+
+    def pointcloud(self, step: int = 4, max_depth_mm: int = 4000) -> np.ndarray:
+        """Returns Nx6 array of [x,y,z,r,g,b] in camera frame (meters, uint8)."""
+        points, _ = self.pointcloud_with_meta(step=step, max_depth_mm=max_depth_mm)
+        return points
+
+    def pointcloud_with_meta(self, step: int = 4, max_depth_mm: int = 4000
+                             ) -> tuple[np.ndarray, dict[str, Any]]:
+        """Full-frame point cloud in Nuwa native frame (X-right, Y-up, Z-back).
+        Payload: {"data": {"points": [[x,y,z,r,g,b],...], "intrinsics":{...}}}.
+        """
+        return self._fetch_pointcloud("/api/pointcloud",
+                                      {"step": step, "max_depth_mm": max_depth_mm})
+
+    def pointcloud_roi(self, u_min: int, v_min: int, u_max: int, v_max: int,
+                       step: int = 1, max_depth_mm: int = 4000
+                       ) -> tuple[np.ndarray, dict[str, Any]]:
+        """Crop-and-fetch: Nuwa computes only the points inside the pixel ROI.
+
+        IMPORTANT: points come back in `[x, -y, -z, r, g, b]` — i.e. OpenCV
+        camera frame (X-right, Y-down, Z-forward). Compatible with the
+        hand-eye matrix `T_base_camera` (also OpenCV-frame).
+        """
+        return self._fetch_pointcloud("/api/pointcloud/roi", {
+            "u_min": int(u_min), "v_min": int(v_min),
+            "u_max": int(u_max), "v_max": int(v_max),
+            "step": int(step), "max_depth_mm": int(max_depth_mm),
+        })
+
+    def _fetch_pointcloud(self, path: str, params: dict[str, Any]
+                          ) -> tuple[np.ndarray, dict[str, Any]]:
+        r = self._get(path, params=params)
+        payload = r.json()
+        data = payload.get("data", payload) if isinstance(payload, dict) else payload
+        if isinstance(data, dict):
+            points_raw = data.get("points") or []
+            meta = {k: v for k, v in data.items() if k != "points"}
+        else:
+            points_raw = data
+            meta = {}
+        if not points_raw:
+            return np.zeros((0, 6), dtype=np.float64), meta
+        arr = np.asarray(points_raw, dtype=np.float64)
+        if arr.ndim != 2 or arr.shape[1] < 3:
+            raise NuwaError(f"unexpected pointcloud shape: {arr.shape}")
+        return arr, meta
+
+    def measure(self, u: int, v: int, radius: int = 6) -> dict[str, Any]:
+        """Back-project pixel (u,v) to 3D in camera frame."""
+        r = self._get("/api/measure", params={"u": u, "v": v, "radius": radius})
+        data = r.json()
+        return data.get("data", data) if isinstance(data, dict) else data

src/presa_mano/clients/yolo.py

@@ -0,0 +1,49 @@
+"""Client for the YOLO server (port 8879)."""
+
+from __future__ import annotations
+
+from typing import Any
+
+import httpx
+
+
+class YoloClient:
+    def __init__(self, base_url: str, timeout_s: float = 10.0):
+        self.base_url = base_url.rstrip("/")
+        self.timeout_s = timeout_s
+
+    def detect_url(self, image_url: str, conf: float | None = None) -> dict[str, Any]:
+        data: dict[str, Any] = {"url": image_url}
+        if conf is not None:
+            data["conf"] = str(conf)
+        with httpx.Client(timeout=self.timeout_s) as c:
+            r = c.post(f"{self.base_url}/detect", data=data)
+        r.raise_for_status()
+        return r.json()
+
+    def classes(self) -> list[dict[str, Any]]:
+        """Returns [{id, name}, ...]. Empty list if yolo-server doesn't expose /classes
+        yet or the model carries no names. Accepts both flat and {success,data} shapes."""
+        with httpx.Client(timeout=self.timeout_s) as c:
+            r = c.get(f"{self.base_url}/classes")
+        if r.status_code == 404:
+            return []
+        r.raise_for_status()
+        payload: Any = r.json()
+        if isinstance(payload, dict) and isinstance(payload.get("data"), dict):
+            payload = payload["data"]
+        items: Any = payload.get("classes") if isinstance(payload, dict) else payload
+        out: list[dict[str, Any]] = []
+        if isinstance(items, dict):
+            # {0: "Base", 1: "Mortaio", ...}
+            items = [{"id": int(k), "name": str(v)} for k, v in items.items()]
+        for i, it in enumerate(items or []):
+            if isinstance(it, str):
+                out.append({"id": i, "name": it})
+            elif isinstance(it, dict):
+                name = it.get("name") or it.get("label")
+                if name is None:
+                    continue
+                out.append({"id": int(it.get("id", i)), "name": str(name)})
+        out.sort(key=lambda x: x["id"])
+        return out

src/presa_mano/config.py

@@ -0,0 +1,73 @@
+"""Configuration loader with runtime override support."""
+
+from __future__ import annotations
+
+import os
+import threading
+from copy import deepcopy
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+
+
+
+_LOCK = threading.RLock()
+_STATE: dict[str, Any] = {"config": None, "path": None}
+
+
+def _default_config_path() -> Path:
+    env = os.environ.get("PRESA_MANO_CONFIG")
+    if env:
+        return Path(env)
+    return Path(__file__).resolve().parents[2] / "config.yaml"
+
+
+def data_dir() -> Path:
+    env = os.environ.get("PRESA_MANO_DATA")
+    p = Path(env) if env else _default_config_path().parent / "data"
+    p.mkdir(parents=True, exist_ok=True)
+    return p
+
+
+def load_config(path: Path | None = None) -> dict[str, Any]:
+    with _LOCK:
+        if _STATE["config"] is not None and path is None:
+            return deepcopy(_STATE["config"])
+        cfg_path = path or _default_config_path()
+        with open(cfg_path, "r") as f:
+            cfg = yaml.safe_load(f) or {}
+        _STATE["config"] = cfg
+        _STATE["path"] = cfg_path
+        return deepcopy(cfg)
+
+
+def get_config() -> dict[str, Any]:
+    with _LOCK:
+        if _STATE["config"] is None:
+            load_config()
+        return deepcopy(_STATE["config"])
+
+
+def update_config(patch: dict[str, Any]) -> dict[str, Any]:
+    """Merge a patch into the in-memory config and persist to disk."""
+    with _LOCK:
+        if _STATE["config"] is None:
+            load_config()
+        merged = _deep_merge(_STATE["config"], patch)
+        _STATE["config"] = merged
+        if _STATE["path"]:
+            with open(_STATE["path"], "w") as f:
+                yaml.safe_dump(merged, f, sort_keys=False)
+        return deepcopy(merged)
+
+
+def _deep_merge(base: dict[str, Any], patch: dict[str, Any]) -> dict[str, Any]:
+    out = deepcopy(base)
+    for k, v in patch.items():
+        if isinstance(v, dict) and isinstance(out.get(k), dict):
+            out[k] = _deep_merge(out[k], v)
+        else:
+            out[k] = v
+    return out

src/presa_mano/geometry.py

@@ -0,0 +1,111 @@
+"""SE(3) helpers: compose, invert, conversions rvec/quat/euler/matrix."""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
+    import cv2
+    R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=np.float64).reshape(3, 1))
+    return R
+
+
+def R_to_rvec(R: np.ndarray) -> np.ndarray:
+    import cv2
+    rvec, _ = cv2.Rodrigues(np.asarray(R, dtype=np.float64))
+    return rvec.flatten()
+
+
+def quat_xyzw_to_R(q: np.ndarray) -> np.ndarray:
+    x, y, z, w = [float(v) for v in q]
+    n = x * x + y * y + z * z + w * w
+    if n == 0.0:
+        return np.eye(3)
+    s = 2.0 / n
+    return np.array([
+        [1 - s * (y * y + z * z), s * (x * y - z * w),     s * (x * z + y * w)],
+        [s * (x * y + z * w),     1 - s * (x * x + z * z), s * (y * z - x * w)],
+        [s * (x * z - y * w),     s * (y * z + x * w),     1 - s * (x * x + y * y)],
+    ])
+
+
+def R_to_quat_xyzw(R: np.ndarray) -> np.ndarray:
+    m = np.asarray(R, dtype=np.float64)
+    t = m[0, 0] + m[1, 1] + m[2, 2]
+    if t > 0:
+        s = np.sqrt(t + 1.0) * 2
+        w = 0.25 * s
+        x = (m[2, 1] - m[1, 2]) / s
+        y = (m[0, 2] - m[2, 0]) / s
+        z = (m[1, 0] - m[0, 1]) / s
+    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
+        s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2
+        w = (m[2, 1] - m[1, 2]) / s
+        x = 0.25 * s
+        y = (m[0, 1] + m[1, 0]) / s
+        z = (m[0, 2] + m[2, 0]) / s
+    elif m[1, 1] > m[2, 2]:
+        s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2
+        w = (m[0, 2] - m[2, 0]) / s
+        x = (m[0, 1] + m[1, 0]) / s
+        y = 0.25 * s
+        z = (m[1, 2] + m[2, 1]) / s
+    else:
+        s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2
+        w = (m[1, 0] - m[0, 1]) / s
+        x = (m[0, 2] + m[2, 0]) / s
+        y = (m[1, 2] + m[2, 1]) / s
+        z = 0.25 * s
+    return np.array([x, y, z, w])
+
+
+def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = np.asarray(t, dtype=np.float64).flatten()
+    return T
+
+
+def invert_T(T: np.ndarray) -> np.ndarray:
+    R = T[:3, :3]
+    t = T[:3, 3]
+    Ti = np.eye(4)
+    Ti[:3, :3] = R.T
+    Ti[:3, 3] = -R.T @ t
+    return Ti
+
+
+def T_from_pos_quat(pos: np.ndarray, quat_xyzw: np.ndarray) -> np.ndarray:
+    return make_T(quat_xyzw_to_R(np.asarray(quat_xyzw)), np.asarray(pos))
+
+
+def pos_quat_from_T(T: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    return T[:3, 3].copy(), R_to_quat_xyzw(T[:3, :3])
+
+
+def T_from_rvec_tvec(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray:
+    return make_T(rvec_to_R(rvec), np.asarray(tvec).flatten())
+
+
+def R_to_euler_xyz(R: np.ndarray) -> np.ndarray:
+    """Extrinsic XYZ euler (roll, pitch, yaw) in radians."""
+    sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
+    singular = sy < 1e-6
+    if not singular:
+        x = np.arctan2(R[2, 1], R[2, 2])
+        y = np.arctan2(-R[2, 0], sy)
+        z = np.arctan2(R[1, 0], R[0, 0])
+    else:
+        x = np.arctan2(-R[1, 2], R[1, 1])
+        y = np.arctan2(-R[2, 0], sy)
+        z = 0.0
+    return np.array([x, y, z])
+
+
+def transform_points(T: np.ndarray, pts: np.ndarray) -> np.ndarray:
+    """Apply T (4x4) to Nx3 points."""
+    pts = np.asarray(pts, dtype=np.float64)
+    R = T[:3, :3]
+    t = T[:3, 3]
+    return pts @ R.T + t

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,
+    )

src/presa_mano/main.py

@@ -0,0 +1,110 @@
+"""FastAPI entrypoint for presa-mano."""
+
+from __future__ import annotations
+
+import logging
+from pathlib import Path
+
+from fastapi import FastAPI, Request
+from fastapi.responses import HTMLResponse, JSONResponse
+from fastapi.staticfiles import StaticFiles
+
+from .api import (
+    agent_routes,
+    calibration_routes,
+    config_routes,
+    grasp_routes,
+    home_routes,
+    perception_routes,
+    posture_routes,
+    robot_routes,
+    wrist_routes,
+)
+from .clients import G1StatusError
+from .config import load_config
+
+logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s")
+
+app = FastAPI(
+    title="presa-mano",
+    description="Hand-eye calibration + object grasping pipeline (Nuwa + ArUco + YOLO + G1)",
+    version="0.1.0",
+    docs_url="/apidocs",
+    redoc_url="/redoc",
+)
+
+app.include_router(config_routes.router, prefix="/api/config", tags=["config"])
+app.include_router(home_routes.router, prefix="/api/home", tags=["home"])
+app.include_router(agent_routes.router, prefix="/api/agent", tags=["agent"])
+app.include_router(calibration_routes.router, prefix="/api/calibration", tags=["calibration"])
+app.include_router(perception_routes.router, prefix="/api/perception", tags=["perception"])
+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"])
+
+WEB_DIR = Path(__file__).resolve().parents[2] / "web"
+if WEB_DIR.exists():
+    app.mount("/static", StaticFiles(directory=str(WEB_DIR)), name="static")
+
+
+@app.on_event("startup")
+def _startup() -> None:
+    load_config()
+    _push_intrinsics_to_marker_finder()
+
+
+def _push_intrinsics_to_marker_finder() -> None:
+    """Read live RGB intrinsics from Nuwa and push them as the new default
+    on marker-finder. After this every `/detect` produces poses computed with
+    the correct intrinsics, so callers don't have to override per-request.
+
+    Best-effort: if Nuwa or marker-finder is down we just log and continue —
+    presa-mano must come up regardless. Callers can also pass intrinsics
+    per-request as a fallback.
+    """
+    log = logging.getLogger(__name__)
+    try:
+        from .api import _deps
+        from .perception.aruco_pose import nuwa_intrinsics_to_opencv_payload
+        intr = _deps.nuwa().intrinsics()
+        payload = nuwa_intrinsics_to_opencv_payload(intr)
+        _deps.marker().set_intrinsics(
+            camera_matrix=payload["camera_matrix"],
+            dist_coeffs=payload["dist_coeffs"],
+            persist=False,
+        )
+        log.info("pushed Nuwa intrinsics to marker-finder")
+    except Exception as e:
+        log.warning("could not push Nuwa intrinsics to marker-finder: %s "
+                    "(callers can still pass them per-request)", e)
+
+
+@app.exception_handler(G1StatusError)
+def _g1_status_error(_: Request, exc: G1StatusError) -> JSONResponse:
+    # Forward the upstream status code verbatim — 409 (conflict, arms busy),
+    # 412 (precondition: DDS not ready), etc. are actionable by the caller and
+    # should not be flattened into a generic 502.
+    return JSONResponse(
+        status_code=exc.status,
+        content={"detail": f"G1 {exc.path}: {exc.detail}"},
+    )
+
+
+@app.get("/health", tags=["meta"])
+def health() -> dict:
+    return {"status": "ok"}
+
+
+@app.get("/", include_in_schema=False)
+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", "autocal3d.js", "servo.js", "style.css"):
+        f = WEB_DIR / name
+        if f.exists():
+            v = int(f.stat().st_mtime)
+            html = html.replace(f"/static/{name}", f"/static/{name}?v={v}")
+    # Also tell the browser not to cache the shell itself.
+    return HTMLResponse(html, headers={"Cache-Control": "no-store"})

src/presa_mano/perception/__init__.py

@@ -0,0 +1,4 @@
+from .detector import ObjectDetector
+from .pointcloud import crop_bbox, filter_outliers, nuwa_to_cv_frame
+
+__all__ = ["ObjectDetector", "crop_bbox", "filter_outliers", "nuwa_to_cv_frame"]

src/presa_mano/perception/aruco_pose.py

@@ -0,0 +1,97 @@
+"""Local ArUco pose estimation with Nuwa RGB intrinsics.
+
+The marker-finder service bakes D435i intrinsics into its solvePnP, which
+produces systematic pose errors on Nuwa images. We recompute the pose from the
+2D corners (still trustworthy) using the real Nuwa intrinsics, disambiguating
+the planar-marker ambiguity via IPPE_SQUARE + smallest reprojection error.
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+import cv2
+import numpy as np
+
+
+def nuwa_intrinsics_to_matrix(intr: dict[str, Any]
+                              ) -> tuple[np.ndarray, np.ndarray]:
+    """Accept the `data` payload from Nuwa /api/intrinsics and return
+    (K_3x3, dist_5x1) for RGB. Distortion fields default to 0 if absent."""
+    rgb = intr.get("rgb") if isinstance(intr, dict) else None
+    if not isinstance(rgb, dict):
+        raise ValueError(f"Nuwa intrinsics missing 'rgb' block: {intr!r}")
+    K = np.array([
+        [float(rgb["fx"]), 0.0,              float(rgb["cx"])],
+        [0.0,              float(rgb["fy"]), float(rgb["cy"])],
+        [0.0,              0.0,              1.0],
+    ], dtype=np.float64)
+    d = rgb.get("distortion") or {}
+    dist = np.array([
+        float(d.get("k1", 0.0)),
+        float(d.get("k2", 0.0)),
+        float(d.get("p1", 0.0)),
+        float(d.get("p2", 0.0)),
+        float(d.get("k3", 0.0)),
+    ], dtype=np.float64)
+    return K, dist
+
+
+def nuwa_intrinsics_to_opencv_payload(intr: dict[str, Any]) -> dict[str, list]:
+    """Build the wire-format dict that marker-finder's /detect (and /intrinsics
+    POST) expects: `{camera_matrix: 3x3, dist_coeffs: [k1,k2,p1,p2,k3]}`.
+    Plain Python lists so it serialises through JSON."""
+    K, dist = nuwa_intrinsics_to_matrix(intr)
+    return {"camera_matrix": K.tolist(), "dist_coeffs": dist.tolist()}
+
+
+def solve_square_marker(corners_px: np.ndarray, size_m: float,
+                        K: np.ndarray, dist: np.ndarray
+                        ) -> dict[str, Any]:
+    """Recover (R, t) for a square planar marker of side `size_m` from its
+    4 image corners in the order TL, TR, BR, BL (ArUco convention).
+
+    Uses solvePnPGeneric with SOLVEPNP_IPPE_SQUARE, which returns both planar
+    solutions; we pick the one with smaller reprojection error. The runner-up
+    error is also returned so the caller can detect ambiguous detections
+    (both errors similar → the marker is nearly fronto-parallel and the pose
+    is fundamentally underdetermined by a single view).
+    """
+    s = size_m / 2.0
+    obj = np.array([
+        [-s,  s, 0.0],
+        [ s,  s, 0.0],
+        [ s, -s, 0.0],
+        [-s, -s, 0.0],
+    ], dtype=np.float64)
+    img = np.asarray(corners_px, dtype=np.float64).reshape(-1, 2)
+    if img.shape[0] != 4:
+        raise ValueError(f"expected 4 corners, got shape {img.shape}")
+
+    retval, rvecs, tvecs, errs = cv2.solvePnPGeneric(
+        obj, img, K, dist, flags=cv2.SOLVEPNP_IPPE_SQUARE
+    )
+    if not retval or rvecs is None or len(rvecs) == 0:
+        raise RuntimeError("solvePnP (IPPE_SQUARE) failed to converge")
+
+    errs_arr = np.asarray(errs).flatten() if errs is not None else np.array([0.0])
+    best = int(np.argmin(errs_arr)) if errs_arr.size > 0 else 0
+    R, _ = cv2.Rodrigues(rvecs[best])
+    t = np.asarray(tvecs[best]).flatten()
+
+    # runner-up error for ambiguity reporting
+    if errs_arr.size > 1:
+        runner = float(np.partition(errs_arr, 1)[1])
+    else:
+        runner = float("inf")
+    best_err = float(errs_arr[best])
+    # ratio close to 1 means the two solutions are almost equally plausible.
+    ambiguity = float(best_err / runner) if runner > 0 else 0.0
+    return {
+        "R": R,
+        "t": t,
+        "reprojection_error_px": best_err,
+        "runnerup_error_px": runner,
+        "ambiguity_ratio": ambiguity,
+        "n_solutions": int(len(rvecs)),
+    }

src/presa_mano/perception/depth_correction.py

@@ -0,0 +1,136 @@
+"""Per-installation depth-bias correction for the Nuwa-HP60C.
+
+The Nuwa depth sensor exhibits an offset on `depth_mm` that doesn't fit a
+simple multiplicative scale (see /api/perception/measure_rgb experiments
+on a known marker — bias varies with distance). We model the bias as an
+affine map:
+
+    depth_raw = scale * depth_true + offset_mm     (sensor)
+    depth_true = (depth_raw - offset_mm) / scale   (correction)
+
+and fit (scale, offset_mm) from N≥2 samples where `depth_true` is taken
+from a trusted source (typically a PnP-derived Z on a known-size ArUco
+marker held at varying distances).
+
+State (params + samples) lives in a single JSON file so it can be
+inspected/edited by hand:
+
+    data/depth_correction.json
+    {
+      "enabled": true,
+      "scale": 0.871,
+      "offset_mm": 185.0,
+      "fit_meta": {"n_samples": 4, "rms_residual_mm": 2.1, ...},
+      "samples": [
+        {"depth_raw_mm": 1030, "depth_true_mm": 970, "note": "...", "added_at": ...},
+        ...
+      ]
+    }
+"""
+
+from __future__ import annotations
+
+import json
+import time
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+
+def default_state() -> dict[str, Any]:
+    return {
+        "enabled": False,
+        "scale": 1.0,
+        "offset_mm": 0.0,
+        "fit_meta": None,
+        "samples": [],
+    }
+
+
+def load(path: Path) -> dict[str, Any]:
+    if not path.exists():
+        return default_state()
+    try:
+        with open(path, "r", encoding="utf-8") as f:
+            data = json.load(f)
+        if not isinstance(data, dict):
+            return default_state()
+        # Fill in any missing keys with defaults.
+        merged = default_state()
+        merged.update(data)
+        return merged
+    except json.JSONDecodeError:
+        return default_state()
+
+
+def save(path: Path, state: dict[str, Any]) -> None:
+    path.parent.mkdir(parents=True, exist_ok=True)
+    with open(path, "w", encoding="utf-8") as f:
+        json.dump(state, f, ensure_ascii=False, indent=2)
+
+
+def apply_correction(depth_raw_mm: int, state: dict[str, Any] | None) -> int:
+    """Convert a raw sensor depth (mm) to the corrected depth (mm). Returns
+    the input unchanged if correction is disabled or state is None."""
+    if not state or not state.get("enabled") or depth_raw_mm <= 0:
+        return depth_raw_mm
+    scale = float(state.get("scale") or 1.0)
+    offset = float(state.get("offset_mm") or 0.0)
+    if scale <= 0:
+        return depth_raw_mm
+    corrected = (depth_raw_mm - offset) / scale
+    return max(0, int(round(corrected)))
+
+
+def fit_affine(samples: list[dict[str, Any]]) -> dict[str, Any]:
+    """Least-squares fit `depth_raw = scale * depth_true + offset_mm`.
+
+    Needs ≥2 samples with non-degenerate range of depth_true. With exactly
+    2 samples the fit is exact and rms_residual_mm = 0. With more samples
+    the residual is meaningful for assessing fit quality.
+    """
+    if len(samples) < 2:
+        raise ValueError("need at least 2 samples to fit affine model")
+    raw = np.array([float(s["depth_raw_mm"]) for s in samples], dtype=np.float64)
+    true = np.array([float(s["depth_true_mm"]) for s in samples], dtype=np.float64)
+    if true.max() - true.min() < 50:
+        raise ValueError("samples cover too narrow a range of depth_true (<50 mm)")
+    A = np.column_stack([true, np.ones_like(true)])
+    sol, *_ = np.linalg.lstsq(A, raw, rcond=None)
+    scale = float(sol[0])
+    offset = float(sol[1])
+    pred = scale * true + offset
+    residuals = pred - raw
+    rms = float(np.sqrt(np.mean(residuals ** 2)))
+    max_abs = float(np.max(np.abs(residuals)))
+    return {
+        "scale": scale,
+        "offset_mm": offset,
+        "n_samples": len(samples),
+        "rms_residual_mm": rms,
+        "max_abs_residual_mm": max_abs,
+        "depth_true_min_mm": float(true.min()),
+        "depth_true_max_mm": float(true.max()),
+        "fitted_at": time.time(),
+    }
+
+
+def add_sample(state: dict[str, Any], depth_raw_mm: int, depth_true_mm: int,
+               note: str = "") -> dict[str, Any]:
+    samples = list(state.get("samples") or [])
+    samples.append({
+        "depth_raw_mm": int(depth_raw_mm),
+        "depth_true_mm": int(depth_true_mm),
+        "note": note,
+        "added_at": time.time(),
+    })
+    new_state = dict(state)
+    new_state["samples"] = samples
+    return new_state
+
+
+def clear_samples(state: dict[str, Any]) -> dict[str, Any]:
+    new_state = dict(state)
+    new_state["samples"] = []
+    return new_state

src/presa_mano/perception/depth_to_rgb.py

@@ -0,0 +1,189 @@
+"""Back-project an RGB pixel to 3D using the depth sensor, with proper
+IR↔RGB registration.
+
+Why this exists
+---------------
+Nuwa-HP60C is RGB-D with two physically distinct cameras (RGB + IR) that
+have different intrinsics and a small baseline:
+
+    fx_rgb=593.6  fx_ir=445.8     cx_rgb=348.1  cx_ir=316.6
+    t_ir_to_rgb ≈ (-12.05, +0.08, -0.03) mm
+
+Nuwa's `/api/measure?u=&v=` interprets `(u,v)` as **IR/depth-image** pixel
+coords and back-projects with IR intrinsics. So if we have a pixel `(u,v)`
+in the **RGB** image (e.g. a YOLO bbox center, an ArUco corner) and we want
+the actual 3D point in the RGB camera frame, we need to handle the
+registration ourselves.
+
+Algorithm (single fixed-point iteration is enough at typical robot ranges):
+    1. Initial guess depth z0 from sampling depth[v_rgb, u_rgb] (depth is in
+       IR coords — at small baselines it's already close).
+    2. Hypothesise the 3D point on the RGB ray at distance z0.
+    3. Project that 3D point into the IR image → (u_ir, v_ir).
+    4. Sample depth at (u_ir, v_ir) → z1.
+    5. Use (u_ir, v_ir, z1) to back-project with IR intrinsics → P_ir.
+    6. Transform P_ir → P_rgb via the IR→RGB extrinsics. Done.
+
+Two iterations of step 2-4 reduce residual error to sub-mm even with
+mediocre initial guesses; we do two unconditionally because they're cheap.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any
+
+import cv2
+import numpy as np
+
+from ..clients import NuwaClient
+
+
+@dataclass(frozen=True)
+class CameraRegistration:
+    """Cached intrinsics+extrinsics for a Nuwa-HP60C-style RGB-D camera."""
+    K_rgb: np.ndarray          # 3x3
+    K_ir: np.ndarray           # 3x3
+    R_ir_to_rgb: np.ndarray    # 3x3
+    t_ir_to_rgb: np.ndarray    # (3,) in **meters**
+    rgb_width: int
+    rgb_height: int
+    ir_width: int
+    ir_height: int
+
+    @classmethod
+    def from_nuwa(cls, intr: dict[str, Any]) -> "CameraRegistration":
+        rgb = intr.get("rgb") or {}
+        ir = intr.get("ir") or {}
+        ext = intr.get("extrinsics_ir_to_rgb") or {}
+        if not rgb or not ir:
+            raise ValueError(f"Nuwa intrinsics must contain 'rgb' and 'ir' blocks: {intr!r}")
+        K_rgb = np.array([[rgb["fx"], 0, rgb["cx"]],
+                          [0, rgb["fy"], rgb["cy"]],
+                          [0, 0, 1]], dtype=np.float64)
+        K_ir = np.array([[ir["fx"], 0, ir["cx"]],
+                         [0, ir["fy"], ir["cy"]],
+                         [0, 0, 1]], dtype=np.float64)
+        R = np.asarray(ext.get("rotation") or [1, 0, 0, 0, 1, 0, 0, 0, 1], dtype=np.float64)
+        if R.size == 9:
+            R = R.reshape(3, 3)
+        elif R.shape != (3, 3):
+            raise ValueError(f"extrinsics rotation must be 9 elements or 3x3, got {R.shape}")
+        # Translation in Nuwa is in mm; we convert to meters once.
+        t_mm = np.asarray(ext.get("translation") or [0, 0, 0], dtype=np.float64).flatten()
+        t = t_mm / 1000.0
+        return cls(
+            K_rgb=K_rgb, K_ir=K_ir,
+            R_ir_to_rgb=R, t_ir_to_rgb=t,
+            rgb_width=int(rgb.get("width") or 640),
+            rgb_height=int(rgb.get("height") or 480),
+            ir_width=int(ir.get("width") or 640),
+            ir_height=int(ir.get("height") or 480),
+        )
+
+
+def fetch_depth_mm(nuwa: NuwaClient) -> np.ndarray:
+    """Fetch the current depth frame as an int32 array (mm), shape (H, W).
+    Zero values mean 'no depth' — caller should mask them out."""
+    png = nuwa.depth_png()
+    arr = np.frombuffer(png, dtype=np.uint8)
+    img = cv2.imdecode(arr, cv2.IMREAD_UNCHANGED)
+    if img is None:
+        raise RuntimeError("could not decode Nuwa depth PNG")
+    if img.dtype != np.uint16:
+        # Some configurations return 8-bit (color preview); we need raw mm.
+        raise RuntimeError(f"expected 16-bit depth PNG, got dtype={img.dtype}")
+    return img.astype(np.int32)
+
+
+def _sample_depth(depth_mm: np.ndarray, u: float, v: float, radius: int = 4) -> int:
+    """Nearest valid (non-zero) depth value within `radius` of (u,v) on the
+    depth image. Returns 0 if nothing found."""
+    h, w = depth_mm.shape
+    u0, v0 = int(round(u)), int(round(v))
+    if u0 < 0 or v0 < 0 or u0 >= w or v0 >= h:
+        return 0
+    z = int(depth_mm[v0, u0])
+    if z > 0:
+        return z
+    # Spiral / box search outward to `radius`.
+    for r in range(1, radius + 1):
+        u_lo, u_hi = max(0, u0 - r), min(w, u0 + r + 1)
+        v_lo, v_hi = max(0, v0 - r), min(h, v0 + r + 1)
+        patch = depth_mm[v_lo:v_hi, u_lo:u_hi]
+        valid = patch[patch > 0]
+        if valid.size:
+            return int(valid.min())   # nearest along ray = smallest depth
+    return 0
+
+
+def back_project_rgb_pixel(
+    u_rgb: float, v_rgb: float,
+    depth_mm: np.ndarray,
+    reg: CameraRegistration,
+    *, sample_radius: int = 4, n_iters: int = 2,
+    depth_correction: dict[str, Any] | None = None,
+) -> dict[str, Any]:
+    """Back-project an RGB pixel using the registered depth image.
+
+    If `depth_correction` is supplied (dict with `enabled`, `scale`,
+    `offset_mm`), each raw depth sample is corrected before back-projection.
+    See `depth_correction.apply_correction`.
+
+    Returns a dict:
+        {
+            "ok": bool,
+            "rgb_pixel": [u, v],
+            "ir_pixel": [u_ir, v_ir],
+            "depth_mm_raw": int,        # what the sensor reported
+            "depth_mm": int,            # corrected (== raw if correction off)
+            "point_rgb_m": [X, Y, Z],   # in RGB camera frame, OpenCV convention
+            "point_ir_m":  [X, Y, Z],   # in IR camera frame
+        }
+    """
+    from .depth_correction import apply_correction
+
+    out: dict[str, Any] = {"ok": False, "rgb_pixel": [float(u_rgb), float(v_rgb)]}
+    K_rgb_inv = np.linalg.inv(reg.K_rgb)
+
+    ray_rgb = K_rgb_inv @ np.array([u_rgb, v_rgb, 1.0])
+
+    z_raw_first = _sample_depth(depth_mm, u_rgb, v_rgb, radius=sample_radius)
+    if z_raw_first <= 0:
+        out["error"] = f"no valid depth near RGB pixel ({u_rgb:.0f},{v_rgb:.0f})"
+        return out
+
+    z_guess = apply_correction(z_raw_first, depth_correction)
+    P_rgb = (z_guess / 1000.0) * ray_rgb
+
+    u_ir = v_ir = None
+    z_raw = z_raw_first
+    z_corr = z_guess
+    for _ in range(max(1, n_iters)):
+        P_ir = reg.R_ir_to_rgb.T @ (P_rgb - reg.t_ir_to_rgb)
+        if P_ir[2] <= 0:
+            out["error"] = "RGB ray hits behind IR camera"
+            return out
+        u_ir = reg.K_ir[0, 0] * P_ir[0] / P_ir[2] + reg.K_ir[0, 2]
+        v_ir = reg.K_ir[1, 1] * P_ir[1] / P_ir[2] + reg.K_ir[1, 2]
+        z_raw = _sample_depth(depth_mm, u_ir, v_ir, radius=sample_radius)
+        if z_raw <= 0:
+            break
+        z_corr = apply_correction(z_raw, depth_correction)
+        K_ir_inv = np.linalg.inv(reg.K_ir)
+        P_ir_new = (z_corr / 1000.0) * (K_ir_inv @ np.array([u_ir, v_ir, 1.0]))
+        P_rgb = reg.R_ir_to_rgb @ P_ir_new + reg.t_ir_to_rgb
+
+    if z_raw <= 0:
+        out["error"] = f"no valid depth near IR pixel ({u_ir:.0f},{v_ir:.0f})"
+        return out
+
+    out.update({
+        "ok": True,
+        "ir_pixel": [float(u_ir), float(v_ir)],
+        "depth_mm_raw": int(z_raw),
+        "depth_mm": int(z_corr),
+        "point_rgb_m": P_rgb.tolist(),
+        "point_ir_m": (reg.R_ir_to_rgb.T @ (P_rgb - reg.t_ir_to_rgb)).tolist(),
+    })
+    return out

src/presa_mano/perception/detector.py

@@ -0,0 +1,64 @@
+"""Wrap YOLO detection + parse response."""
+
+from __future__ import annotations
+
+from typing import Any
+
+from ..clients import NuwaClient, YoloClient
+
+
+class ObjectDetector:
+    def __init__(self, yolo: YoloClient, nuwa: NuwaClient):
+        self.yolo = yolo
+        self.nuwa = nuwa
+
+    def detect(self, conf: float | None = None) -> list[dict[str, Any]]:
+        """Returns list of {class, confidence, bbox: {x1,y1,x2,y2}} from current Nuwa RGB."""
+        raw = self.yolo.detect_url(self.nuwa.rgb_url(), conf=conf)
+        return _normalize(raw)
+
+
+def _normalize(payload: Any) -> list[dict[str, Any]]:
+    """YOLO responses vary; try to extract a list of detections uniformly.
+
+    Supports:
+      - [ {...}, {...} ]                               # plain list
+      - {"detections": [...]}                          # flat
+      - {"success": true, "data": {"detections":[...]}}# envelope used by this yolo-server
+    """
+    # Unwrap {success, data, error} envelope
+    if isinstance(payload, dict) and isinstance(payload.get("data"), dict):
+        payload = payload["data"]
+
+    items: Any = payload
+    if isinstance(payload, dict):
+        items = []
+        for key in ("detections", "predictions", "results"):
+            if isinstance(payload.get(key), list):
+                items = payload[key]
+                break
+
+    out: list[dict[str, Any]] = []
+    for it in items or []:
+        bbox = it.get("bbox") or it.get("box") or it
+        x1, y1, x2, y2 = _xyxy(bbox)
+        out.append({
+            "class": it.get("class") or it.get("label") or it.get("name"),
+            "confidence": float(it.get("confidence") or it.get("conf") or it.get("score") or 0.0),
+            "bbox": {"x1": x1, "y1": y1, "x2": x2, "y2": y2},
+        })
+    return out
+
+
+def _xyxy(b: Any) -> tuple[float, float, float, float]:
+    if isinstance(b, dict):
+        if "x1" in b:
+            return float(b["x1"]), float(b["y1"]), float(b["x2"]), float(b["y2"])
+        if "xmin" in b:
+            return float(b["xmin"]), float(b["ymin"]), float(b["xmax"]), float(b["ymax"])
+        if "x" in b and "w" in b:
+            x, y, w, h = float(b["x"]), float(b["y"]), float(b["w"]), float(b["h"])
+            return x, y, x + w, y + h
+    if isinstance(b, (list, tuple)) and len(b) >= 4:
+        return float(b[0]), float(b[1]), float(b[2]), float(b[3])
+    raise ValueError(f"cannot parse bbox: {b}")

src/presa_mano/perception/pointcloud.py

@@ -0,0 +1,65 @@
+"""Point cloud utilities: crop to bbox in image space + outlier filtering.
+
+Nuwa /api/pointcloud returns points in camera frame; we need to project them
+back to pixel space to keep only those inside a YOLO bbox.
+
+Frame note:
+  Nuwa publishes points in X-right / Y-up / Z-back (Z<0 in front of camera).
+  `cv2.solvePnP` (used by marker-finder to produce T_cam_marker, hence
+  T_base_camera after hand-eye) uses X-right / Y-down / Z-forward (Z>0 in front).
+  Before applying T_base_camera we flip Y and Z via `nuwa_to_cv_frame`.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+def nuwa_to_cv_frame(points_xyz: np.ndarray) -> np.ndarray:
+    """Flip Y and Z to convert Nuwa camera points to OpenCV convention."""
+    pts = np.asarray(points_xyz, dtype=np.float64).copy()
+    pts[:, 1] *= -1.0
+    pts[:, 2] *= -1.0
+    return pts
+
+
+def project(points_cam: np.ndarray, K: np.ndarray) -> np.ndarray:
+    """Project Nx3 camera-frame points to Nx2 pixels via pinhole."""
+    pts = np.asarray(points_cam, dtype=np.float64)
+    z = pts[:, 2]
+    z = np.where(z == 0, 1e-9, z)
+    u = K[0, 0] * pts[:, 0] / z + K[0, 2]
+    v = K[1, 1] * pts[:, 1] / z + K[1, 2]
+    return np.stack([u, v], axis=1)
+
+
+def crop_bbox(points_cam: np.ndarray, bbox_xyxy: tuple[float, float, float, float],
+              K: np.ndarray) -> np.ndarray:
+    """Keep only 3D points whose projection falls inside bbox (in pixel coords).
+
+    Works regardless of depth-sign convention (OpenCV: Z>0 in front;
+    Nuwa/OpenGL-like: Z<0 in front) — the server-provided point cloud already
+    obeys u = fx·x/z + cx with its own convention.
+    """
+    if points_cam.size == 0:
+        return points_cam[:0, :]
+    xyz = points_cam[:, :3]
+    uv = project(xyz, K)
+    x1, y1, x2, y2 = bbox_xyxy
+    mask = (
+        (uv[:, 0] >= x1) & (uv[:, 0] <= x2) &
+        (uv[:, 1] >= y1) & (uv[:, 1] <= y2) &
+        (np.abs(xyz[:, 2]) > 1e-6)
+    )
+    return points_cam[mask]
+
+
+def filter_outliers(points: np.ndarray, n_std: float = 2.0) -> np.ndarray:
+    """Remove points whose distance from centroid exceeds n_std * std."""
+    if len(points) < 10:
+        return points
+    xyz = points[:, :3]
+    c = xyz.mean(axis=0)
+    d = np.linalg.norm(xyz - c, axis=1)
+    keep = d < (d.mean() + n_std * d.std())
+    return points[keep]

src/presa_mano/timing.py

@@ -0,0 +1,31 @@
+"""Simple step-timer used to annotate responses with per-phase latency."""
+
+from __future__ import annotations
+
+import time
+from contextlib import contextmanager
+
+
+class Stopwatch:
+    def __init__(self) -> None:
+        self._t0 = time.perf_counter()
+        self._marks: dict[str, float] = {}
+
+    @contextmanager
+    def step(self, name: str):
+        t0 = time.perf_counter()
+        try:
+            yield
+        finally:
+            self._marks[name] = (time.perf_counter() - t0) * 1000.0
+
+    def add(self, name: str, dt_ms: float) -> None:
+        self._marks[name] = float(dt_ms)
+
+    def total_ms(self) -> float:
+        return (time.perf_counter() - self._t0) * 1000.0
+
+    def as_dict(self) -> dict[str, float]:
+        out = {k: round(v, 2) for k, v in self._marks.items()}
+        out["total"] = round(self.total_ms(), 2)
+        return out