refactor(pose_3d): rimuovi agent/chat, aggiungi cloud+diagnostics+camera polso
Rimosso il modulo agent (LLM chat) e la relativa config OpenRouter. La tab Pose 3D ora: espone i giunti reali via G1 /diagnostics per specchiare l'URDF senza IK; acquisisce point cloud Nuwa+D435i (/api/pose_3d/clouds); mostra il modello 3D della camera sul polso; degrada gracefully a 200 (wrist=null) quando il robot è offline. Il vecchio renderer canvas-2D (app.js view3d + diagnostic3d.js) è stato consolidato nel viewport Three.js di pose_3d.js. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
1 parent
9d30073d57
commit
6c2452fe20
20 files changed
+152757
-2714
No files matched your search
@@ -1,10 +0,0 @@
|
||||
"""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
|
||||
@@ -1,121 +0,0 @@
|
||||
"""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.
|
||||
- **Modalità precise**: per default passa SEMPRE `precise=true` su
|
||||
`move_wrist_absolute` e `move_wrist_relative`. Imposta `precise=false`
|
||||
solo se l'utente lo chiede esplicitamente (es. "muovi veloce",
|
||||
"avvicinati grossomodo"). Lo `precise=true` garantisce che il controller
|
||||
raggiunga la posa target con accuratezza fine — necessario per
|
||||
acquisizione sample di calibrazione, avvicinamento al grasp, e in generale
|
||||
ogni movimento dove la posa finale conta.
|
||||
- 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}}`.
|
||||
|
||||
## Posizione di riposo (home) delle braccia
|
||||
- Quando l'operatore dice **"torna a casa"**, **"home"**, **"posizione di
|
||||
riposo"** o simili, chiama `arms_go_neutral()`. Agisce su entrambe le
|
||||
braccia simultaneamente (è un comando del bridge, niente side da scegliere).
|
||||
- Default `dry_run=true` come per gli altri movimenti — chiedi conferma
|
||||
prima di mandarlo dal vivo.
|
||||
|
||||
## 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"
|
||||
@@ -1,174 +0,0 @@
|
||||
"""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()
|
||||
@@ -1,732 +0,0 @@
|
||||
"""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
|
||||
try:
|
||||
return _ok(**_status())
|
||||
except Exception as e:
|
||||
return _err(str(e))
|
||||
|
||||
|
||||
# ----- 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))
|
||||
precise = bool(args.get("precise", True))
|
||||
result = _deps.g1(dry_run=dry_run).wrist_absolute(
|
||||
side=side, position=position, quaternion=quaternion,
|
||||
wait=True, timeout_s=timeout_s, precise=precise,
|
||||
)
|
||||
return _ok(dry_run=dry_run, side=side, precise=precise, 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))
|
||||
precise = bool(args.get("precise", True))
|
||||
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, precise=precise,
|
||||
)
|
||||
return _ok(dry_run=dry_run, side=side, precise=precise, response=result)
|
||||
except (G1Unavailable, G1StatusError) as e:
|
||||
return _err(f"motion failed: {e}")
|
||||
except Exception as e:
|
||||
return _err(str(e))
|
||||
|
||||
|
||||
def _h_arms_go_neutral(args: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Riporta entrambe le braccia alla posa di home / riposo built-in del
|
||||
bridge (POST /arms/go_neutral). È l'unico modo standard di "tornare a
|
||||
casa" — non serve registrare pose custom."""
|
||||
try:
|
||||
dry_run = bool(args.get("dry_run", True))
|
||||
result = _deps.g1(dry_run=dry_run).arms_go_neutral()
|
||||
return _ok(dry_run=dry_run, response=result)
|
||||
except (G1Unavailable, G1StatusError) as e:
|
||||
return _err(f"go_neutral 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,
|
||||
"arms_go_neutral": _h_arms_go_neutral,
|
||||
"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. Usa precise=true per "
|
||||
"movimenti che richiedono accuratezza fine (es. acquisizione sample di "
|
||||
"calibrazione, avvicinamento finale al grasp); precise=false è il default, "
|
||||
"più veloce ma con tolleranza maggiore."
|
||||
),
|
||||
"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},
|
||||
"precise": {"type": "boolean", "default": True,
|
||||
"description": "true = controllo accurato (più lento, default)"},
|
||||
"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. "
|
||||
"Usa precise=true se serve accuratezza fine."
|
||||
),
|
||||
"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},
|
||||
"precise": {"type": "boolean", "default": True},
|
||||
"timeout_s": {"type": "number", "default": 5.0},
|
||||
},
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
"type": "function",
|
||||
"function": {
|
||||
"name": "arms_go_neutral",
|
||||
"description": (
|
||||
"Riporta ENTRAMBE le braccia alla posa di home/riposo del robot "
|
||||
"(POST /arms/go_neutral sul bridge). Usa quando l'utente dice "
|
||||
"'torna a casa', 'home', 'posizione di riposo'. Default dry_run=true."
|
||||
),
|
||||
"parameters": {
|
||||
"type": "object",
|
||||
"properties": {"dry_run": {"type": "boolean", "default": True}},
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
"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
|
||||
@@ -1,9 +1,8 @@
|
||||
# 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.
|
||||
# before any of the route modules (which import `from ..api import _deps`)
|
||||
# start executing.
|
||||
from . import _deps # noqa: F401
|
||||
from . import (
|
||||
agent_routes,
|
||||
calibration_routes,
|
||||
config_routes,
|
||||
grasp_routes,
|
||||
@@ -16,7 +15,6 @@ from . import (
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"agent_routes",
|
||||
"calibration_routes",
|
||||
"config_routes",
|
||||
"grasp_routes",
|
||||
|
||||
@@ -1,203 +0,0 @@
|
||||
"""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)}
|
||||
@@ -12,6 +12,7 @@ from pydantic import BaseModel, Field
|
||||
from ..clients import G1Unavailable
|
||||
from ..config import get_config, set_config_value
|
||||
from ..geometry import normalize_quaternion
|
||||
from ..perception.pointcloud import nuwa_to_cv_frame
|
||||
from ..timing import Stopwatch
|
||||
from . import _deps
|
||||
|
||||
@@ -57,15 +58,14 @@ class GoReq(BaseModel):
|
||||
@router.get("/state")
|
||||
def get_state() -> dict[str, Any]:
|
||||
sw = Stopwatch()
|
||||
wrist_payload: dict[str, Any] | None = None
|
||||
joints_payload: dict[str, float] | None = None
|
||||
robot_error: str | None = None
|
||||
try:
|
||||
with sw.step("g1_wrist"):
|
||||
w = _deps.g1().wrist()
|
||||
except G1Unavailable as e:
|
||||
raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
|
||||
cfg = get_config().get("pose_3d", {}) or {}
|
||||
return {
|
||||
"robot_active": _deps.active_robot_key(),
|
||||
"wrist": {
|
||||
g1 = _deps.g1()
|
||||
w = g1.wrist()
|
||||
wrist_payload = {
|
||||
"left": {
|
||||
"position": list(w["left_wrist"]["position"]),
|
||||
"quaternion": list(w["left_wrist"]["quaternion"]),
|
||||
@@ -74,9 +74,31 @@ def get_state() -> dict[str, Any]:
|
||||
"position": list(w["right_wrist"]["position"]),
|
||||
"quaternion": list(w["right_wrist"]["quaternion"]),
|
||||
},
|
||||
},
|
||||
}
|
||||
# Tutti i giunti reali (mappa name → q in rad) per specchiare il robot
|
||||
# nell'URDF senza ricorrere all'IK.
|
||||
try:
|
||||
with sw.step("g1_diagnostics"):
|
||||
d = g1.diagnostics()
|
||||
motors = d.get("motors") or []
|
||||
joints_payload = {m["name"]: float(m["q"]) for m in motors
|
||||
if isinstance(m, dict) and "name" in m and "q" in m}
|
||||
except Exception:
|
||||
joints_payload = None
|
||||
except G1Unavailable as e:
|
||||
# Robot offline: la tab deve restare utilizzabile per editare config /
|
||||
# pose salvate e visualizzare il modello (camera marker compreso).
|
||||
robot_error = f"G1 unavailable: {e}"
|
||||
cfg = get_config().get("pose_3d", {}) or {}
|
||||
return {
|
||||
"robot_active": _deps.active_robot_key(),
|
||||
"wrist": wrist_payload,
|
||||
"joints": joints_payload,
|
||||
"robot_error": robot_error,
|
||||
"poses": cfg.get("named_poses") or {},
|
||||
"live_max_step_m": float(cfg.get("live_max_step_m", 0.15)),
|
||||
"right_wrist_camera": cfg.get("right_wrist_camera") or {},
|
||||
"clouds": cfg.get("clouds") or {},
|
||||
"timings_ms": sw.as_dict(),
|
||||
}
|
||||
|
||||
@@ -174,6 +196,51 @@ def delete_pose(name: str) -> dict[str, Any]:
|
||||
return {"deleted": name}
|
||||
|
||||
|
||||
@router.get("/clouds")
|
||||
def get_clouds(step: int = 8, max_depth_mm: int = 2000) -> dict[str, Any]:
|
||||
"""Acquisisce cloud da Nuwa-HP60C (eye-in-hand polso DX) e D435i (front
|
||||
camera fissa sul torso) in parallelo. Ritorna entrambi in **OpenCV optical
|
||||
frame** (X right, Y down, Z forward) come array `[N, 6]` con XYZ+RGB
|
||||
(RGB in 0-255). Il client li attacca ai link URDF `right_wrist_yaw_link`
|
||||
e `d435_link` rispettivamente, con offset/rpy configurabili.
|
||||
"""
|
||||
sw = Stopwatch()
|
||||
result: dict[str, Any] = {"timings_ms": {}}
|
||||
|
||||
def _pack(pts: "np.ndarray") -> list[list[float]]:
|
||||
# Accetta sia (N,3) che (N,6). In caso di (N,3) aggiunge grigio neutro.
|
||||
import numpy as np
|
||||
if pts.ndim != 2:
|
||||
return []
|
||||
if pts.shape[1] >= 6:
|
||||
out = pts[:, :6]
|
||||
else:
|
||||
n = pts.shape[0]
|
||||
rgb = np.full((n, 3), 180.0, dtype=pts.dtype)
|
||||
out = np.hstack([pts[:, :3], rgb])
|
||||
return out.astype(float).tolist()
|
||||
|
||||
# Nuwa: native frame (X right, Y up, Z back) → OpenCV optical via Y/Z flip
|
||||
try:
|
||||
with sw.step("nuwa_cloud"):
|
||||
pts = _deps.nuwa().pointcloud(step=step, max_depth_mm=max_depth_mm)
|
||||
pts_cv = nuwa_to_cv_frame(pts)
|
||||
result["nuwa"] = {"n": int(pts_cv.shape[0]), "points": _pack(pts_cv)}
|
||||
except Exception as e:
|
||||
result["nuwa"] = {"error": f"{type(e).__name__}: {e}"}
|
||||
|
||||
# D435i: stesso client NuwaClient ma backend RealSense, gia' in OpenCV
|
||||
try:
|
||||
with sw.step("d435_cloud"):
|
||||
pts = _deps.front_camera().pointcloud(step=step, max_depth_mm=max_depth_mm)
|
||||
result["d435"] = {"n": int(pts.shape[0]), "points": _pack(pts)}
|
||||
except Exception as e:
|
||||
result["d435"] = {"error": f"{type(e).__name__}: {e}"}
|
||||
|
||||
result["timings_ms"] = sw.as_dict()
|
||||
return result
|
||||
|
||||
|
||||
@router.post("/unlock")
|
||||
def post_unlock() -> dict[str, Any]:
|
||||
"""Proxy a POST {bridge}/unlock. Recovery rapido da arms: error_timeout."""
|
||||
|
||||
@@ -58,6 +58,13 @@ class G1Client:
|
||||
"""Hand finger state. Shape: {'left': {'fingers': {pinky,ring,middle,index,thumb_bend,thumb_rotation}, 'age_s'}, 'right': {...}}."""
|
||||
return self._get("/hand")
|
||||
|
||||
def diagnostics(self) -> dict[str, Any]:
|
||||
"""Full motor state: {ok, demo, motors: [{name, q, dq, ...}, ...]}.
|
||||
|
||||
`q` is the joint position in radians. Use for live URDF mirroring.
|
||||
"""
|
||||
return self._get("/diagnostics")
|
||||
|
||||
# ----- mutating -----
|
||||
|
||||
def wrist_absolute(self, side: str, position: list[float], quaternion: list[float],
|
||||
|
||||
@@ -10,7 +10,6 @@ from fastapi.responses import HTMLResponse, JSONResponse
|
||||
from fastapi.staticfiles import StaticFiles
|
||||
|
||||
from .api import (
|
||||
agent_routes,
|
||||
calibration_routes,
|
||||
config_routes,
|
||||
grasp_routes,
|
||||
@@ -39,7 +38,6 @@ app = FastAPI(
|
||||
|
||||
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"])
|
||||
|
||||
Reference in new issue
Block a user