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>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
commit
ac573505b7
121 files changed
+67199
No files matched your search
@@ -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
|
||||
@@ -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"
|
||||
@@ -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()
|
||||
@@ -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
|
||||
Reference in new issue
Block a user