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
+152744
-2701
No files matched your search
+64
-15
@@ -48,7 +48,10 @@ tab_right_arm:
|
||||
x: 1
|
||||
y: -1
|
||||
z: -1
|
||||
cam_to_wrist_perm: [z, x, y] # wrist_X<-cam_Z, wrist_Y<-cam_X, wrist_Z<-cam_Y
|
||||
cam_to_wrist_perm:
|
||||
- z
|
||||
- x
|
||||
- y
|
||||
camera_offset_wrist_mm:
|
||||
x: 0.0
|
||||
y: 0.0
|
||||
@@ -79,7 +82,66 @@ tab_right_arm:
|
||||
- 0
|
||||
pose_3d:
|
||||
live_max_step_m: 0.15
|
||||
named_poses: {}
|
||||
right_wrist_camera:
|
||||
enabled: true
|
||||
mesh_url: /static/assets/nuwa/nuwa-hp60c.stl
|
||||
offset_m:
|
||||
- 0.05
|
||||
- 0
|
||||
- 0.1
|
||||
rpy_deg:
|
||||
- 0
|
||||
- -90
|
||||
- -90
|
||||
clouds:
|
||||
nuwa:
|
||||
use_nuwa_camera_transform: true
|
||||
extra_offset_m:
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
extra_rpy_deg:
|
||||
- 0
|
||||
- 0
|
||||
- 0
|
||||
d435:
|
||||
offset_m:
|
||||
- -0.01
|
||||
- 0
|
||||
- 0
|
||||
rpy_deg:
|
||||
- -90
|
||||
- 86
|
||||
- 0
|
||||
transform_pelvis_4x4:
|
||||
- - -0.011344227963137242
|
||||
- -0.9974094742471121
|
||||
- -0.07103273311663486
|
||||
- 0.007340882385689307
|
||||
- - -0.5556320632728338
|
||||
- 0.06534842452974676
|
||||
- -0.8288561960163252
|
||||
- 0.3109738415126284
|
||||
- - 0.8313508998943191
|
||||
- 0.03006533042523672
|
||||
- -0.5549340115286931
|
||||
- 0.2869260834772309
|
||||
- - 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
named_poses:
|
||||
Base:
|
||||
side: left
|
||||
position:
|
||||
- 0.2561414814483791
|
||||
- 0.14648003458557432
|
||||
- 0.10874469031742239
|
||||
quaternion:
|
||||
- -0.005339006082777725
|
||||
- -0.047703617194205114
|
||||
- -0.005110044879619669
|
||||
- 0.9988341941293186
|
||||
grasp:
|
||||
approach_offset_m: 0.1
|
||||
pre_grasp_clearance_m: 0.05
|
||||
@@ -103,16 +165,3 @@ calibration:
|
||||
method: DANIILIDIS
|
||||
file: data/calibration.yaml
|
||||
samples_file: data/samples.json
|
||||
chat:
|
||||
openrouter:
|
||||
base_url: https://openrouter.ai/api/v1
|
||||
model: anthropic/claude-sonnet-4.5
|
||||
timeout_s: 60.0
|
||||
max_tool_iterations: 8
|
||||
temperature: 0.1
|
||||
api_key_env: OPENROUTER_API_KEY
|
||||
referer: http://localhost:8890
|
||||
title: presa-mano
|
||||
user_prompt_addendum: Misure sempre in mm interi. Sii conciso ma riporta i risultati
|
||||
di OGNI tool che hai chiamato (anche solo in una riga compatta), non saltare
|
||||
informazioni rilevanti.
|
||||
@@ -0,0 +1,144 @@
|
||||
# Bug report — D435i camera service: intrinsics mismatch nel back-projection 3D
|
||||
|
||||
**Servizio:** `front_camera` (RealSense D435i) — `http://192.168.123.99:8878`
|
||||
**Severità:** alta — punti 3D ritornati con XY sovrastimati di ~54%
|
||||
**Endpoint affetti:** almeno `/api/measure`; molto probabilmente anche `/api/pointcloud` e `/api/pointcloud/roi`
|
||||
**Endpoint NON affetti:** `/api/intrinsics` (espone i parametri corretti)
|
||||
**Data test:** 2026-05-27
|
||||
**Reporter:** sviluppo presa-mano
|
||||
|
||||
---
|
||||
|
||||
## 1. Sintomo
|
||||
|
||||
Le coordinate 3D `(X, Y, Z)` ritornate dal back-projection lato server sono **stirate** lateralmente di un fattore **~1.54** sugli assi X e Y, mentre Z (depth) è plausibile.
|
||||
|
||||
Conseguenza: oggetti di dimensioni note appaiono ~50% più grandi del reale in qualsiasi misurazione 3D, e i point cloud non si sovrappongono ad altre camere correttamente calibrate.
|
||||
|
||||
---
|
||||
|
||||
## 2. Test di verifica
|
||||
|
||||
Marker ArUco fisico di **lato 103.0 mm** (`id=20`, dictionary DICT_4X4_50) posizionato davanti al D435i a ~735 mm.
|
||||
|
||||
### 2.1 Intrinsics riportate dal servizio (`GET /api/intrinsics`)
|
||||
|
||||
```json
|
||||
{
|
||||
"ir": { "fx": 392.30963134765625, "fy": 392.30963134765625,
|
||||
"cx": 318.48004150390625, "cy": 242.43441772460938 },
|
||||
"rgb": { "fx": 603.88037109375, "fy": 603.6130981445312,
|
||||
"cx": 320.6182556152344, "cy": 250.35804748535156 },
|
||||
"extrinsics_ir_to_rgb": { "rotation": [...], "translation": [14.68, -0.07, 0.29] }
|
||||
}
|
||||
```
|
||||
|
||||
### 2.2 Detect del marker (su RGB image del D435i)
|
||||
|
||||
Marker-finder ritorna le 4 corner del tag nel sistema **pixel del frame RGB**:
|
||||
|
||||
```
|
||||
corners (RGB px): [(377, 68), (372, 152), (289, 147), (294, 63)]
|
||||
```
|
||||
|
||||
### 2.3 `GET /api/measure?u=&v=&radius=3` per ciascuna corner
|
||||
|
||||
```
|
||||
pixel (377, 68) → x=+0.1092 y=-0.3255 z=0.732 m
|
||||
pixel (372, 152) → x=+0.0999 y=-0.1687 z=0.732 m
|
||||
pixel (289, 147) → x=-0.0554 y=-0.1793 z=0.737 m
|
||||
pixel (294, 63) → x=-0.0461 y=-0.3375 z=0.738 m
|
||||
```
|
||||
|
||||
### 2.4 Misurazioni geometriche dai punti 3D ritornati
|
||||
|
||||
| Misura | Atteso | Ottenuto | Errore |
|
||||
|-----------------|---------------|---------------|----------|
|
||||
| Lato C0–C1 | 103.00 mm | 157.13 mm | **+52.6%** |
|
||||
| Lato C1–C2 | 103.00 mm | 155.71 mm | **+51.2%** |
|
||||
| Lato C2–C3 | 103.00 mm | 158.48 mm | **+53.9%** |
|
||||
| Lato C3–C0 | 103.00 mm | 155.92 mm | **+51.4%** |
|
||||
| Diagonale 0–2 | 145.66 mm | 220.19 mm | **+51.2%** |
|
||||
| Diagonale 1–3 | 145.66 mm | 223.32 mm | **+53.3%** |
|
||||
| Z medio (depth) | — | 735 mm | plausibile |
|
||||
|
||||
L'errore è **omogeneo ~52%** su tutte le coppie. Z (depth) è coerente — l'errore è confinato a X, Y.
|
||||
|
||||
---
|
||||
|
||||
## 3. Diagnosi (root cause)
|
||||
|
||||
Il rapporto fra le focali è esattamente:
|
||||
|
||||
```
|
||||
fx_rgb / fx_ir = 603.88037 / 392.30963 = 1.5393
|
||||
```
|
||||
|
||||
L'errore osservato sui lati è **1.524 ± 0.014**.
|
||||
|
||||
Verifica numerica della corner `(u=377, v=68)`, depth `z=0.732 m`:
|
||||
|
||||
```
|
||||
Con RGB intrinsics (corretto): X = (377 − 320.62) × 0.732 / 603.88 = 0.0683 m
|
||||
Con IR intrinsics (errato): X = (377 − 318.48) × 0.732 / 392.31 = 0.1091 m
|
||||
Server ritorna: = 0.1092 m ← coincide con IR
|
||||
```
|
||||
|
||||
**Conclusione:** il servizio sta usando le **intrinsics IR** (`fx=392.31, cx=318.48`) per back-projettare pixel che sono in **coordinate RGB** (allineamento depth-to-color attivo). Il risultato è X e Y inflati di `fx_rgb/fx_ir ≈ 1.54×`.
|
||||
|
||||
Il Z è corretto perché dipende solo dal valore di depth letto al pixel, non dalle intrinsics.
|
||||
|
||||
---
|
||||
|
||||
## 4. Fix consigliato
|
||||
|
||||
Nella routine di back-projection (probabilmente in `/api/measure`, `/api/pointcloud`, `/api/pointcloud/roi`):
|
||||
|
||||
```python
|
||||
# ERRATO (sospetto attuale):
|
||||
X = (u - cx_ir) * z / fx_ir
|
||||
Y = (v - cy_ir) * z / fy_ir
|
||||
|
||||
# CORRETTO (depth allineata al frame color → usa RGB intrinsics):
|
||||
X = (u - cx_rgb) * z / fx_rgb
|
||||
Y = (v - cy_rgb) * z / fy_rgb
|
||||
```
|
||||
|
||||
Se la pipeline NON sta facendo `align_to(color)` sul depth stream, allora il fix opposto: i pixel passati devono essere quelli del frame IR e si usano le intrinsics IR. Ma la semantica attuale (`/api/rgb` espone il frame color, `/api/measure` accetta `(u,v)` con range coerente col color frame ~640×480) suggerisce che l'API è progettata per RGB-aligned depth — quindi è il back-projection lato server che ha l'intrinsics sbagliata.
|
||||
|
||||
---
|
||||
|
||||
## 5. Test di non-regressione consigliati
|
||||
|
||||
Dopo il fix, verificare che:
|
||||
|
||||
1. **Tag di dimensione nota** (es. 103mm): le 4 corner back-proiettate formano un quadrato con lato `103.00 ± 5 mm` a 0.7m di distanza.
|
||||
2. **Coerenza con `/api/intrinsics`**: per ogni pixel `(u, v)` con depth `z`, vale:
|
||||
```
|
||||
x_returned ≈ (u - cx_rgb) * z / fx_rgb (entro tolleranza distorsione)
|
||||
y_returned ≈ (v - cy_rgb) * z / fy_rgb
|
||||
```
|
||||
3. **Cross-camera consistency**: il point cloud del D435i di un oggetto sovrapposto al cloud Nuwa (eye-in-hand polso DX) dello stesso oggetto deve avere errore residuo `< 30 mm` dopo hand-eye calibration.
|
||||
|
||||
---
|
||||
|
||||
## 6. Workaround temporaneo (lato client)
|
||||
|
||||
Nel viewport 3D di presa-mano è stato aggiunto un campo `D435i scale XY` con default **0.65** (`= fx_ir / fx_rgb`) che riscala il cloud per compensare il bug. Da rimuovere/portare a 1.0 quando il server è fixato.
|
||||
|
||||
---
|
||||
|
||||
## 7. Comandi per riprodurre
|
||||
|
||||
```bash
|
||||
# 1) Verifica intrinsics
|
||||
curl -sS http://192.168.123.99:8878/api/intrinsics
|
||||
|
||||
# 2) Detect marker (presuppone marker-finder a :8082 e tag 20 visibile)
|
||||
curl -sS -X POST http://localhost:8082/detect \
|
||||
-F "image_url=http://192.168.123.99:8878/api/rgb" \
|
||||
-F 'markers=[{"id":20,"size_mm":103}]'
|
||||
|
||||
# 3) Per ogni corner (u,v) ritornata, misura 3D
|
||||
curl -sS "http://192.168.123.99:8878/api/measure?u=377&v=68&radius=3"
|
||||
```
|
||||
@@ -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"])
|
||||
|
||||
@@ -30,17 +30,26 @@ def test_state_returns_both_wrists():
|
||||
assert body["wrist"]["right"]["position"] == [0.30, -0.20, 0.30]
|
||||
assert "poses" in body
|
||||
assert "live_max_step_m" in body
|
||||
assert "right_wrist_camera" in body
|
||||
assert body["robot_active"] in ("g1", "h1")
|
||||
|
||||
|
||||
def test_state_502_when_g1_unavailable():
|
||||
def test_state_returns_200_with_wrist_null_when_g1_unavailable():
|
||||
# La tab deve restare utilizzabile per editare config / pose salvate
|
||||
# e visualizzare il modello anche senza robot vivo.
|
||||
from presa_mano.clients import G1Unavailable
|
||||
with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
|
||||
g1 = MagicMock()
|
||||
g1.wrist.side_effect = G1Unavailable("connection refused")
|
||||
g1_factory.return_value = g1
|
||||
r = client.get("/api/pose_3d/state")
|
||||
assert r.status_code == 502
|
||||
assert r.status_code == 200
|
||||
body = r.json()
|
||||
assert body["wrist"] is None
|
||||
assert "G1 unavailable" in (body.get("robot_error") or "")
|
||||
# config esposta comunque
|
||||
assert "right_wrist_camera" in body
|
||||
assert "poses" in body
|
||||
|
||||
|
||||
def test_apply_dry_run_calls_g1_with_dry_run_true():
|
||||
|
||||
-732
@@ -642,392 +642,6 @@ $('unlockBtn').addEventListener('click', async () => {
|
||||
} catch (e) { $('eeLog').textContent = e.message; }
|
||||
});
|
||||
|
||||
// ==================== VISTA 3D ====================
|
||||
// Minimal canvas-2D 3D renderer: orthographic camera with yaw/pitch around a
|
||||
// target point, painter's-algorithm depth sorting, mouse drag/zoom/pan.
|
||||
|
||||
const view3dCanvas = $('view3dCanvas');
|
||||
const view3dCtx = view3dCanvas.getContext('2d');
|
||||
|
||||
const view3d = {
|
||||
// camera
|
||||
yaw: -0.6, // around base Z
|
||||
pitch: 0.5, // 0 = horizontal, π/2 = top-down
|
||||
scale: 350, // pixels per meter
|
||||
target: [0.30, 0.0, 0.20], // world point at canvas center
|
||||
// scene data
|
||||
wrist: null, // { left:{position,quaternion}, right:{...} }
|
||||
object: null, // { points:[[x,y,z],...], centroid:[x,y,z], n_points }
|
||||
plan: null, // { grasp:{position,quaternion_xyzw}, pre_grasp:{position} }
|
||||
calibrated: true, // false → object data unavailable
|
||||
lastErr: null,
|
||||
};
|
||||
|
||||
// ---- math helpers ----
|
||||
function quatToR(q) {
|
||||
const x=q[0], y=q[1], z=q[2], w=q[3];
|
||||
const n = x*x + y*y + z*z + w*w;
|
||||
if (n === 0) return [[1,0,0],[0,1,0],[0,0,1]];
|
||||
const s = 2/n;
|
||||
return [
|
||||
[1 - s*(y*y + z*z), s*(x*y - z*w), s*(x*z + y*w)],
|
||||
[s*(x*y + z*w), 1 - s*(x*x + z*z), s*(y*z - x*w)],
|
||||
[s*(x*z - y*w), s*(y*z + x*w), 1 - s*(x*x + y*y)],
|
||||
];
|
||||
}
|
||||
|
||||
// Project world point → {sx, sy, depth}. depth = camera-axis distance for sorting.
|
||||
function project(p) {
|
||||
const dx = p[0] - view3d.target[0];
|
||||
const dy = p[1] - view3d.target[1];
|
||||
const dz = p[2] - view3d.target[2];
|
||||
const cy = Math.cos(view3d.yaw), sy = Math.sin(view3d.yaw);
|
||||
// Yaw around Z
|
||||
const x1 = cy*dx + sy*dy;
|
||||
const y1 = -sy*dx + cy*dy;
|
||||
const z1 = dz;
|
||||
// Pitch around X (camera looks roughly toward +Y after yaw, tilting down by pitch)
|
||||
const cp = Math.cos(view3d.pitch), sp = Math.sin(view3d.pitch);
|
||||
const y2 = cp*y1 + sp*z1;
|
||||
const z2 = -sp*y1 + cp*z1;
|
||||
// Orthographic: screen X = +x1; screen Y = -z2 (so +Z up on screen)
|
||||
const sx = view3dCanvas.width / 2 + view3d.scale * x1;
|
||||
const sxy = view3dCanvas.height / 2 - view3d.scale * z2;
|
||||
// depth: y2 increases away from viewer → larger y2 = farther
|
||||
return { sx, sy: sxy, depth: y2 };
|
||||
}
|
||||
|
||||
function drawSegment(items, p1, p2, color, width = 2) {
|
||||
const a = project(p1), b = project(p2);
|
||||
items.push({ depth: (a.depth + b.depth) / 2, draw: () => {
|
||||
view3dCtx.strokeStyle = color;
|
||||
view3dCtx.lineWidth = width;
|
||||
view3dCtx.beginPath();
|
||||
view3dCtx.moveTo(a.sx, a.sy);
|
||||
view3dCtx.lineTo(b.sx, b.sy);
|
||||
view3dCtx.stroke();
|
||||
}});
|
||||
}
|
||||
|
||||
function drawDot(items, p, color, radius = 3) {
|
||||
const a = project(p);
|
||||
items.push({ depth: a.depth, draw: () => {
|
||||
view3dCtx.fillStyle = color;
|
||||
view3dCtx.beginPath();
|
||||
view3dCtx.arc(a.sx, a.sy, radius, 0, 2 * Math.PI);
|
||||
view3dCtx.fill();
|
||||
}});
|
||||
}
|
||||
|
||||
function drawLabel(items, p, text, color, dx = 8, dy = -8) {
|
||||
const a = project(p);
|
||||
items.push({ depth: a.depth - 1e-6, draw: () => {
|
||||
view3dCtx.fillStyle = color;
|
||||
view3dCtx.font = 'bold 11px ui-monospace, monospace';
|
||||
view3dCtx.fillText(text, a.sx + dx, a.sy + dy);
|
||||
}});
|
||||
}
|
||||
|
||||
function drawTriad(items, position, R, len = 0.06) {
|
||||
// Columns of R = axis directions in base frame.
|
||||
const axes = [
|
||||
{ col: 0, color: '#e74c3c' }, // X
|
||||
{ col: 1, color: '#3ecf8e' }, // Y
|
||||
{ col: 2, color: '#4da3ff' }, // Z
|
||||
];
|
||||
for (const a of axes) {
|
||||
const tip = [
|
||||
position[0] + R[0][a.col] * len,
|
||||
position[1] + R[1][a.col] * len,
|
||||
position[2] + R[2][a.col] * len,
|
||||
];
|
||||
drawSegment(items, position, tip, a.color, 2);
|
||||
}
|
||||
}
|
||||
|
||||
function drawGroundGrid(items) {
|
||||
if (!$('view3dShowGrid').checked) return;
|
||||
const half = 0.6, step = 0.05;
|
||||
const minor = '#1a1f27', major = '#2a2f37';
|
||||
for (let x = -half; x <= half + 1e-6; x += step) {
|
||||
const isMajor = Math.abs(x % 0.10) < 1e-6;
|
||||
drawSegment(items, [x, -half, 0], [x, half, 0], isMajor ? major : minor, 1);
|
||||
}
|
||||
for (let y = -half; y <= half + 1e-6; y += step) {
|
||||
const isMajor = Math.abs(y % 0.10) < 1e-6;
|
||||
drawSegment(items, [-half, y, 0], [half, y, 0], isMajor ? major : minor, 1);
|
||||
}
|
||||
}
|
||||
|
||||
function drawBaseAxes(items) {
|
||||
drawTriad(items, [0,0,0], [[1,0,0],[0,1,0],[0,0,1]], 0.12);
|
||||
drawLabel(items, [0.12, 0, 0], 'X', '#e74c3c');
|
||||
drawLabel(items, [0, 0.12, 0], 'Y', '#3ecf8e');
|
||||
drawLabel(items, [0, 0, 0.12], 'Z', '#4da3ff');
|
||||
drawLabel(items, [0,0,0], 'BASE', '#9aa0a6', 8, 14);
|
||||
}
|
||||
|
||||
function drawWrist(items, side, pose, color) {
|
||||
if (!pose || !pose.position || !pose.quaternion) return;
|
||||
const p = pose.position;
|
||||
const R = quatToR(pose.quaternion);
|
||||
drawTriad(items, p, R, 0.08);
|
||||
drawDot(items, p, color, 5);
|
||||
drawLabel(items, p, `polso ${side}`, color, 10, -6);
|
||||
}
|
||||
|
||||
function drawObject(items) {
|
||||
if (!view3d.object) return;
|
||||
if ($('view3dShowCloud').checked && view3d.object.points) {
|
||||
// Cap render to avoid huge per-frame work.
|
||||
const max = 1500;
|
||||
const pts = view3d.object.points;
|
||||
const stride = Math.max(1, Math.ceil(pts.length / max));
|
||||
for (let i = 0; i < pts.length; i += stride) {
|
||||
drawDot(items, pts[i], '#aaaaaa', 1.4);
|
||||
}
|
||||
}
|
||||
if (view3d.object.centroid) {
|
||||
drawDot(items, view3d.object.centroid, '#ffd54f', 6);
|
||||
drawLabel(items, view3d.object.centroid, 'oggetto', '#ffd54f', 10, -8);
|
||||
}
|
||||
}
|
||||
|
||||
function drawPlan(items) {
|
||||
if (!view3d.plan) return;
|
||||
const g = view3d.plan.grasp, pg = view3d.plan.pre_grasp;
|
||||
if (!g || !g.position) return;
|
||||
const R = quatToR(g.quaternion_xyzw);
|
||||
// grasp pose triad + marker
|
||||
drawTriad(items, g.position, R, 0.07);
|
||||
drawDot(items, g.position, '#ff5e7e', 6);
|
||||
drawLabel(items, g.position, 'grasp', '#ff5e7e', 10, -8);
|
||||
// pre-grasp marker + approach line
|
||||
if (pg && pg.position) {
|
||||
drawDot(items, pg.position, '#ffffff', 4);
|
||||
drawLabel(items, pg.position, 'pre-grasp', '#ffffff', 10, -8);
|
||||
drawSegment(items, pg.position, g.position, '#ffffff', 1.5);
|
||||
}
|
||||
}
|
||||
|
||||
function renderView3d() {
|
||||
const ctx = view3dCtx;
|
||||
ctx.fillStyle = '#0d1014';
|
||||
ctx.fillRect(0, 0, view3dCanvas.width, view3dCanvas.height);
|
||||
|
||||
const items = [];
|
||||
drawGroundGrid(items);
|
||||
drawBaseAxes(items);
|
||||
if (view3d.wrist) {
|
||||
drawWrist(items, 'right', view3d.wrist.right, '#f0ad4e');
|
||||
drawWrist(items, 'left', view3d.wrist.left, '#9b59b6');
|
||||
}
|
||||
drawObject(items);
|
||||
drawPlan(items);
|
||||
|
||||
// Painter's algorithm: far first.
|
||||
items.sort((a, b) => b.depth - a.depth);
|
||||
items.forEach(it => it.draw());
|
||||
|
||||
// HUD
|
||||
ctx.fillStyle = '#9aa0a6';
|
||||
ctx.font = '11px ui-monospace, monospace';
|
||||
ctx.fillText(`yaw=${(view3d.yaw*180/Math.PI).toFixed(0)}° pitch=${(view3d.pitch*180/Math.PI).toFixed(0)}° scale=${view3d.scale.toFixed(0)} px/m`, 8, 14);
|
||||
ctx.fillText(`target=[${view3d.target.map(v => v.toFixed(2)).join(', ')}]`, 8, 28);
|
||||
if (!view3d.calibrated) {
|
||||
ctx.fillStyle = '#f0ad4e';
|
||||
ctx.fillText('⚠ calibrazione mancante: punti oggetto non disponibili in frame base', 8, view3dCanvas.height - 8);
|
||||
}
|
||||
if (view3d.lastErr) {
|
||||
ctx.fillStyle = '#e74c3c';
|
||||
ctx.fillText(view3d.lastErr, 8, view3dCanvas.height - 22);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- mouse interaction ----
|
||||
let drag3d = null;
|
||||
view3dCanvas.addEventListener('mousedown', (ev) => {
|
||||
drag3d = { x: ev.clientX, y: ev.clientY, pan: ev.shiftKey };
|
||||
});
|
||||
window.addEventListener('mouseup', () => { drag3d = null; });
|
||||
window.addEventListener('mousemove', (ev) => {
|
||||
if (!drag3d) return;
|
||||
const dx = ev.clientX - drag3d.x;
|
||||
const dy = ev.clientY - drag3d.y;
|
||||
drag3d.x = ev.clientX; drag3d.y = ev.clientY;
|
||||
if (drag3d.pan) {
|
||||
// Pan in screen plane → world XY (approx, ignoring pitch foreshortening on Z)
|
||||
const cy = Math.cos(view3d.yaw), sy = Math.sin(view3d.yaw);
|
||||
const wx = cy*dx - sy*(-dy) * Math.cos(view3d.pitch);
|
||||
const wy = sy*dx + cy*(-dy) * Math.cos(view3d.pitch);
|
||||
view3d.target[0] -= wx / view3d.scale;
|
||||
view3d.target[1] -= wy / view3d.scale;
|
||||
// also let vertical drag move target Z when looking near horizontal
|
||||
view3d.target[2] += (-dy) * Math.sin(view3d.pitch) / view3d.scale;
|
||||
} else {
|
||||
view3d.yaw += dx * 0.01;
|
||||
view3d.pitch += dy * 0.01;
|
||||
view3d.pitch = Math.max(-Math.PI/2 + 0.05, Math.min(Math.PI/2 - 0.05, view3d.pitch));
|
||||
}
|
||||
renderView3d();
|
||||
});
|
||||
view3dCanvas.addEventListener('wheel', (ev) => {
|
||||
ev.preventDefault();
|
||||
const f = Math.exp(-ev.deltaY * 0.001);
|
||||
view3d.scale = Math.max(50, Math.min(3000, view3d.scale * f));
|
||||
renderView3d();
|
||||
}, { passive: false });
|
||||
view3dCanvas.addEventListener('dblclick', () => resetView3d());
|
||||
|
||||
function resetView3d() {
|
||||
view3d.yaw = -0.6;
|
||||
view3d.pitch = 0.5;
|
||||
view3d.scale = 350;
|
||||
view3d.target = [0.30, 0.0, 0.20];
|
||||
renderView3d();
|
||||
}
|
||||
$('view3dResetBtn').addEventListener('click', resetView3d);
|
||||
|
||||
function autoFitView3d() {
|
||||
if (!$('view3dAutoFit').checked) return;
|
||||
const pts = [];
|
||||
if (view3d.wrist) {
|
||||
if (view3d.wrist.right && view3d.wrist.right.position) pts.push(view3d.wrist.right.position);
|
||||
if (view3d.wrist.left && view3d.wrist.left.position) pts.push(view3d.wrist.left.position);
|
||||
}
|
||||
if (view3d.object && view3d.object.centroid) pts.push(view3d.object.centroid);
|
||||
if (view3d.plan && view3d.plan.grasp && view3d.plan.grasp.position) pts.push(view3d.plan.grasp.position);
|
||||
if (pts.length === 0) return;
|
||||
const min = [Infinity, Infinity, Infinity], max = [-Infinity, -Infinity, -Infinity];
|
||||
for (const p of pts) for (let i = 0; i < 3; i++) {
|
||||
if (p[i] < min[i]) min[i] = p[i];
|
||||
if (p[i] > max[i]) max[i] = p[i];
|
||||
}
|
||||
view3d.target = [(min[0]+max[0])/2, (min[1]+max[1])/2, (min[2]+max[2])/2];
|
||||
const span = Math.max(0.15, max[0]-min[0], max[1]-min[1], max[2]-min[2]);
|
||||
// fit ~70% of canvas width
|
||||
view3d.scale = Math.min(2000, (view3dCanvas.width * 0.7) / (span * 2.5));
|
||||
}
|
||||
|
||||
// ---- data fetching ----
|
||||
async function refreshView3d() {
|
||||
view3d.lastErr = null;
|
||||
const out = { wrist: null, object: null, plan: null, calibrated: true };
|
||||
const summary = [];
|
||||
// Wrist (always; doesn't need calibration — already in base frame).
|
||||
try {
|
||||
const w = await api('GET', '/api/wrist');
|
||||
// Server returns {left_wrist, right_wrist}; tolerate {left,right} too.
|
||||
const data = (w && w.data) || w || {};
|
||||
out.wrist = {
|
||||
left: data.left_wrist || data.left || null,
|
||||
right: data.right_wrist || data.right || null,
|
||||
};
|
||||
summary.push(`wrist: ${out.wrist.right ? 'R✓' : 'R—'} ${out.wrist.left ? 'L✓' : 'L—'}`);
|
||||
} catch (e) {
|
||||
summary.push(`wrist: err (${e.message})`);
|
||||
}
|
||||
// Object points + centroid in base frame.
|
||||
try {
|
||||
const body = {
|
||||
conf: +$('view3dConf').value || null,
|
||||
class_name: $('view3dClass').value || null,
|
||||
max_points: 3000,
|
||||
};
|
||||
const r = await api('POST', '/api/perception/object_points', body);
|
||||
const pts = r.points_base || null;
|
||||
if (!pts) {
|
||||
out.calibrated = false;
|
||||
summary.push('oggetto: serve calibrazione');
|
||||
} else {
|
||||
let cx=0, cy=0, cz=0;
|
||||
for (const p of pts) { cx+=p[0]; cy+=p[1]; cz+=p[2]; }
|
||||
const n = Math.max(1, pts.length);
|
||||
out.object = {
|
||||
points: pts,
|
||||
centroid: [cx/n, cy/n, cz/n],
|
||||
n_points: pts.length,
|
||||
detection: r.detection,
|
||||
};
|
||||
summary.push(`oggetto: ${pts.length} pt`);
|
||||
}
|
||||
} catch (e) {
|
||||
if (/^412\b/.test(e.message)) out.calibrated = false;
|
||||
summary.push(`oggetto: ${e.message}`);
|
||||
}
|
||||
// Grasp plan (optional).
|
||||
if ($('view3dShowPlan').checked && out.calibrated && out.object) {
|
||||
try {
|
||||
const body = {
|
||||
conf: +$('view3dConf').value || null,
|
||||
class_name: $('view3dClass').value || null,
|
||||
};
|
||||
const r = await api('POST', '/api/grasp/plan', body);
|
||||
out.plan = r.plan || null;
|
||||
if (out.plan && out.plan.grasp) {
|
||||
const p = out.plan.grasp.position;
|
||||
summary.push(`grasp: [${p.map(v => v.toFixed(2)).join(', ')}]`);
|
||||
}
|
||||
} catch (e) {
|
||||
summary.push(`plan: ${e.message}`);
|
||||
}
|
||||
}
|
||||
Object.assign(view3d, out);
|
||||
autoFitView3d();
|
||||
renderView3d();
|
||||
$('view3dStatusLine').textContent = summary.join(' · ');
|
||||
$('view3dStatusLine').style.color = view3d.lastErr ? 'var(--danger)' : 'var(--dim)';
|
||||
$('view3dOut').textContent = JSON.stringify({
|
||||
wrist: out.wrist,
|
||||
object: out.object && {
|
||||
n_points: out.object.n_points,
|
||||
centroid: out.object.centroid,
|
||||
detection: out.object.detection,
|
||||
},
|
||||
plan: out.plan,
|
||||
}, null, 2);
|
||||
}
|
||||
|
||||
$('view3dRefreshBtn').addEventListener('click', async () => {
|
||||
setStatus('Aggiorno scena 3D…');
|
||||
try { await refreshView3d(); setStatus('Scena 3D aggiornata', 'ok'); }
|
||||
catch (e) { setStatus(e.message, 'err'); }
|
||||
});
|
||||
|
||||
// loop
|
||||
let view3dTimer = null;
|
||||
let view3dInFlight = false;
|
||||
async function view3dTick() {
|
||||
if (view3dInFlight) return;
|
||||
view3dInFlight = true;
|
||||
try { await refreshView3d(); } catch {} finally { view3dInFlight = false; }
|
||||
}
|
||||
function startView3dLoop() {
|
||||
if (view3dTimer) return;
|
||||
const rate = Math.max(200, +$('view3dRateMs').value || 800);
|
||||
view3dTick();
|
||||
view3dTimer = setInterval(view3dTick, rate);
|
||||
const b = $('view3dLoopBtn');
|
||||
b.textContent = '■ Stop loop';
|
||||
b.classList.add('running');
|
||||
}
|
||||
function stopView3dLoop() {
|
||||
if (view3dTimer) { clearInterval(view3dTimer); view3dTimer = null; }
|
||||
const b = $('view3dLoopBtn');
|
||||
b.textContent = '▶ Loop';
|
||||
b.classList.remove('running');
|
||||
}
|
||||
$('view3dLoopBtn').addEventListener('click', () => view3dTimer ? stopView3dLoop() : startView3dLoop());
|
||||
|
||||
// Pause loop when leaving the tab (mirrors the grasp-tab behavior).
|
||||
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
|
||||
if (btn.dataset.tab !== 'view3d' && view3dTimer) stopView3dLoop();
|
||||
if (btn.dataset.tab === 'view3d') renderView3d();
|
||||
}));
|
||||
|
||||
// Initial empty render so the canvas isn't blank when first shown.
|
||||
renderView3d();
|
||||
|
||||
// ==================== HOME ====================
|
||||
// Shows reachability + latency for every upstream microservice and lets the
|
||||
// user switch the active robot (G1 ↔ H1). The orchestrator runs the actual
|
||||
@@ -1136,349 +750,3 @@ startHomePoll();
|
||||
} catch {}
|
||||
refreshWrist();
|
||||
})();
|
||||
|
||||
// ==================== ASSISTENTE AI ====================
|
||||
// Curated tool-use chat: l'utente parla in linguaggio naturale, il backend
|
||||
// passa la richiesta a OpenRouter con i tool definiti in agent/tools.py.
|
||||
// La cronologia è tenuta lato browser e ribaltata indietro a ogni POST.
|
||||
const agentState = {
|
||||
history: [], // OpenAI-format messages (escluso system, ricostruito server-side)
|
||||
visualMessages: [], // {role, content, traceIdx?} per il render
|
||||
busy: false,
|
||||
health: null,
|
||||
};
|
||||
|
||||
function agentEscape(s) {
|
||||
return String(s == null ? '' : s)
|
||||
.replace(/&/g, '&').replace(/</g, '<').replace(/>/g, '>');
|
||||
}
|
||||
|
||||
function renderAgentChat() {
|
||||
const host = $('agentChat');
|
||||
host.innerHTML = '';
|
||||
if (agentState.visualMessages.length === 0) {
|
||||
const sys = document.createElement('div');
|
||||
sys.className = 'agent-msg system';
|
||||
sys.textContent = 'Conversazione vuota. Scrivi una richiesta sotto.';
|
||||
host.appendChild(sys);
|
||||
return;
|
||||
}
|
||||
for (const m of agentState.visualMessages) {
|
||||
const div = document.createElement('div');
|
||||
div.className = `agent-msg ${m.role}`;
|
||||
div.textContent = m.content || '';
|
||||
if (m.tools && m.tools.length) {
|
||||
const meta = document.createElement('div');
|
||||
meta.className = 'meta';
|
||||
for (const t of m.tools) {
|
||||
const pill = document.createElement('span');
|
||||
pill.className = 'agent-tool-pill' + (t.ok ? '' : ' err');
|
||||
pill.textContent = `${t.name} · ${t.latency_ms}ms`;
|
||||
pill.title = JSON.stringify(t.args || {}, null, 2);
|
||||
meta.appendChild(pill);
|
||||
}
|
||||
div.appendChild(meta);
|
||||
}
|
||||
host.appendChild(div);
|
||||
}
|
||||
host.scrollTop = host.scrollHeight;
|
||||
}
|
||||
|
||||
async function refreshAgentHealth() {
|
||||
try {
|
||||
const h = await api('GET', '/api/agent/health');
|
||||
agentState.health = h;
|
||||
const el = $('agentHealth');
|
||||
if (h.configured) {
|
||||
el.textContent = `pronto · modello ${h.model}`;
|
||||
el.style.color = 'var(--ok)';
|
||||
$('agentSendBtn').disabled = false;
|
||||
} else {
|
||||
el.textContent = `API key mancante (env ${h.api_key_env}): chat disabilitata`;
|
||||
el.style.color = 'var(--danger)';
|
||||
$('agentSendBtn').disabled = true;
|
||||
}
|
||||
} catch (e) {
|
||||
$('agentHealth').textContent = `errore: ${e.message}`;
|
||||
$('agentHealth').style.color = 'var(--danger)';
|
||||
}
|
||||
// mirror del robot attivo nella pill della tab
|
||||
try {
|
||||
const r = await api('GET', '/api/home/robot');
|
||||
const pill = $('agentActiveRobotPill');
|
||||
if (pill) pill.textContent = `robot: ${(r.active || '—').toUpperCase()}`;
|
||||
} catch {}
|
||||
}
|
||||
|
||||
async function sendAgentMessage() {
|
||||
if (agentState.busy) return;
|
||||
const input = $('agentInput');
|
||||
const message = input.value.trim();
|
||||
if (!message) return;
|
||||
agentState.busy = true;
|
||||
$('agentSendBtn').disabled = true;
|
||||
$('agentStatus').textContent = 'invio…';
|
||||
$('agentStatus').style.color = 'var(--dim)';
|
||||
|
||||
agentState.visualMessages.push({ role: 'user', content: message });
|
||||
renderAgentChat();
|
||||
input.value = '';
|
||||
|
||||
try {
|
||||
const r = await api('POST', '/api/agent/chat', {
|
||||
message,
|
||||
history: agentState.history,
|
||||
});
|
||||
agentState.history = r.history || [];
|
||||
const tools = (r.tool_calls || []).map(tc => ({
|
||||
name: tc.name, ok: tc.result && tc.result.ok !== false,
|
||||
latency_ms: tc.latency_ms, args: tc.arguments,
|
||||
}));
|
||||
agentState.visualMessages.push({
|
||||
role: 'assistant', content: r.reply || '(nessuna risposta)',
|
||||
tools,
|
||||
});
|
||||
renderAgentChat();
|
||||
// Tool trace dettagliata nel pannello di destra.
|
||||
if (r.tool_calls && r.tool_calls.length) {
|
||||
$('agentTrace').textContent = JSON.stringify(r.tool_calls, null, 2);
|
||||
} else {
|
||||
$('agentTrace').textContent = '(nessuna chiamata)';
|
||||
}
|
||||
const usage = r.usage || {};
|
||||
const u = usage.total_tokens != null ? `· ${usage.total_tokens} tokens` : '';
|
||||
$('agentStatus').textContent = `ok · ${r.iterations} iterazioni · ${tools.length} tool ${u}`;
|
||||
$('agentStatus').style.color = 'var(--ok)';
|
||||
// se la risposta ha cambiato robot attivo, aggiorna pill
|
||||
refreshAgentHealth().catch(() => {});
|
||||
} catch (e) {
|
||||
agentState.visualMessages.push({
|
||||
role: 'error', content: e.message,
|
||||
});
|
||||
renderAgentChat();
|
||||
$('agentStatus').textContent = `errore: ${e.message}`;
|
||||
$('agentStatus').style.color = 'var(--danger)';
|
||||
} finally {
|
||||
agentState.busy = false;
|
||||
$('agentSendBtn').disabled = !(agentState.health && agentState.health.configured);
|
||||
}
|
||||
}
|
||||
|
||||
$('agentSendBtn').addEventListener('click', sendAgentMessage);
|
||||
$('agentInput').addEventListener('keydown', (e) => {
|
||||
if (e.ctrlKey && e.key === 'Enter') {
|
||||
e.preventDefault();
|
||||
sendAgentMessage();
|
||||
}
|
||||
});
|
||||
|
||||
$('agentClearBtn').addEventListener('click', () => {
|
||||
if (agentState.visualMessages.length === 0 && agentState.history.length === 0) return;
|
||||
if (!confirm('Resetta la conversazione corrente?')) return;
|
||||
agentState.history = [];
|
||||
agentState.visualMessages = [];
|
||||
renderAgentChat();
|
||||
$('agentTrace').textContent = '(nessuna chiamata)';
|
||||
$('agentStatus').textContent = 'conversazione svuotata';
|
||||
});
|
||||
|
||||
// ----- Task salvati -----
|
||||
async function loadAgentTasks() {
|
||||
const ul = $('agentTaskList');
|
||||
try {
|
||||
const r = await api('GET', '/api/agent/tasks');
|
||||
const items = r.tasks || [];
|
||||
ul.innerHTML = '';
|
||||
if (items.length === 0) {
|
||||
ul.innerHTML = '<li class="dim">(nessun task salvato)</li>';
|
||||
return;
|
||||
}
|
||||
for (const it of items) {
|
||||
const li = document.createElement('li');
|
||||
const name = document.createElement('span');
|
||||
name.className = 'task-name';
|
||||
name.textContent = it.name;
|
||||
const desc = document.createElement('span');
|
||||
desc.className = 'task-desc';
|
||||
desc.textContent = it.description || it.prompt;
|
||||
desc.title = it.prompt;
|
||||
const useBtn = document.createElement('button');
|
||||
useBtn.className = 'link-btn';
|
||||
useBtn.textContent = '↥ usa';
|
||||
useBtn.title = 'Carica questo prompt nella casella di input';
|
||||
useBtn.addEventListener('click', () => {
|
||||
$('agentInput').value = it.prompt;
|
||||
$('agentInput').focus();
|
||||
});
|
||||
const runBtn = document.createElement('button');
|
||||
runBtn.className = 'link-btn';
|
||||
runBtn.textContent = '▶ esegui';
|
||||
runBtn.addEventListener('click', () => {
|
||||
$('agentInput').value = it.prompt;
|
||||
sendAgentMessage();
|
||||
});
|
||||
const delBtn = document.createElement('button');
|
||||
delBtn.className = 'link-btn';
|
||||
delBtn.textContent = '✕';
|
||||
delBtn.title = 'Elimina';
|
||||
delBtn.addEventListener('click', async () => {
|
||||
if (!confirm(`Eliminare task "${it.name}"?`)) return;
|
||||
try {
|
||||
await api('DELETE', `/api/agent/tasks/${encodeURIComponent(it.name)}`);
|
||||
loadAgentTasks();
|
||||
} catch (e) { toast('Eliminazione fallita', e.message, false); }
|
||||
});
|
||||
li.appendChild(name);
|
||||
li.appendChild(desc);
|
||||
li.appendChild(useBtn);
|
||||
li.appendChild(runBtn);
|
||||
li.appendChild(delBtn);
|
||||
ul.appendChild(li);
|
||||
}
|
||||
} catch (e) {
|
||||
ul.innerHTML = `<li class="err">errore: ${agentEscape(e.message)}</li>`;
|
||||
}
|
||||
}
|
||||
|
||||
$('agentTaskSaveBtn').addEventListener('click', async (ev) => {
|
||||
const name = $('agentTaskName').value.trim();
|
||||
const prompt = $('agentInput').value.trim();
|
||||
const description = $('agentTaskDesc').value.trim() || null;
|
||||
if (!name) { toast('Manca il nome', 'Indica un nome per il task', false); return; }
|
||||
if (!prompt) { toast('Prompt vuoto', 'Scrivi prima il prompt nella casella', false); return; }
|
||||
try {
|
||||
await api('POST', '/api/agent/tasks', { name, prompt, description });
|
||||
flashBtn(ev.currentTarget, true);
|
||||
toast('Task salvato', name, true);
|
||||
$('agentTaskName').value = '';
|
||||
$('agentTaskDesc').value = '';
|
||||
loadAgentTasks();
|
||||
} catch (e) {
|
||||
flashBtn(ev.currentTarget, false);
|
||||
toast('Salvataggio fallito', e.message, false);
|
||||
}
|
||||
});
|
||||
|
||||
$('agentTaskReloadBtn').addEventListener('click', loadAgentTasks);
|
||||
|
||||
// Carica health + task quando si apre la tab Assistente AI.
|
||||
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
|
||||
if (btn.dataset.tab === 'agent') {
|
||||
refreshAgentHealth();
|
||||
loadAgentTasks();
|
||||
}
|
||||
}));
|
||||
|
||||
// Bootstrap iniziale (anche se la tab non è attiva, così la pill si popola).
|
||||
refreshAgentHealth();
|
||||
loadAgentTasks();
|
||||
|
||||
// ==================== IMPOSTAZIONI AI ====================
|
||||
async function loadAiSettings() {
|
||||
const select = $('aiModelSelect');
|
||||
// Lista modelli (statica con override custom): si carica una sola volta.
|
||||
if (select && select.options.length <= 1) {
|
||||
try {
|
||||
const r = await api('GET', '/api/agent/known_models');
|
||||
select.innerHTML = '';
|
||||
for (const m of r.models) {
|
||||
const opt = document.createElement('option');
|
||||
opt.value = m.id;
|
||||
opt.textContent = `${m.id} — ${m.label}`;
|
||||
select.appendChild(opt);
|
||||
}
|
||||
} catch (e) {
|
||||
select.innerHTML = `<option value="">errore: ${e.message}</option>`;
|
||||
}
|
||||
}
|
||||
// Carica config corrente
|
||||
try {
|
||||
const cfg = await api('GET', '/api/config');
|
||||
const o = (cfg.chat && cfg.chat.openrouter) || {};
|
||||
const model = o.model || 'anthropic/claude-sonnet-4.5';
|
||||
if ([...$('aiModelSelect').options].some(opt => opt.value === model)) {
|
||||
$('aiModelSelect').value = model;
|
||||
$('aiModelCustom').value = '';
|
||||
} else {
|
||||
$('aiModelCustom').value = model;
|
||||
}
|
||||
$('aiTemperature').value = (o.temperature != null) ? o.temperature : 0.1;
|
||||
$('aiMaxIterations').value = o.max_tool_iterations || 8;
|
||||
$('aiTimeout').value = o.timeout_s || 60;
|
||||
$('aiPromptAddendum').value = o.user_prompt_addendum || '';
|
||||
$('aiSettingsStatus').textContent = 'caricato da config';
|
||||
$('aiSettingsStatus').style.color = 'var(--dim)';
|
||||
} catch (e) {
|
||||
$('aiSettingsStatus').textContent = `errore: ${e.message}`;
|
||||
$('aiSettingsStatus').style.color = 'var(--danger)';
|
||||
}
|
||||
}
|
||||
|
||||
async function saveAiSettings(btn) {
|
||||
const customModel = $('aiModelCustom').value.trim();
|
||||
const model = customModel || $('aiModelSelect').value;
|
||||
if (!model) { toast('Modello mancante', 'Seleziona o scrivi un modello', false); return; }
|
||||
const temperature = parseFloat($('aiTemperature').value);
|
||||
const max_tool_iterations = parseInt($('aiMaxIterations').value, 10);
|
||||
const timeout_s = parseFloat($('aiTimeout').value);
|
||||
if (Number.isNaN(temperature) || temperature < 0 || temperature > 2) {
|
||||
toast('Temperature non valido', '0..2', false); return;
|
||||
}
|
||||
if (Number.isNaN(max_tool_iterations) || max_tool_iterations < 1) {
|
||||
toast('Max iterations non valido', '>=1', false); return;
|
||||
}
|
||||
const patch = {
|
||||
chat: { openrouter: {
|
||||
model,
|
||||
temperature,
|
||||
max_tool_iterations,
|
||||
timeout_s,
|
||||
user_prompt_addendum: $('aiPromptAddendum').value,
|
||||
}},
|
||||
};
|
||||
try {
|
||||
await api('PUT', '/api/config', patch);
|
||||
flashBtn(btn, true);
|
||||
toast('Salvato', `model=${model}, T=${temperature}`, true);
|
||||
$('aiSettingsStatus').textContent = `salvato · model=${model} · T=${temperature}`;
|
||||
$('aiSettingsStatus').style.color = 'var(--ok)';
|
||||
refreshAgentHealth().catch(() => {});
|
||||
} catch (e) {
|
||||
flashBtn(btn, false);
|
||||
toast('Errore salvataggio', e.message, false);
|
||||
$('aiSettingsStatus').textContent = e.message;
|
||||
$('aiSettingsStatus').style.color = 'var(--danger)';
|
||||
}
|
||||
}
|
||||
|
||||
async function previewSystemPrompt() {
|
||||
const pre = $('aiPromptPreview');
|
||||
pre.textContent = 'caricamento…';
|
||||
try {
|
||||
const r = await api('GET', '/api/agent/system_prompt_preview');
|
||||
pre.textContent = r.system_prompt || '(vuoto)';
|
||||
} catch (e) {
|
||||
pre.textContent = `errore: ${e.message}`;
|
||||
}
|
||||
}
|
||||
|
||||
$('aiSaveBtn').addEventListener('click', (ev) => saveAiSettings(ev.currentTarget));
|
||||
$('aiReloadBtn').addEventListener('click', loadAiSettings);
|
||||
$('aiResetTempBtn').addEventListener('click', () => { $('aiTemperature').value = 0.1; });
|
||||
$('aiPromptPreviewBtn').addEventListener('click', previewSystemPrompt);
|
||||
$('aiPromptCopyBtn').addEventListener('click', async () => {
|
||||
try {
|
||||
await navigator.clipboard.writeText($('aiPromptPreview').textContent);
|
||||
toast('Copiato', 'Prompt nel clipboard', true);
|
||||
} catch (e) { toast('Copia fallita', e.message, false); }
|
||||
});
|
||||
// Quando l'utente seleziona un modello dalla combo, svuota il custom (priorità chiara).
|
||||
$('aiModelSelect').addEventListener('change', () => { $('aiModelCustom').value = ''; });
|
||||
|
||||
// Carica i valori quando si apre la tab.
|
||||
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
|
||||
if (btn.dataset.tab === 'ai-settings') {
|
||||
loadAiSettings();
|
||||
previewSystemPrompt();
|
||||
}
|
||||
}));
|
||||
File diff suppressed because it is too large.
Load diff
Binary file not shown.
@@ -1,479 +0,0 @@
|
||||
/* G1 3D diagnostic viewer.
|
||||
*
|
||||
* Loads the URDF, builds the scene graph with two-level groups per joint
|
||||
* (outer = <origin>, inner = motion driven by diagnostic q), renders meshes
|
||||
* tinted by per-motor temperature and colored spheres at joint origins.
|
||||
* Exposes a tiny global API consumed by app.js powerPollOnce:
|
||||
* window.g1Diag3d.update(diagData)
|
||||
*
|
||||
* Deliberately avoids depending on any URDF library — the URDF subset we
|
||||
* need (fixed/revolute/continuous/prismatic joints, single mesh per visual,
|
||||
* relative mesh paths) is small enough to parse in ~50 lines.
|
||||
*/
|
||||
|
||||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
|
||||
import { STLLoader } from 'three/addons/loaders/STLLoader.js';
|
||||
|
||||
const URDF_URL = '/static/assets/g1/g1_body29_hand14.urdf';
|
||||
const MESH_BASE = '/static/assets/g1/';
|
||||
|
||||
const WARN_C_DEFAULT = 75;
|
||||
const CRIT_C_DEFAULT = 85;
|
||||
|
||||
function parseOrigin(el) {
|
||||
if (!el) return { xyz: [0, 0, 0], rpy: [0, 0, 0] };
|
||||
const parse = (s) => s ? s.trim().split(/\s+/).map(Number) : [0, 0, 0];
|
||||
return {
|
||||
xyz: parse(el.getAttribute('xyz')),
|
||||
rpy: parse(el.getAttribute('rpy')),
|
||||
};
|
||||
}
|
||||
function parseAxis(el) {
|
||||
if (!el) return [1, 0, 0];
|
||||
const s = el.getAttribute('xyz');
|
||||
return s ? s.trim().split(/\s+/).map(Number) : [1, 0, 0];
|
||||
}
|
||||
|
||||
async function parseURDF(xmlText) {
|
||||
const doc = new DOMParser().parseFromString(xmlText, 'application/xml');
|
||||
const links = {};
|
||||
for (const el of doc.querySelectorAll('link')) {
|
||||
const name = el.getAttribute('name');
|
||||
if (!name) continue;
|
||||
const v = el.querySelector('visual');
|
||||
let mesh = null, visualOrigin = { xyz: [0,0,0], rpy: [0,0,0] };
|
||||
if (v) {
|
||||
visualOrigin = parseOrigin(v.querySelector('origin'));
|
||||
const me = v.querySelector('geometry mesh');
|
||||
if (me) mesh = me.getAttribute('filename');
|
||||
}
|
||||
links[name] = { name, mesh, visualOrigin };
|
||||
}
|
||||
const joints = [];
|
||||
for (const el of doc.querySelectorAll('joint')) {
|
||||
const parent = el.querySelector('parent');
|
||||
const child = el.querySelector('child');
|
||||
if (!parent || !child) continue;
|
||||
joints.push({
|
||||
name: el.getAttribute('name'),
|
||||
type: el.getAttribute('type') || 'fixed',
|
||||
parent: parent.getAttribute('link'),
|
||||
child: child.getAttribute('link'),
|
||||
origin: parseOrigin(el.querySelector('origin')),
|
||||
axis: parseAxis(el.querySelector('axis')),
|
||||
});
|
||||
}
|
||||
return { links, joints };
|
||||
}
|
||||
|
||||
function applyTransform(obj, o) {
|
||||
obj.position.set(o.xyz[0], o.xyz[1], o.xyz[2]);
|
||||
obj.rotation.order = 'ZYX'; // URDF RPY: intrinsic rotations yaw * pitch * roll
|
||||
obj.rotation.set(o.rpy[0], o.rpy[1], o.rpy[2]);
|
||||
}
|
||||
|
||||
function tempToColor(t, warn, crit) {
|
||||
if (!Number.isFinite(t)) return 0x888888;
|
||||
if (t >= crit) return 0xe74c3c;
|
||||
if (t >= warn) return 0xf0ad4e;
|
||||
// green->yellow interpolation below warn
|
||||
const u = Math.max(0, Math.min(1, (t - 30) / (warn - 30)));
|
||||
const r = Math.round(0x3e + u * (0xf0 - 0x3e));
|
||||
const g = Math.round(0xcf - u * (0xcf - 0xad));
|
||||
const b = Math.round(0x8e - u * (0x8e - 0x4e));
|
||||
return (r << 16) | (g << 8) | b;
|
||||
}
|
||||
|
||||
class G1Viewer {
|
||||
constructor(canvas) {
|
||||
this.canvas = canvas;
|
||||
this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
this.renderer.setPixelRatio(window.devicePixelRatio || 1);
|
||||
this.scene = new THREE.Scene();
|
||||
this.scene.background = new THREE.Color(0x0d1014);
|
||||
|
||||
// URDF is Z-up. Keep Z-up throughout for coherence with the robot base frame.
|
||||
this.camera = new THREE.PerspectiveCamera(45, canvas.clientWidth / canvas.clientHeight, 0.05, 20);
|
||||
this.camera.up.set(0, 0, 1);
|
||||
this.camera.position.set(1.8, -1.8, 1.2);
|
||||
this.camera.lookAt(0, 0, 0.4);
|
||||
|
||||
this.controls = new OrbitControls(this.camera, canvas);
|
||||
this.controls.target.set(0, 0, 0.4);
|
||||
this.controls.update();
|
||||
|
||||
const hemi = new THREE.HemisphereLight(0xffffff, 0x222233, 0.6);
|
||||
this.scene.add(hemi);
|
||||
const dir = new THREE.DirectionalLight(0xffffff, 0.8);
|
||||
dir.position.set(2, 2, 4);
|
||||
this.scene.add(dir);
|
||||
const dir2 = new THREE.DirectionalLight(0xffffff, 0.3);
|
||||
dir2.position.set(-2, -1, 2);
|
||||
this.scene.add(dir2);
|
||||
|
||||
this.grid = new THREE.GridHelper(2, 20, 0x444444, 0x222233);
|
||||
this.grid.rotation.x = Math.PI / 2; // grid is XY plane in Z-up
|
||||
this.scene.add(this.grid);
|
||||
|
||||
this.robotRoot = new THREE.Group();
|
||||
this.scene.add(this.robotRoot);
|
||||
|
||||
this.linkGroups = {};
|
||||
this.linkMeshes = {};
|
||||
this.jointGroups = {}; // static <origin> holders
|
||||
this.jointMotions = {}; // inner motion-driven holders
|
||||
this.jointIndicators = {}; // sphere at joint origin
|
||||
this.motorByJointName = {}; // joint_name -> motor dict (from /diagnostics)
|
||||
this.warnC = WARN_C_DEFAULT;
|
||||
this.critC = CRIT_C_DEFAULT;
|
||||
this.showMeshTint = true;
|
||||
this.showSpheres = true;
|
||||
this.pinnedJoint = null;
|
||||
|
||||
this.raycaster = new THREE.Raycaster();
|
||||
this.mouseNDC = new THREE.Vector2();
|
||||
|
||||
this._onResize = () => this.resize();
|
||||
window.addEventListener('resize', this._onResize);
|
||||
this.canvas.addEventListener('pointermove', (ev) => this._onHover(ev));
|
||||
this.canvas.addEventListener('click', (ev) => this._onClick(ev));
|
||||
|
||||
this._animate = this._animate.bind(this);
|
||||
this.resize();
|
||||
this._animate();
|
||||
}
|
||||
|
||||
resize() {
|
||||
const w = this.canvas.clientWidth || 720;
|
||||
const h = this.canvas.clientHeight || 560;
|
||||
this.renderer.setSize(w, h, false);
|
||||
this.camera.aspect = w / h;
|
||||
this.camera.updateProjectionMatrix();
|
||||
}
|
||||
|
||||
_animate() {
|
||||
this._raf = requestAnimationFrame(this._animate);
|
||||
this.controls.update();
|
||||
this.renderer.render(this.scene, this.camera);
|
||||
}
|
||||
|
||||
async load() {
|
||||
const res = await fetch(URDF_URL);
|
||||
if (!res.ok) throw new Error(`URDF fetch ${res.status}`);
|
||||
const xml = await res.text();
|
||||
const urdf = await parseURDF(xml);
|
||||
this._urdf = urdf;
|
||||
this._build(urdf);
|
||||
await this._loadMeshes(urdf);
|
||||
this._placeFloor();
|
||||
}
|
||||
|
||||
_build(urdf) {
|
||||
const childSet = new Set(urdf.joints.map(j => j.child));
|
||||
const rootName = Object.keys(urdf.links).find(n => !childSet.has(n));
|
||||
const buildLink = (name) => {
|
||||
const link = urdf.links[name];
|
||||
const g = new THREE.Group();
|
||||
g.name = `link:${name}`;
|
||||
g.userData.linkName = name;
|
||||
this.linkGroups[name] = g;
|
||||
// child joints
|
||||
for (const j of urdf.joints.filter(x => x.parent === name)) {
|
||||
const origin = new THREE.Group();
|
||||
applyTransform(origin, j.origin);
|
||||
origin.userData.joint = j;
|
||||
this.jointGroups[j.name] = origin;
|
||||
const motion = new THREE.Group();
|
||||
origin.add(motion);
|
||||
this.jointMotions[j.name] = motion;
|
||||
// sphere indicator at joint origin (in origin's local frame → origin(0,0,0))
|
||||
const sphere = new THREE.Mesh(
|
||||
new THREE.SphereGeometry(0.012, 16, 12),
|
||||
new THREE.MeshBasicMaterial({ color: 0x888888, depthTest: false, transparent: true, opacity: 0.9 }),
|
||||
);
|
||||
sphere.renderOrder = 999;
|
||||
sphere.userData.jointName = j.name;
|
||||
origin.add(sphere);
|
||||
this.jointIndicators[j.name] = sphere;
|
||||
motion.add(buildLink(j.child));
|
||||
g.add(origin);
|
||||
}
|
||||
return g;
|
||||
};
|
||||
this.robotRoot.add(buildLink(rootName));
|
||||
}
|
||||
|
||||
async _loadMeshes(urdf) {
|
||||
const loader = new STLLoader();
|
||||
const tasks = [];
|
||||
for (const name of Object.keys(urdf.links)) {
|
||||
const link = urdf.links[name];
|
||||
if (!link.mesh) continue;
|
||||
const url = MESH_BASE + link.mesh;
|
||||
tasks.push(new Promise((resolve) => {
|
||||
loader.load(url, (geom) => {
|
||||
geom.computeVertexNormals();
|
||||
const mat = new THREE.MeshStandardMaterial({
|
||||
color: 0x888888, metalness: 0.1, roughness: 0.6,
|
||||
flatShading: false,
|
||||
});
|
||||
const mesh = new THREE.Mesh(geom, mat);
|
||||
mesh.userData.linkName = name;
|
||||
applyTransform(mesh, link.visualOrigin);
|
||||
this.linkMeshes[name] = mesh;
|
||||
this.linkGroups[name].add(mesh);
|
||||
resolve();
|
||||
}, undefined, (err) => {
|
||||
console.warn('STL load failed:', url, err);
|
||||
resolve();
|
||||
});
|
||||
}));
|
||||
}
|
||||
await Promise.all(tasks);
|
||||
}
|
||||
|
||||
_placeFloor() {
|
||||
// position grid at model's feet (min z of bounding box).
|
||||
const box = new THREE.Box3().setFromObject(this.robotRoot);
|
||||
if (isFinite(box.min.z)) this.grid.position.z = box.min.z;
|
||||
}
|
||||
|
||||
setShowGrid(v) { this.grid.visible = v; }
|
||||
setShowSpheres(v) {
|
||||
this.showSpheres = v;
|
||||
for (const s of Object.values(this.jointIndicators)) s.visible = v;
|
||||
}
|
||||
setShowMeshTint(v) {
|
||||
this.showMeshTint = v;
|
||||
this._applyColors();
|
||||
}
|
||||
|
||||
fitView() {
|
||||
const box = new THREE.Box3().setFromObject(this.robotRoot);
|
||||
if (!isFinite(box.min.z)) return;
|
||||
const c = new THREE.Vector3();
|
||||
box.getCenter(c);
|
||||
const s = new THREE.Vector3();
|
||||
box.getSize(s);
|
||||
const d = Math.max(s.x, s.y, s.z) * 1.8;
|
||||
this.controls.target.copy(c);
|
||||
const dir = new THREE.Vector3(1, -1, 0.4).normalize();
|
||||
this.camera.position.copy(c).addScaledVector(dir, d);
|
||||
this.controls.update();
|
||||
}
|
||||
resetView() {
|
||||
this.controls.target.set(0, 0, 0.4);
|
||||
this.camera.position.set(1.8, -1.8, 1.2);
|
||||
this.controls.update();
|
||||
}
|
||||
|
||||
update(diagData) {
|
||||
if (!diagData || !Array.isArray(diagData.motors)) return;
|
||||
this.warnC = Number(diagData.warn_temp_c ?? WARN_C_DEFAULT);
|
||||
this.critC = Number(diagData.crit_temp_c ?? CRIT_C_DEFAULT);
|
||||
this.motorByJointName = {};
|
||||
for (const m of diagData.motors) {
|
||||
const jn = m.name + '_joint';
|
||||
this.motorByJointName[jn] = m;
|
||||
// Apply joint angle.
|
||||
const jg = this.jointMotions[jn];
|
||||
if (jg) {
|
||||
const joint = this.jointGroups[jn].userData.joint;
|
||||
const ax = new THREE.Vector3(...joint.axis).normalize();
|
||||
if (joint.type === 'revolute' || joint.type === 'continuous') {
|
||||
jg.quaternion.setFromAxisAngle(ax, Number(m.q) || 0);
|
||||
} else if (joint.type === 'prismatic') {
|
||||
jg.position.copy(ax.multiplyScalar(Number(m.q) || 0));
|
||||
}
|
||||
}
|
||||
}
|
||||
this._applyColors();
|
||||
if (this.pinnedJoint) this._renderPinned();
|
||||
this._renderSummary(diagData);
|
||||
}
|
||||
|
||||
_applyColors() {
|
||||
for (const [jointName, motor] of Object.entries(this.motorByJointName)) {
|
||||
const joint = this.jointGroups[jointName];
|
||||
if (!joint) continue;
|
||||
const temp = motor.temperature_motor;
|
||||
const faulted = !!motor.faulted;
|
||||
const enabled = motor.enabled !== false;
|
||||
let color = tempToColor(temp, this.warnC, this.critC);
|
||||
if (!enabled) color = 0x333333;
|
||||
// sphere
|
||||
const sph = this.jointIndicators[jointName];
|
||||
if (sph) {
|
||||
sph.material.color.setHex(color);
|
||||
if (faulted) sph.material.color.setHex(0xe74c3c);
|
||||
}
|
||||
// mesh tint on child link
|
||||
const childLink = joint.userData.joint.child;
|
||||
const mesh = this.linkMeshes[childLink];
|
||||
if (mesh) {
|
||||
if (this.showMeshTint) {
|
||||
mesh.material.color.setHex(color);
|
||||
mesh.material.emissive.setHex(faulted ? 0x330000 : 0x000000);
|
||||
mesh.material.emissiveIntensity = faulted ? 0.8 : 0;
|
||||
} else {
|
||||
mesh.material.color.setHex(0x888888);
|
||||
mesh.material.emissiveIntensity = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_pickJointAt(clientX, clientY) {
|
||||
const rect = this.canvas.getBoundingClientRect();
|
||||
this.mouseNDC.x = ((clientX - rect.left) / rect.width) * 2 - 1;
|
||||
this.mouseNDC.y = -((clientY - rect.top) / rect.height) * 2 + 1;
|
||||
this.raycaster.setFromCamera(this.mouseNDC, this.camera);
|
||||
// Prefer hits on joint spheres first (small but always on top).
|
||||
const sphHits = this.raycaster.intersectObjects(Object.values(this.jointIndicators), false);
|
||||
if (sphHits.length) return sphHits[0].object.userData.jointName;
|
||||
// Fall back to mesh hits (walk up the tree to find the joint whose motion group is the ancestor).
|
||||
const meshHits = this.raycaster.intersectObjects(Object.values(this.linkMeshes), false);
|
||||
if (!meshHits.length) return null;
|
||||
const linkName = meshHits[0].object.userData.linkName;
|
||||
// Find the joint whose child == linkName.
|
||||
const joint = this._urdf.joints.find(j => j.child === linkName && this.motorByJointName[j.name]);
|
||||
return joint ? joint.name : null;
|
||||
}
|
||||
|
||||
_onHover(ev) {
|
||||
const tt = document.getElementById('diag3dTooltip');
|
||||
const joint = this._pickJointAt(ev.clientX, ev.clientY);
|
||||
if (!joint) { tt.classList.add('hidden'); return; }
|
||||
const m = this.motorByJointName[joint];
|
||||
if (!m) { tt.classList.add('hidden'); return; }
|
||||
const cls = m.faulted ? 'err' : (m.temperature_motor >= this.critC ? 'err' :
|
||||
m.temperature_motor >= this.warnC ? 'warn' : '');
|
||||
tt.className = 'diag3d-tooltip ' + cls;
|
||||
tt.innerHTML = this._tooltipHTML(m);
|
||||
const rect = this.canvas.getBoundingClientRect();
|
||||
tt.style.left = (ev.clientX - rect.left + 14) + 'px';
|
||||
tt.style.top = (ev.clientY - rect.top + 14) + 'px';
|
||||
}
|
||||
|
||||
_onClick(ev) {
|
||||
const joint = this._pickJointAt(ev.clientX, ev.clientY);
|
||||
if (!joint) return;
|
||||
this.pinnedJoint = joint;
|
||||
this._renderPinned();
|
||||
}
|
||||
|
||||
_tooltipHTML(m) {
|
||||
const rows = [
|
||||
['T motore', `${m.temperature_motor}°C`],
|
||||
['T ctrl', `${m.temperature_ctrl}°C`],
|
||||
['q', Number(m.q).toFixed(3) + ' rad'],
|
||||
['dq', Number(m.dq).toFixed(3) + ' rad/s'],
|
||||
['tau_est', Number(m.tau_est).toFixed(2) + ' Nm'],
|
||||
['V', Number(m.voltage).toFixed(2) + ' V'],
|
||||
['mode', m.mode + (m.enabled === false ? ' (OFF)' : '')],
|
||||
['fault', m.faulted ? ('0x' + Number(m.motorstate).toString(16)) : 'no'],
|
||||
];
|
||||
const body = rows.map(([k, v]) => `<div class="tt-row"><span class="k">${k}</span><span>${v}</span></div>`).join('');
|
||||
return `<div class="tt-title">#${m.index} · ${m.name}</div>${body}`;
|
||||
}
|
||||
|
||||
_renderPinned() {
|
||||
const host = document.getElementById('diag3dPinned');
|
||||
if (!host) return;
|
||||
if (!this.pinnedJoint) return;
|
||||
const m = this.motorByJointName[this.pinnedJoint];
|
||||
if (!m) {
|
||||
host.innerHTML = `<div class="dim">giunto ${this.pinnedJoint}: nessun motore associato</div>`;
|
||||
return;
|
||||
}
|
||||
const statusCls = m.faulted ? 'err' :
|
||||
m.temperature_motor >= this.critC ? 'err' :
|
||||
m.temperature_motor >= this.warnC ? 'warn' : '';
|
||||
const rows = [
|
||||
['nome', m.name],
|
||||
['indice', m.index],
|
||||
['T motore (°C)', m.temperature_motor],
|
||||
['T ctrl (°C)', m.temperature_ctrl],
|
||||
['q (rad)', Number(m.q).toFixed(4)],
|
||||
['dq (rad/s)', Number(m.dq).toFixed(4)],
|
||||
['tau_est (Nm)', Number(m.tau_est).toFixed(3)],
|
||||
['voltaggio (V)', Number(m.voltage).toFixed(3)],
|
||||
['mode', m.mode],
|
||||
['enabled', m.enabled ? 'sì' : 'no'],
|
||||
['motorstate raw', '0x' + Number(m.motorstate).toString(16)],
|
||||
['faulted', m.faulted ? 'SÌ' : 'no'],
|
||||
['derating', m.derating ? 'sì' : 'no'],
|
||||
];
|
||||
host.innerHTML =
|
||||
`<div><strong>#${m.index} · ${m.name}</strong> <span class="dim">(giunto: ${this.pinnedJoint})</span></div>` +
|
||||
`<table><tbody>${rows.map(([k,v]) =>
|
||||
`<tr class="${statusCls}"><td>${k}</td><td>${v}</td></tr>`).join('')}</tbody></table>`;
|
||||
}
|
||||
|
||||
_renderSummary(d) {
|
||||
const host = document.getElementById('diag3dSummary');
|
||||
if (!host) return;
|
||||
const s = d.summary || {};
|
||||
const fmt = (arr) => (arr && arr.length) ? arr.join(', ') : '—';
|
||||
const rows = [
|
||||
['lowstate', d.stale ? 'STALE' : (d.tick != null ? `tick ${d.tick}` : 'ok')],
|
||||
['max temp (°C)', s.max_motor_temp_c != null ? `${s.max_motor_temp_c} @ ${s.max_motor_temp_joint}` : '—'],
|
||||
['warn / crit (°C)', `${d.warn_temp_c} / ${d.crit_temp_c}`],
|
||||
['derating', fmt(s.derating_joints)],
|
||||
['critical', fmt(s.critical_joints)],
|
||||
['faulted', fmt(s.faulted_joints)],
|
||||
['disabled', fmt(s.disabled_joints)],
|
||||
['min V', s.min_voltage != null ? Number(s.min_voltage).toFixed(2) + ' V' : '—'],
|
||||
];
|
||||
host.innerHTML = `<table><tbody>${rows.map(([k,v]) =>
|
||||
`<tr><td>${k}</td><td>${v}</td></tr>`).join('')}</tbody></table>`;
|
||||
}
|
||||
}
|
||||
|
||||
// ---- Boot and expose global hook. ----
|
||||
|
||||
const canvas = document.getElementById('diag3dCanvas');
|
||||
const statusEl = document.getElementById('diag3dStatus');
|
||||
let viewer = null;
|
||||
|
||||
async function boot() {
|
||||
if (!canvas) return;
|
||||
try {
|
||||
statusEl.textContent = 'caricamento URDF + mesh…';
|
||||
viewer = new G1Viewer(canvas);
|
||||
await viewer.load();
|
||||
statusEl.textContent = 'URDF caricato · in attesa di /diagnostics…';
|
||||
// Wire up toolbar controls.
|
||||
const by = (id) => document.getElementById(id);
|
||||
by('diag3dMeshTint').addEventListener('change', (e) => viewer.setShowMeshTint(e.target.checked));
|
||||
by('diag3dSpheres').addEventListener('change', (e) => viewer.setShowSpheres(e.target.checked));
|
||||
by('diag3dGrid').addEventListener('change', (e) => viewer.setShowGrid(e.target.checked));
|
||||
by('diag3dResetBtn').addEventListener('click', () => viewer.resetView());
|
||||
by('diag3dFitBtn').addEventListener('click', () => viewer.fitView());
|
||||
// Re-size when the diagnostic tab becomes active (canvas may have 0 size while hidden).
|
||||
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
|
||||
if (btn.dataset.tab === 'diagnostic') setTimeout(() => viewer && viewer.resize(), 50);
|
||||
}));
|
||||
// Global hook for app.js to push diagnostic updates.
|
||||
window.g1Diag3d = {
|
||||
update: (d) => {
|
||||
try { viewer.update(d); statusEl.textContent =
|
||||
d.stale ? 'lowstate STALE' :
|
||||
d.available === false ? (d.reason || '/diagnostics non disponibile') :
|
||||
`motori aggiornati · tick ${d.tick ?? '?'}`; }
|
||||
catch (e) { console.warn('g1Diag3d.update:', e); }
|
||||
},
|
||||
};
|
||||
} catch (e) {
|
||||
console.error('diag3d boot failed:', e);
|
||||
statusEl.textContent = 'errore caricamento: ' + e.message;
|
||||
}
|
||||
}
|
||||
|
||||
// Defer boot until the tab is first opened — avoids eager CDN fetch and STL load
|
||||
// for users who never visit this tab.
|
||||
let booted = false;
|
||||
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
|
||||
if (btn.dataset.tab === 'diagnostic' && !booted) { booted = true; boot(); }
|
||||
}));
|
||||
+109
-162
@@ -12,11 +12,8 @@
|
||||
<h1>presa-mano</h1>
|
||||
<nav class="tabs">
|
||||
<button class="tab active" data-tab="home">Home</button>
|
||||
<button class="tab" data-tab="agent">Assistente AI</button>
|
||||
<button class="tab" data-tab="ai-settings">Impostazioni AI</button>
|
||||
<button class="tab" data-tab="workplane">Calibrazione Piano</button>
|
||||
<button class="tab" data-tab="grasp">Cerca modello</button>
|
||||
<button class="tab" data-tab="view3d">Vista 3D</button>
|
||||
<button class="tab" data-tab="right-arm">Braccio DX</button>
|
||||
<button class="tab" data-tab="pose-3d">Pose 3D</button>
|
||||
<button class="tab" data-tab="ee">Movimento EE</button>
|
||||
@@ -68,115 +65,6 @@
|
||||
<pre id="homeLog" class="log small">—</pre>
|
||||
</section>
|
||||
|
||||
<!-- ============================== ASSISTENTE AI ============================== -->
|
||||
<section class="panel" id="tab-agent">
|
||||
<div class="posture-toolbar">
|
||||
<span class="post-health" id="agentHealth">—</span>
|
||||
<span class="active-robot-pill" id="agentActiveRobotPill" title="Robot attivo">robot: —</span>
|
||||
<button id="agentClearBtn" title="Resetta la conversazione">Nuova conversazione</button>
|
||||
</div>
|
||||
|
||||
<div class="grid-2 agent-layout">
|
||||
<!-- LEFT: chat history + input -->
|
||||
<div>
|
||||
<h2>Chat</h2>
|
||||
<div id="agentChat" class="agent-chat">
|
||||
<div class="agent-msg system">Ciao. Dimmi cosa vuoi fare con il robot. Es: "muovi il braccio destro 200 mm in X+", "calibra il braccio destro", "mostra lo stato dei dispositivi".</div>
|
||||
</div>
|
||||
<div class="agent-input-row">
|
||||
<textarea id="agentInput" rows="3" placeholder="Scrivi un comando o una richiesta in linguaggio naturale… (Ctrl+Invio per inviare)"></textarea>
|
||||
<button id="agentSendBtn" class="primary">Invia ▸</button>
|
||||
</div>
|
||||
<div class="marker-info" id="agentStatus">—</div>
|
||||
</div>
|
||||
|
||||
<!-- RIGHT: saved tasks + tool trace -->
|
||||
<div>
|
||||
<h2>Task salvati</h2>
|
||||
<div class="info-box">
|
||||
Salva un prompt usato spesso (es. "calibra braccio dx") per riutilizzarlo con un click. Il prompt corrente nella textarea viene salvato.
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>Nome <input type="text" id="agentTaskName" placeholder="es. calibrazione_dx" maxlength="80"></label>
|
||||
<label>Descrizione <input type="text" id="agentTaskDesc" placeholder="(opzionale)" maxlength="400"></label>
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="agentTaskSaveBtn" class="primary">💾 Salva prompt corrente</button>
|
||||
<button id="agentTaskReloadBtn">⟳ Ricarica</button>
|
||||
</div>
|
||||
<ul id="agentTaskList" class="agent-task-list">
|
||||
<li class="dim">caricamento…</li>
|
||||
</ul>
|
||||
|
||||
<h2>Tool trace ultima risposta</h2>
|
||||
<pre id="agentTrace" class="log small">(nessuna chiamata)</pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== IMPOSTAZIONI AI ============================== -->
|
||||
<section class="panel" id="tab-ai-settings">
|
||||
<div class="info-box">
|
||||
Parametri del client OpenRouter usato dall'Assistente AI. Le modifiche vengono persistite in <code>config.yaml</code> via <code>PUT /api/config</code> e applicate alla chat successiva (non c'è bisogno di restart). Il <strong>prompt aggiuntivo</strong> viene appeso al prompt di sistema dinamico — non sostituisce le regole base (frame, tool, sicurezza).
|
||||
</div>
|
||||
|
||||
<div class="grid-2">
|
||||
<div>
|
||||
<h2>Modello e parametri</h2>
|
||||
<div class="ctrl-block">
|
||||
<div class="kv compact">
|
||||
<label>Modello (combo)
|
||||
<select id="aiModelSelect">
|
||||
<option value="">caricamento…</option>
|
||||
</select>
|
||||
</label>
|
||||
<label>Modello (custom — sovrascrive la combo)
|
||||
<input type="text" id="aiModelCustom" placeholder="es. anthropic/claude-sonnet-4.5" />
|
||||
</label>
|
||||
<label title="0 = deterministico, 1 = creativo. Per orchestrazione robotica tieni basso (0.0–0.3).">
|
||||
Temperature
|
||||
<input type="number" id="aiTemperature" min="0" max="2" step="0.05" />
|
||||
</label>
|
||||
<label title="Massimo numero di iterazioni di tool calling per singolo messaggio">
|
||||
Max tool iterations
|
||||
<input type="number" id="aiMaxIterations" min="1" max="20" step="1" />
|
||||
</label>
|
||||
<label title="Timeout HTTP per la singola chiamata a OpenRouter (secondi)">
|
||||
Timeout (s)
|
||||
<input type="number" id="aiTimeout" min="5" max="300" step="5" />
|
||||
</label>
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="aiSaveBtn" class="primary">💾 Salva impostazioni</button>
|
||||
<button id="aiReloadBtn">⟳ Ricarica da config</button>
|
||||
<button id="aiResetTempBtn" title="Riporta temperature a 0.1 (default safe)">Reset temp</button>
|
||||
</div>
|
||||
<div class="marker-info" id="aiSettingsStatus">—</div>
|
||||
</div>
|
||||
|
||||
<h2>Prompt aggiuntivo</h2>
|
||||
<div class="ctrl-block">
|
||||
<div class="info-box">
|
||||
Testo che viene appeso al system prompt dinamico (sezione "Istruzioni aggiuntive dell'operatore"). Usa per personalizzare lo stile, aggiungere contesto specifico al tuo setup, o regole custom. Lascia vuoto per non aggiungere nulla.
|
||||
</div>
|
||||
<textarea id="aiPromptAddendum" rows="10" style="width:100%;font-family:ui-monospace,monospace;font-size:12px;padding:8px;background:#0e1116;color:inherit;border:1px solid var(--border);border-radius:4px"></textarea>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div>
|
||||
<h2>Anteprima system prompt corrente</h2>
|
||||
<div class="info-box">
|
||||
Quello che il modello riceve davvero al prossimo messaggio (include lo stato corrente: robot attivo, marker, ecc., più l'eventuale addendum sopra dopo che lo salvi).
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="aiPromptPreviewBtn">⟳ Aggiorna anteprima</button>
|
||||
<button id="aiPromptCopyBtn">📋 Copia</button>
|
||||
</div>
|
||||
<pre id="aiPromptPreview" class="log small" style="max-height:560px;overflow:auto">(premi 'Aggiorna anteprima')</pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== CALIBRAZIONE PIANO ============================== -->
|
||||
<section class="panel" id="tab-workplane">
|
||||
<div class="diag3d-toolbar">
|
||||
@@ -292,50 +180,6 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== VISTA 3D ============================== -->
|
||||
<section class="panel" id="tab-view3d">
|
||||
<div class="grid-2">
|
||||
<div>
|
||||
<h2>Scena 3D — frame base robot</h2>
|
||||
<div class="canvas-wrap">
|
||||
<canvas id="view3dCanvas" width="720" height="540"></canvas>
|
||||
</div>
|
||||
<div class="marker-info" id="view3dHint">drag = ruota · wheel = zoom · doppio click = reset vista · shift+drag = pan</div>
|
||||
<div class="marker-info" id="view3dStatusLine">—</div>
|
||||
</div>
|
||||
<div>
|
||||
<h2>Dati</h2>
|
||||
<div class="kv">
|
||||
<label>Classe (filtro) <input type="text" id="view3dClass" placeholder="(tutte)" /></label>
|
||||
<label>Confidenza <input type="number" id="view3dConf" value="0.5" step="0.05" min="0" max="1" /></label>
|
||||
<label class="inline"><input type="checkbox" id="view3dShowCloud" checked /> Punti oggetto</label>
|
||||
<label class="inline"><input type="checkbox" id="view3dShowGrid" checked /> Griglia tavolo</label>
|
||||
<label class="inline"><input type="checkbox" id="view3dShowPlan" checked /> Pianifica grasp</label>
|
||||
<label class="inline"><input type="checkbox" id="view3dAutoFit" checked /> Auto-fit vista</label>
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="view3dRefreshBtn" class="primary">Aggiorna scena</button>
|
||||
<button id="view3dLoopBtn">▶ Loop</button>
|
||||
<label class="inline" style="margin-left:8px"><input type="number" id="view3dRateMs" value="800" min="200" step="50" style="width:80px"> ms</label>
|
||||
<button id="view3dResetBtn">Reset vista</button>
|
||||
</div>
|
||||
<div class="legend">
|
||||
<div class="legend-row"><span class="dot" style="background:#e74c3c"></span> X (rosso) — base</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#3ecf8e"></span> Y (verde) — base</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#4da3ff"></span> Z (blu) — base</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#f0ad4e"></span> Polso destro</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#9b59b6"></span> Polso sinistro</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#aaaaaa"></span> Punti oggetto</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#ffd54f"></span> Centroide oggetto</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#ff5e7e"></span> Punto di presa</div>
|
||||
<div class="legend-row"><span class="dot" style="background:#ffffff;border:1px solid #888"></span> Pre-grasp + traiettoria</div>
|
||||
</div>
|
||||
<h3>Stato</h3>
|
||||
<pre id="view3dOut" class="log">(nessun dato — premi "Aggiorna scena")</pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== BRACCIO DX ============================== -->
|
||||
<section class="panel" id="tab-right-arm">
|
||||
<div class="ra-grid">
|
||||
@@ -519,15 +363,118 @@
|
||||
</div>
|
||||
|
||||
<div class="p3-section">
|
||||
<h3>Pose salvate</h3>
|
||||
<h3>Pose salvate — DX</h3>
|
||||
<div class="p3-row">
|
||||
<select id="p3PoseSelect"><option value="">(seleziona)</option></select>
|
||||
<select id="p3PoseSelectR"><option value="">(seleziona)</option></select>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<button id="p3PoseGoBtn">Vai</button>
|
||||
<input type="text" id="p3PoseName" placeholder="nome" maxlength="40">
|
||||
<button id="p3PoseSaveBtn">Salva</button>
|
||||
<button id="p3PoseDelBtn">✕</button>
|
||||
<button id="p3PoseGoBtnR">Vai</button>
|
||||
<input type="text" id="p3PoseNameR" placeholder="nome" maxlength="40">
|
||||
<button id="p3PoseSaveBtnR">Salva</button>
|
||||
<button id="p3PoseDelBtnR">✕</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="p3-section">
|
||||
<h3>Pose salvate — SX</h3>
|
||||
<div class="p3-row">
|
||||
<select id="p3PoseSelectL"><option value="">(seleziona)</option></select>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<button id="p3PoseGoBtnL">Vai</button>
|
||||
<input type="text" id="p3PoseNameL" placeholder="nome" maxlength="40">
|
||||
<button id="p3PoseSaveBtnL">Salva</button>
|
||||
<button id="p3PoseDelBtnL">✕</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="p3-section">
|
||||
<h3>Camera Nuwa (polso DX)</h3>
|
||||
<div class="p3-row">
|
||||
<label><input type="checkbox" id="p3NuwaEnabled"> Abilitata</label>
|
||||
</div>
|
||||
<div class="p3-readout">
|
||||
<label>dX <input id="p3NuwaX" type="number" step="1" /> mm</label>
|
||||
<label>dY <input id="p3NuwaY" type="number" step="1" /> mm</label>
|
||||
<label>dZ <input id="p3NuwaZ" type="number" step="1" /> mm</label>
|
||||
<label>R <input id="p3NuwaR" type="number" step="1" /> °</label>
|
||||
<label>P <input id="p3NuwaP" type="number" step="1" /> °</label>
|
||||
<label>W <input id="p3NuwaW" type="number" step="1" /> °</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<button id="p3NuwaApplyBtn">Applica</button>
|
||||
<button id="p3NuwaResetBtn">Default</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="p3-section">
|
||||
<h3>Camera D435i (offset URDF)</h3>
|
||||
<div class="p3-readout">
|
||||
<label>dX <input id="p3D435X" type="number" step="1" /> mm</label>
|
||||
<label>dY <input id="p3D435Y" type="number" step="1" /> mm</label>
|
||||
<label>dZ <input id="p3D435Z" type="number" step="1" /> mm</label>
|
||||
<label>R <input id="p3D435R" type="number" step="1" /> °</label>
|
||||
<label>P <input id="p3D435P" type="number" step="1" /> °</label>
|
||||
<label>W <input id="p3D435W" type="number" step="1" /> °</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<button id="p3D435ApplyBtn">Applica</button>
|
||||
<button id="p3D435ResetBtn">Default</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="p3-section">
|
||||
<h3>Cloud cross-camera</h3>
|
||||
<div class="p3-row">
|
||||
<label><input type="checkbox" id="p3ShowNuwaCloud"> Nuwa</label>
|
||||
<label><input type="checkbox" id="p3FlipNuwaX">↔X</label>
|
||||
<label><input type="checkbox" id="p3FlipNuwaY">↔Y</label>
|
||||
<label><input type="checkbox" id="p3FlipNuwaZ">↔Z</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<label><input type="checkbox" id="p3ShowD435Cloud"> D435i</label>
|
||||
<label><input type="checkbox" id="p3FlipD435X">↔X</label>
|
||||
<label><input type="checkbox" id="p3FlipD435Y">↔Y</label>
|
||||
<label><input type="checkbox" id="p3FlipD435Z">↔Z</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<label><input type="checkbox" id="p3D435UseHandeye"> D435i: usa hand-eye (vs URDF)</label>
|
||||
</div>
|
||||
<div class="p3-row" title="Workaround per bug intrinsics-mismatch del servizio D435i (fx_ir/fx_rgb ≈ 0.65). Scala solo X e Y del cloud.">
|
||||
<label>D435i scale XY
|
||||
<input id="p3D435ScaleXY" type="number" min="0.1" max="3" step="0.01" value="0.65" style="width:60px">
|
||||
</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<label>step <input id="p3CloudStep" type="number" min="1" max="32" value="8" style="width:50px"></label>
|
||||
<label>max <input id="p3CloudMaxDepth" type="number" min="100" max="5000" step="100" value="2000" style="width:70px"> mm</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<label>size Nuwa <input id="p3NuwaCloudSize" type="number" min="0.5" max="100" step="0.5" value="5" style="width:55px"> mm</label>
|
||||
<label>size D435i <input id="p3D435CloudSize" type="number" min="0.5" max="100" step="0.5" value="5" style="width:55px"> mm</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<button id="p3CloudAcquireBtn">Acquisisci</button>
|
||||
<button id="p3CloudClearBtn">✕</button>
|
||||
</div>
|
||||
<div class="marker-info" id="p3CloudStatus">—</div>
|
||||
</div>
|
||||
|
||||
<div class="p3-section">
|
||||
<h3>Debug</h3>
|
||||
<div class="p3-row">
|
||||
<label><input type="checkbox" id="p3ShowJointUCS"> Mostra UCS giunti</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<label>Scala
|
||||
<input id="p3JointUCSSize" type="number" min="10" max="200" step="5" value="40" style="width:60px"> mm
|
||||
</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<label><input type="checkbox" id="p3ShowD435CalibUCS" checked> UCS D435i hand-eye (arancio)</label>
|
||||
</div>
|
||||
<div class="p3-row">
|
||||
<label><input type="checkbox" id="p3ShowRobotBody" checked> Mostra robot (mesh)</label>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
|
||||
@@ -85,6 +85,14 @@
|
||||
align-items: center;
|
||||
gap: 4px;
|
||||
}
|
||||
/* Label colorate come gli assi UCS: X/Y/Z = rosso/verde/blu (riga 1),
|
||||
R/P/W = stessa tonalita' desaturata (riga 2, rotazione attorno a X/Y/Z). */
|
||||
#tab-pose-3d .p3-readout label:nth-child(1) { color: #e74c3c; }
|
||||
#tab-pose-3d .p3-readout label:nth-child(2) { color: #3ecf8e; }
|
||||
#tab-pose-3d .p3-readout label:nth-child(3) { color: #4da3ff; }
|
||||
#tab-pose-3d .p3-readout label:nth-child(4) { color: #c8857e; }
|
||||
#tab-pose-3d .p3-readout label:nth-child(5) { color: #88c2a8; }
|
||||
#tab-pose-3d .p3-readout label:nth-child(6) { color: #8aaedf; }
|
||||
#tab-pose-3d .p3-readout input {
|
||||
width: 60px;
|
||||
background: #0d1014;
|
||||
|
||||
+756
-26
@@ -47,6 +47,147 @@ function makeLabelSprite(text, color = '#ffffff') {
|
||||
return sprite;
|
||||
}
|
||||
|
||||
// Carica la mesh STL della Nuwa HP60C come child del polso destro: segue
|
||||
// rigidamente il wrist frame, posizione/orientamento applicati da config.
|
||||
// Aggiunge un UCS (assi colorati) all'origine di ogni link URDF. I helper
|
||||
// vengono memorizzati in state.joint_ucses per show/hide e resize rapidi.
|
||||
// I link "*_hand_*" vengono saltati (mani gia' nascoste).
|
||||
function addJointUCSes(robot) {
|
||||
const helpers = [];
|
||||
const visited = new Set();
|
||||
robot.traverse((obj) => {
|
||||
if (!obj.name || visited.has(obj.name)) return;
|
||||
if (obj.name.startsWith('left_hand_') || obj.name.startsWith('right_hand_')) return;
|
||||
if (obj.isURDFLink !== true) return;
|
||||
visited.add(obj.name);
|
||||
const ucs = makeUCS(0.10); // unit length; resize tramite .scale
|
||||
ucs.userData.isJointUCS = true;
|
||||
ucs.userData.linkName = obj.name;
|
||||
ucs.visible = false; // di default nascosto, abilita dal flag
|
||||
obj.add(ucs);
|
||||
helpers.push(ucs);
|
||||
});
|
||||
console.log(`[pose_3d] joint UCSes attached: ${helpers.length}`);
|
||||
return helpers;
|
||||
}
|
||||
|
||||
function setJointUCSVisible(visible) {
|
||||
for (const h of state.joint_ucses || []) h.visible = visible;
|
||||
}
|
||||
|
||||
function setJointUCSScale(sizeM) {
|
||||
const s = sizeM / 0.10;
|
||||
for (const h of state.joint_ucses || []) h.scale.setScalar(s);
|
||||
}
|
||||
|
||||
// UCS sempre visibile per ogni sensore fisso modellato nell'URDF
|
||||
// (RealSense D435i frontale, Livox MID360 lidar). I link esistono come
|
||||
// frame senza mesh; aggiungiamo qui un UCS + label per visualizzarli.
|
||||
function addFixedSensorMarkers(robot) {
|
||||
const sensors = [
|
||||
{ link: 'd435_link', label: 'D435i', color: '#ffd54f', size: 0.06 },
|
||||
{ link: 'mid360_link', label: 'MID360', color: '#9b59b6', size: 0.05 },
|
||||
];
|
||||
for (const s of sensors) {
|
||||
const link = (robot.links && robot.links[s.link])
|
||||
|| (() => { let f = null; robot.traverse(o => { if (o.name === s.link) f = o; }); return f; })();
|
||||
if (!link) continue;
|
||||
const ucs = makeUCS(s.size);
|
||||
link.add(ucs);
|
||||
const label = makeLabelSprite(s.label, s.color);
|
||||
label.position.set(0, 0, s.size + 0.02);
|
||||
link.add(label);
|
||||
}
|
||||
}
|
||||
|
||||
// Nasconde i mesh delle mani (palm + tutte le dita) lasciando i joint nel
|
||||
// grafo. Riconosce i nomi che iniziano per `left_hand_` / `right_hand_`.
|
||||
function hideHandMeshes(robot) {
|
||||
let n = 0;
|
||||
robot.traverse((obj) => {
|
||||
const name = obj.name || '';
|
||||
if (name.startsWith('left_hand_') || name.startsWith('right_hand_')) {
|
||||
obj.visible = false;
|
||||
n++;
|
||||
}
|
||||
});
|
||||
console.log(`[pose_3d] hand meshes hidden: ${n}`);
|
||||
}
|
||||
|
||||
// Trova il link URDF al quale la Nuwa e' fissata rigidamente (ultimo link
|
||||
// della catena polso destro). Ritorna null se l'URDF non e' ancora caricato.
|
||||
function getRightWristUrdfLink() {
|
||||
const r = state.robot;
|
||||
if (!r) return null;
|
||||
if (r.links && r.links['right_wrist_yaw_link']) return r.links['right_wrist_yaw_link'];
|
||||
let found = null;
|
||||
r.traverse((obj) => { if (obj.name === 'right_wrist_yaw_link') found = obj; });
|
||||
return found;
|
||||
}
|
||||
|
||||
// Aggancia la camera al link URDF: cosi' segue rotazione+posizione del joint,
|
||||
// non solo la posizione della sfera marker.
|
||||
function reparentNuwaToUrdf() {
|
||||
const cam = state.nuwa_mesh;
|
||||
const link = getRightWristUrdfLink();
|
||||
if (!cam || !link) return;
|
||||
if (cam.parent === link) return;
|
||||
link.add(cam); // THREE.Object3D.add() rimuove automaticamente dal vecchio parent
|
||||
applyNuwaTransform();
|
||||
}
|
||||
|
||||
function loadNuwaMesh(parent, meshUrl) {
|
||||
return new Promise((resolve, reject) => {
|
||||
const loader = new STLLoader();
|
||||
loader.load(
|
||||
meshUrl,
|
||||
(geom) => {
|
||||
geom.computeVertexNormals();
|
||||
const mesh = new THREE.Mesh(
|
||||
geom,
|
||||
new THREE.MeshStandardMaterial({
|
||||
color: 0xb8c1cb, metalness: 0.2, roughness: 0.5,
|
||||
emissive: 0x223040, emissiveIntensity: 0.6,
|
||||
}),
|
||||
);
|
||||
mesh.userData.isCustom = true; // protetto dal toggle "nascondi robot"
|
||||
mesh.scale.setScalar(0.001); // STL in mm, scena in metri
|
||||
// UCS visibile sull'origine della camera (debug orientamento)
|
||||
const cucs = makeUCS(0.04);
|
||||
const cam = new THREE.Group();
|
||||
cam.add(mesh);
|
||||
cam.add(cucs);
|
||||
// Default visibile anche prima che /state arrivi col config
|
||||
cam.position.set(0, 0, 0.1);
|
||||
parent.add(cam);
|
||||
resolve(cam);
|
||||
},
|
||||
undefined,
|
||||
(e) => { console.warn('[pose_3d] STLLoader error', e); reject(e); },
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
function applyNuwaTransform() {
|
||||
const cfg = state.nuwa_cfg;
|
||||
const cam = state.nuwa_mesh;
|
||||
if (!cam) return;
|
||||
if (cfg && cfg.enabled === false) { cam.visible = false; return; }
|
||||
cam.visible = true;
|
||||
// Se cfg non e' ancora arrivato, lascia i default impostati al load.
|
||||
if (!cfg) return;
|
||||
const [ox = 0, oy = 0, oz = 0.1] = cfg.offset_m || [];
|
||||
cam.position.set(ox, oy, oz);
|
||||
const [rd = 0, pd = 0, yd = 0] = cfg.rpy_deg || [];
|
||||
const e = new THREE.Euler(
|
||||
rd * Math.PI / 180,
|
||||
pd * Math.PI / 180,
|
||||
yd * Math.PI / 180,
|
||||
'XYZ',
|
||||
);
|
||||
cam.quaternion.setFromEuler(e);
|
||||
}
|
||||
|
||||
function makeWristMarker(color) {
|
||||
const group = new THREE.Group();
|
||||
const sphere = new THREE.Mesh(
|
||||
@@ -76,6 +217,11 @@ const state = {
|
||||
live_max_step_m: 0.15,
|
||||
robot: null, // URDF robot loaded via urdf-loader (null se non caricato)
|
||||
arm_links_m: null, // {upper_m, fore_m} computati da URDF al load
|
||||
nuwa_mesh: null, // mesh camera Nuwa montata sul polso destro (null se disabilitata)
|
||||
nuwa_cfg: null, // {enabled, mesh_url, offset_m, rpy_deg} dal config
|
||||
joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug)
|
||||
clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config
|
||||
cloud_points: { nuwa: null, d435: null }, // THREE.Points obj per camera
|
||||
};
|
||||
|
||||
function checkWorkspace(side, position_m) {
|
||||
@@ -149,6 +295,17 @@ async function loadG1Urdf(scene) {
|
||||
state.arm_links_m = measureArmLinkLengths(robot);
|
||||
console.log('[pose_3d] G1 URDF caricato:', robot.name || 'g1',
|
||||
'arm links (m):', state.arm_links_m);
|
||||
// Nascondi visivamente i link delle mani (palm, thumb, index, middle,
|
||||
// ring, pinky). I joint restano nel grafo per non rompere la catena
|
||||
// cinematica; viene oscurato solo il rendering del visual.
|
||||
hideHandMeshes(robot);
|
||||
// UCS per ogni link (di default nascosti, attivabili da checkbox)
|
||||
state.joint_ucses = addJointUCSes(robot);
|
||||
applyJointUCSUIState();
|
||||
// UCS permanenti per le camere fisse (D435i, MID360 lidar)
|
||||
addFixedSensorMarkers(robot);
|
||||
// Se la Nuwa e' gia' caricata, riparent sul link URDF cosi' segue i joint
|
||||
reparentNuwaToUrdf();
|
||||
resolve(robot);
|
||||
},
|
||||
undefined,
|
||||
@@ -233,6 +390,28 @@ function solveTwoBoneIK(shoulderWorldPos, wristTargetWorldPos, L1, L2, poleHintD
|
||||
};
|
||||
}
|
||||
|
||||
// Imposta direttamente i giunti URDF dai valori reali (q in rad) per nome.
|
||||
// Salta silenziosamente i nomi non presenti nell'URDF (joint fisso o
|
||||
// non modellato). Usa il suffisso "_joint" se richiesto da URDFLoader.
|
||||
function applyJointsToUrdf(joints) {
|
||||
const robot = state.robot;
|
||||
if (!robot || !joints) return 0;
|
||||
let n = 0;
|
||||
for (const name in joints) {
|
||||
const q = joints[name];
|
||||
if (!Number.isFinite(q)) continue;
|
||||
let j = robot.joints && (robot.joints[name] || robot.joints[`${name}_joint`]);
|
||||
if (!j) continue;
|
||||
try {
|
||||
robot.setJointValue(j.name, q);
|
||||
n++;
|
||||
} catch (e) {
|
||||
// joint fisso o fuori limiti — ignora
|
||||
}
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
function applyIKToUrdf(side, target_position, target_quaternion) {
|
||||
// Aggiorna i joint dell'URDF G1 per posizionare il polso `side` al target.
|
||||
// No-op se l'URDF non e' caricato.
|
||||
@@ -325,11 +504,18 @@ function initScene() {
|
||||
scene.add(wrist);
|
||||
state.arms[side] = wrist;
|
||||
|
||||
// Etichetta DX / SX sopra la sfera
|
||||
const label = makeLabelSprite(side === 'right' ? 'DX' : 'SX',
|
||||
side === 'right' ? '#4da3ff' : '#ff5e7e');
|
||||
label.position.set(0, 0, 0.08); // 8cm sopra la sfera (locale al gruppo)
|
||||
wrist.add(label);
|
||||
// Nuwa camera rigida sul polso destro (eye-in-hand): mesh STL, trasformata
|
||||
// letta da pose_3d.right_wrist_camera (offset+rpy in wrist frame).
|
||||
if (side === 'right') {
|
||||
loadNuwaMesh(wrist, '/static/assets/nuwa/nuwa-hp60c.stl')
|
||||
.then((cam) => {
|
||||
state.nuwa_mesh = cam;
|
||||
applyNuwaTransform();
|
||||
// Se l'URDF e' gia' caricato, sposta la mesh sul link URDF
|
||||
reparentNuwaToUrdf();
|
||||
})
|
||||
.catch((e) => console.warn('[pose_3d] STL Nuwa non caricato:', e.message));
|
||||
}
|
||||
|
||||
// Linea shoulder → wrist
|
||||
const lineGeom = new THREE.BufferGeometry().setFromPoints([
|
||||
@@ -427,15 +613,44 @@ function rpyDegToQuat(roll, pitch, yaw) {
|
||||
return [qx, qy, qz, qw];
|
||||
}
|
||||
|
||||
// Trova il link URDF del palmo per `side`. Usato per posizionare la sfera
|
||||
// marker selezionabile esattamente sul TCP visivo (= palm_link del modello).
|
||||
function getPalmLink(side) {
|
||||
if (!state.robot) return null;
|
||||
const name = `${side}_hand_palm_link`;
|
||||
if (state.robot.links && state.robot.links[name]) return state.robot.links[name];
|
||||
let found = null;
|
||||
state.robot.traverse(o => { if (o.name === name) found = o; });
|
||||
return found;
|
||||
}
|
||||
|
||||
// ----- state apply to scene -----
|
||||
function updateRealWrists() {
|
||||
const tmpV = new THREE.Vector3();
|
||||
const tmpQ = new THREE.Quaternion();
|
||||
for (const side of ['right', 'left']) {
|
||||
const arm = state.arms[side];
|
||||
if (!arm) continue;
|
||||
// Preferiamo la pose world del palm link URDF (cosi' la sfera coincide
|
||||
// sempre col TCP visivo dato dai giunti reali via /diagnostics).
|
||||
const palm = getPalmLink(side);
|
||||
if (palm) {
|
||||
palm.updateWorldMatrix(true, false);
|
||||
palm.getWorldPosition(tmpV);
|
||||
palm.getWorldQuaternion(tmpQ);
|
||||
arm.position.copy(tmpV);
|
||||
arm.quaternion.copy(tmpQ);
|
||||
} else {
|
||||
// Fallback: TCP da /state (G1 wrist), usato quando URDF non e' carico
|
||||
const pose = state.current[side];
|
||||
if (!pose) continue;
|
||||
const arm = state.arms[side];
|
||||
arm.position.set(pose.position[0], pose.position[1], pose.position[2]);
|
||||
arm.quaternion.set(pose.quaternion[0], pose.quaternion[1],
|
||||
pose.quaternion[2], pose.quaternion[3]);
|
||||
}
|
||||
// URDF: i giunti reali arrivano da pollState e li applichiamo li' (vedi
|
||||
// applyJointsToUrdf). Niente IK qui: l'IK e' un'approssimazione e
|
||||
// l'approccio diretto via /diagnostics e' esatto.
|
||||
// aggiorna line shoulder→wrist
|
||||
const line = state.links[side];
|
||||
const pts = [SHOULDER[side].clone(), arm.position.clone()];
|
||||
@@ -547,10 +762,20 @@ let pollTimer = null;
|
||||
async function pollState() {
|
||||
try {
|
||||
const s = await jsonFetch('/api/pose_3d/state');
|
||||
if (s.wrist) {
|
||||
state.current.right = s.wrist.right;
|
||||
state.current.left = s.wrist.left;
|
||||
}
|
||||
state.saved_poses = s.poses || {};
|
||||
state.live_max_step_m = s.live_max_step_m || 0.15;
|
||||
state.nuwa_cfg = s.right_wrist_camera || null;
|
||||
state.clouds_cfg = s.clouds || null;
|
||||
applyNuwaTransform();
|
||||
updateNuwaInputsFromCfg();
|
||||
updateD435InputsFromCfg();
|
||||
applyCalibratedD435UCS();
|
||||
// Applica TUTTI i giunti reali sul modello URDF (29 motori G1)
|
||||
if (s.joints) applyJointsToUrdf(s.joints);
|
||||
updateRealWrists();
|
||||
syncGhostsToTargetOrReal();
|
||||
updateReadout();
|
||||
@@ -563,16 +788,21 @@ async function pollState() {
|
||||
}
|
||||
|
||||
function refreshPoseSelect() {
|
||||
const sel = $('p3PoseSelect');
|
||||
if (!sel) return;
|
||||
// Popola i due dropdown filtrati per side (right → DX, left → SX).
|
||||
for (const side of ['right', 'left']) {
|
||||
const sel = $(POSE_UI[side].select);
|
||||
if (!sel) continue;
|
||||
const cur = sel.value;
|
||||
sel.innerHTML = '<option value="">(seleziona)</option>';
|
||||
for (const name of Object.keys(state.saved_poses).sort()) {
|
||||
const p = state.saved_poses[name];
|
||||
if (!p || p.side !== side) continue;
|
||||
const opt = document.createElement('option');
|
||||
opt.value = name; opt.textContent = name;
|
||||
sel.appendChild(opt);
|
||||
}
|
||||
sel.value = cur;
|
||||
}
|
||||
}
|
||||
|
||||
// ----- selezione polso via click -----
|
||||
@@ -872,17 +1102,24 @@ function bindJogAutoApply() {
|
||||
});
|
||||
}
|
||||
|
||||
// ----- named poses -----
|
||||
async function onSavePose() {
|
||||
const name = ($('p3PoseName').value || '').trim();
|
||||
// ----- named poses (sezioni separate DX e SX) -----
|
||||
const POSE_UI = {
|
||||
right: { select: 'p3PoseSelectR', go: 'p3PoseGoBtnR', name: 'p3PoseNameR',
|
||||
save: 'p3PoseSaveBtnR', del: 'p3PoseDelBtnR', label: 'DX' },
|
||||
left: { select: 'p3PoseSelectL', go: 'p3PoseGoBtnL', name: 'p3PoseNameL',
|
||||
save: 'p3PoseSaveBtnL', del: 'p3PoseDelBtnL', label: 'SX' },
|
||||
};
|
||||
|
||||
async function onSavePose(side) {
|
||||
const ui = POSE_UI[side];
|
||||
const name = ($(ui.name).value || '').trim();
|
||||
if (!name) { alert('Inserisci un nome'); return; }
|
||||
if (!/^[A-Za-z0-9_-]{1,40}$/.test(name)) {
|
||||
alert('Nome: solo [A-Za-z0-9_-], 1-40 caratteri');
|
||||
return;
|
||||
}
|
||||
const side = state.active_side;
|
||||
const pose = state.target[side] || state.current[side];
|
||||
if (!pose) { alert('Nessuna posa da salvare'); return; }
|
||||
if (!pose) { alert(`Nessuna posa ${ui.label} da salvare`); return; }
|
||||
if (state.saved_poses[name]) {
|
||||
if (!confirm(`Posa "${name}" esiste già — sovrascrivere?`)) return;
|
||||
}
|
||||
@@ -893,19 +1130,20 @@ async function onSavePose() {
|
||||
side, position: pose.position, quaternion: pose.quaternion,
|
||||
}),
|
||||
});
|
||||
$('p3PoseName').value = '';
|
||||
$(ui.name).value = '';
|
||||
await pollState(); // refresh dropdown
|
||||
$('p3PoseSelect').value = name;
|
||||
$(ui.select).value = name;
|
||||
} catch (e) {
|
||||
showError(`Salvataggio fallito: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
async function onGoPose() {
|
||||
const name = $('p3PoseSelect').value;
|
||||
if (!name) { alert('Seleziona una posa dal dropdown'); return; }
|
||||
async function onGoPose(side) {
|
||||
const ui = POSE_UI[side];
|
||||
const name = $(ui.select).value;
|
||||
if (!name) { alert(`Seleziona una posa ${ui.label} dal dropdown`); return; }
|
||||
if (state.mode === 'jog') {
|
||||
if (!confirm(`Eseguire posa "${name}" SUL ROBOT REALE? (Jog mode)`)) return;
|
||||
if (!confirm(`Eseguire posa "${name}" (${ui.label}) SUL ROBOT REALE?`)) return;
|
||||
}
|
||||
const dry_run = (state.mode === 'editor');
|
||||
clearError();
|
||||
@@ -921,7 +1159,7 @@ async function onGoPose() {
|
||||
if (r.reached_target === false || r.stalled === true) {
|
||||
showError(`Vai a "${name}": residual ${(r.position_error_mm||0).toFixed(1)} mm`);
|
||||
} else {
|
||||
$('p3Status').textContent = `✓ Posa "${name}" applicata` +
|
||||
$('p3Status').textContent = `✓ Posa "${name}" (${ui.label}) applicata` +
|
||||
(dry_run ? ' (simulato)' : '');
|
||||
$('p3Status').className = 'p3-status ok';
|
||||
const pose = state.saved_poses[name];
|
||||
@@ -932,9 +1170,10 @@ async function onGoPose() {
|
||||
}
|
||||
}
|
||||
|
||||
async function onDeletePose() {
|
||||
const name = $('p3PoseSelect').value;
|
||||
if (!name) { alert('Seleziona una posa dal dropdown'); return; }
|
||||
async function onDeletePose(side) {
|
||||
const ui = POSE_UI[side];
|
||||
const name = $(ui.select).value;
|
||||
if (!name) { alert(`Seleziona una posa ${ui.label} dal dropdown`); return; }
|
||||
if (!confirm(`Cancellare definitivamente la posa "${name}"?`)) return;
|
||||
try {
|
||||
await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
|
||||
@@ -947,9 +1186,12 @@ async function onDeletePose() {
|
||||
}
|
||||
|
||||
function bindPoseButtons() {
|
||||
$('p3PoseSaveBtn').addEventListener('click', onSavePose);
|
||||
$('p3PoseGoBtn').addEventListener('click', onGoPose);
|
||||
$('p3PoseDelBtn').addEventListener('click', onDeletePose);
|
||||
for (const side of ['right', 'left']) {
|
||||
const ui = POSE_UI[side];
|
||||
$(ui.save).addEventListener('click', () => onSavePose(side));
|
||||
$(ui.go).addEventListener('click', () => onGoPose(side));
|
||||
$(ui.del).addEventListener('click', () => onDeletePose(side));
|
||||
}
|
||||
}
|
||||
|
||||
async function onUnlock() {
|
||||
@@ -967,6 +1209,490 @@ function bindUnlockButton() {
|
||||
$('p3UnlockBtn').addEventListener('click', onUnlock);
|
||||
}
|
||||
|
||||
// UCS al pose hand-eye calibrato della D435i (frame pelvis).
|
||||
// La matrice in config e' la convenzione `T @ p_pelvis = p_optical` (cioe'
|
||||
// trasforma punti DAL pelvis AL frame ottico camera). Per posizionare la
|
||||
// camera nel pelvis frame ci serve l'INVERSA.
|
||||
function applyCalibratedD435UCS() {
|
||||
if (!state.three) return;
|
||||
const cfg = state.clouds_cfg && state.clouds_cfg.d435;
|
||||
const m4 = cfg && cfg.transform_pelvis_4x4;
|
||||
if (!Array.isArray(m4) || m4.length !== 4) return;
|
||||
|
||||
const M_pelvis_to_optical = new THREE.Matrix4();
|
||||
M_pelvis_to_optical.set(
|
||||
m4[0][0], m4[0][1], m4[0][2], m4[0][3],
|
||||
m4[1][0], m4[1][1], m4[1][2], m4[1][3],
|
||||
m4[2][0], m4[2][1], m4[2][2], m4[2][3],
|
||||
m4[3][0], m4[3][1], m4[3][2], m4[3][3],
|
||||
);
|
||||
// Pose della camera in pelvis frame = inversa.
|
||||
const M_optical_in_pelvis = M_pelvis_to_optical.clone().invert();
|
||||
|
||||
if (!state.d435_calib_ucs) {
|
||||
const grp = new THREE.Group();
|
||||
// UCS con assi piu' spessi/lunghi per distinguerlo da quelli URDF
|
||||
const ucs = makeUCS(0.12);
|
||||
grp.add(ucs);
|
||||
// Pallina centrale arancione = identifica visivamente la D435i calibrata
|
||||
const dot = new THREE.Mesh(
|
||||
new THREE.SphereGeometry(0.012, 12, 8),
|
||||
new THREE.MeshBasicMaterial({ color: 0xff6f4d }),
|
||||
);
|
||||
grp.add(dot);
|
||||
const label = makeLabelSprite('D435i hand-eye', '#ff6f4d');
|
||||
label.scale.set(0.14, 0.06, 1); // un po' piu' larga del default
|
||||
label.position.set(0, 0, 0.16);
|
||||
grp.add(label);
|
||||
grp.matrixAutoUpdate = false;
|
||||
state.three.scene.add(grp);
|
||||
state.d435_calib_ucs = grp;
|
||||
}
|
||||
state.d435_calib_ucs.matrix.copy(M_optical_in_pelvis);
|
||||
state.d435_calib_ucs.matrixWorldNeedsUpdate = true;
|
||||
applyD435CalibUCSVisibility();
|
||||
}
|
||||
|
||||
function applyD435CalibUCSVisibility() {
|
||||
if (!state.d435_calib_ucs) return;
|
||||
const visible = $('p3ShowD435CalibUCS')?.checked !== false;
|
||||
state.d435_calib_ucs.visible = visible;
|
||||
}
|
||||
|
||||
// ----- Cloud cross-camera (Nuwa + D435i) -----
|
||||
const CLOUD_COLORS = { nuwa: 0xffd54f, d435: 0x4dd0e1 };
|
||||
|
||||
function readCloudSize(camKey) {
|
||||
// Valore UI in mm; PointsMaterial in modalita' sizeAttenuation:true usa
|
||||
// world units (metri) → divide per 1000.
|
||||
const id = camKey === 'nuwa' ? 'p3NuwaCloudSize' : 'p3D435CloudSize';
|
||||
const mm = parseFloat($(id)?.value);
|
||||
return Number.isFinite(mm) && mm > 0 ? mm / 1000 : 0.005;
|
||||
}
|
||||
|
||||
function applyCloudSize(camKey) {
|
||||
const pts = state.cloud_points[camKey];
|
||||
if (!pts) return;
|
||||
pts.material.size = readCloudSize(camKey);
|
||||
pts.material.needsUpdate = true;
|
||||
}
|
||||
|
||||
function getCloudParent(camKey) {
|
||||
// D435i in modalita' hand-eye: child della scene (pelvis = origin scena)
|
||||
if (camKey === 'd435' && useD435Handeye()) {
|
||||
return state.three && state.three.scene;
|
||||
}
|
||||
if (!state.robot) return null;
|
||||
const linkName = camKey === 'nuwa' ? 'right_wrist_yaw_link' : 'd435_link';
|
||||
if (state.robot.links && state.robot.links[linkName]) return state.robot.links[linkName];
|
||||
let found = null;
|
||||
state.robot.traverse(o => { if (o.name === linkName) found = o; });
|
||||
return found;
|
||||
}
|
||||
|
||||
function useD435Handeye() {
|
||||
return $('p3D435UseHandeye')?.checked === true;
|
||||
}
|
||||
|
||||
// Restituisce la Matrix4 M_optical_in_pelvis dalla calibrazione hand-eye,
|
||||
// oppure null se la matrice non e' presente in config.
|
||||
function getD435HandeyeMatrix() {
|
||||
const cfg = state.clouds_cfg && state.clouds_cfg.d435;
|
||||
const m4 = cfg && cfg.transform_pelvis_4x4;
|
||||
if (!Array.isArray(m4) || m4.length !== 4) return null;
|
||||
const M = new THREE.Matrix4().set(
|
||||
m4[0][0], m4[0][1], m4[0][2], m4[0][3],
|
||||
m4[1][0], m4[1][1], m4[1][2], m4[1][3],
|
||||
m4[2][0], m4[2][1], m4[2][2], m4[2][3],
|
||||
m4[3][0], m4[3][1], m4[3][2], m4[3][3],
|
||||
);
|
||||
return M.invert(); // pelvis→optical → optical pose in pelvis
|
||||
}
|
||||
|
||||
// Trasformazione optical→link da config: per nuwa eredita il
|
||||
// right_wrist_camera (offset+rpy), per d435 usa clouds.d435.
|
||||
function getCloudTransform(camKey) {
|
||||
const c = state.clouds_cfg || {};
|
||||
if (camKey === 'nuwa') {
|
||||
const nuwa = state.nuwa_cfg || {};
|
||||
const ex = (c.nuwa && c.nuwa.extra_offset_m) || [0,0,0];
|
||||
const er = (c.nuwa && c.nuwa.extra_rpy_deg) || [0,0,0];
|
||||
const base_off = nuwa.offset_m || [0,0,0.1];
|
||||
const base_rpy = nuwa.rpy_deg || [0,0,0];
|
||||
return {
|
||||
offset_m: [base_off[0]+ex[0], base_off[1]+ex[1], base_off[2]+ex[2]],
|
||||
rpy_deg: [base_rpy[0]+er[0], base_rpy[1]+er[1], base_rpy[2]+er[2]],
|
||||
};
|
||||
}
|
||||
const d = c.d435 || {};
|
||||
return { offset_m: d.offset_m || [0,0,0], rpy_deg: d.rpy_deg || [-90,0,-90] };
|
||||
}
|
||||
|
||||
function buildOrUpdateCloud(camKey, pointsFlat) {
|
||||
const parent = getCloudParent(camKey);
|
||||
if (!parent) { console.warn('[pose_3d] cloud parent link non trovato per', camKey); return null; }
|
||||
const N = pointsFlat.length;
|
||||
const flat = new Float32Array(N * 3);
|
||||
const colors = new Float32Array(N * 3);
|
||||
const hasRgb = pointsFlat.length > 0 && pointsFlat[0].length >= 6;
|
||||
for (let i = 0; i < N; i++) {
|
||||
flat[3*i] = pointsFlat[i][0];
|
||||
flat[3*i+1] = pointsFlat[i][1];
|
||||
flat[3*i+2] = pointsFlat[i][2];
|
||||
if (hasRgb) {
|
||||
colors[3*i] = pointsFlat[i][3] / 255.0;
|
||||
colors[3*i+1] = pointsFlat[i][4] / 255.0;
|
||||
colors[3*i+2] = pointsFlat[i][5] / 255.0;
|
||||
}
|
||||
}
|
||||
let pts = state.cloud_points[camKey];
|
||||
if (pts) {
|
||||
pts.geometry.dispose();
|
||||
pts.geometry = new THREE.BufferGeometry();
|
||||
pts.geometry.setAttribute('position', new THREE.BufferAttribute(flat, 3));
|
||||
if (hasRgb) pts.geometry.setAttribute('color', new THREE.BufferAttribute(colors, 3));
|
||||
pts.material.vertexColors = hasRgb;
|
||||
pts.material.needsUpdate = true;
|
||||
} else {
|
||||
const geom = new THREE.BufferGeometry();
|
||||
geom.setAttribute('position', new THREE.BufferAttribute(flat, 3));
|
||||
if (hasRgb) geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
|
||||
// RGB reali se disponibili, altrimenti colore camera-specific come fallback.
|
||||
// sizeAttenuation:true → la dimensione e' in metri e scala con la distanza.
|
||||
const mat = new THREE.PointsMaterial({
|
||||
color: hasRgb ? 0xffffff : CLOUD_COLORS[camKey],
|
||||
size: readCloudSize(camKey), sizeAttenuation: true,
|
||||
vertexColors: hasRgb,
|
||||
});
|
||||
pts = new THREE.Points(geom, mat);
|
||||
pts.userData.camKey = camKey;
|
||||
state.cloud_points[camKey] = pts;
|
||||
}
|
||||
// Log diagnostico
|
||||
let xmin=Infinity,xmax=-Infinity,ymin=Infinity,ymax=-Infinity,zmin=Infinity,zmax=-Infinity;
|
||||
for (let i = 0; i < N; i++) {
|
||||
const x=flat[3*i], y=flat[3*i+1], z=flat[3*i+2];
|
||||
if (x<xmin)xmin=x; if (x>xmax)xmax=x;
|
||||
if (y<ymin)ymin=y; if (y>ymax)ymax=y;
|
||||
if (z<zmin)zmin=z; if (z>zmax)zmax=z;
|
||||
}
|
||||
console.log(`[pose_3d] cloud ${camKey}: ${N} pts parent=${parent.name}`,
|
||||
`X[${xmin.toFixed(2)},${xmax.toFixed(2)}]`,
|
||||
`Y[${ymin.toFixed(2)},${ymax.toFixed(2)}]`,
|
||||
`Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`);
|
||||
// (Ri-)attacca al parent corretto e applica transform optical→link
|
||||
if (pts.parent !== parent) parent.add(pts);
|
||||
applyCloudTransform(camKey);
|
||||
applyCloudVisibility();
|
||||
return pts;
|
||||
}
|
||||
|
||||
function applyCloudVisibility() {
|
||||
const showNuwa = $('p3ShowNuwaCloud')?.checked === true;
|
||||
const showD435 = $('p3ShowD435Cloud')?.checked === true;
|
||||
if (state.cloud_points.nuwa) state.cloud_points.nuwa.visible = showNuwa;
|
||||
if (state.cloud_points.d435) state.cloud_points.d435.visible = showD435;
|
||||
}
|
||||
|
||||
// Applica position+rpy+mirror sui punti del cloud `camKey`. Il mirror (sign
|
||||
// flip per asse) avviene IN FRAME OTTICO grazie all'ordine T·R·S di Three.js
|
||||
// (lo scale e' la prima operazione applicata al punto locale).
|
||||
function applyCloudTransform(camKey) {
|
||||
const pts = state.cloud_points[camKey];
|
||||
if (!pts) return;
|
||||
const sv = readCloudScaleVec(camKey); // flip × scale (frame ottico)
|
||||
|
||||
// Modalita' hand-eye per D435i: usa direttamente la matrice 4x4 calibrata
|
||||
if (camKey === 'd435' && useD435Handeye()) {
|
||||
const M = getD435HandeyeMatrix();
|
||||
if (M) {
|
||||
pts.matrixAutoUpdate = false;
|
||||
const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]);
|
||||
pts.matrix.copy(M).multiply(S);
|
||||
pts.matrixWorldNeedsUpdate = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Modalita' URDF link + offset/rpy (default)
|
||||
pts.matrixAutoUpdate = true;
|
||||
const t = getCloudTransform(camKey);
|
||||
pts.position.set(t.offset_m[0], t.offset_m[1], t.offset_m[2]);
|
||||
const e = new THREE.Euler(
|
||||
t.rpy_deg[0] * Math.PI/180,
|
||||
t.rpy_deg[1] * Math.PI/180,
|
||||
t.rpy_deg[2] * Math.PI/180,
|
||||
'XYZ',
|
||||
);
|
||||
pts.quaternion.setFromEuler(e);
|
||||
pts.scale.set(sv[0], sv[1], sv[2]);
|
||||
}
|
||||
|
||||
function readCloudFlip(camKey) {
|
||||
const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435';
|
||||
return [
|
||||
$(`p3Flip${tag}X`)?.checked ? -1 : 1,
|
||||
$(`p3Flip${tag}Y`)?.checked ? -1 : 1,
|
||||
$(`p3Flip${tag}Z`)?.checked ? -1 : 1,
|
||||
];
|
||||
}
|
||||
|
||||
// Scale per asse (workaround bug intrinsics-mismatch). Per D435i applichiamo
|
||||
// fx_ir/fx_rgb ≈ 0.65 su X e Y; Z corretto. Per Nuwa nessuna correzione.
|
||||
function readCloudScale(camKey) {
|
||||
if (camKey !== 'd435') return [1, 1, 1];
|
||||
const v = parseFloat($('p3D435ScaleXY')?.value);
|
||||
const s = Number.isFinite(v) && v > 0 ? v : 1;
|
||||
return [s, s, 1];
|
||||
}
|
||||
|
||||
// Combina flip (±1) e correzione scale per asse.
|
||||
function readCloudScaleVec(camKey) {
|
||||
const f = readCloudFlip(camKey);
|
||||
const sc = readCloudScale(camKey);
|
||||
return [f[0] * sc[0], f[1] * sc[1], f[2] * sc[2]];
|
||||
}
|
||||
|
||||
async function acquireClouds() {
|
||||
// Clamp dei parametri: step oltre ~32 fa 422 sul server pointcloud.
|
||||
let step = parseInt($('p3CloudStep').value, 10);
|
||||
if (!Number.isFinite(step) || step < 1) step = 8;
|
||||
if (step > 32) step = 32;
|
||||
$('p3CloudStep').value = String(step);
|
||||
let max_depth_mm = parseInt($('p3CloudMaxDepth').value, 10);
|
||||
if (!Number.isFinite(max_depth_mm) || max_depth_mm < 100) max_depth_mm = 2000;
|
||||
if (max_depth_mm > 5000) max_depth_mm = 5000;
|
||||
$('p3CloudMaxDepth').value = String(max_depth_mm);
|
||||
$('p3CloudStatus').textContent = 'acquisizione…';
|
||||
$('p3CloudStatus').style.color = 'var(--dim)';
|
||||
try {
|
||||
const r = await jsonFetch(
|
||||
`/api/pose_3d/clouds?step=${step}&max_depth_mm=${max_depth_mm}`,
|
||||
);
|
||||
const parts = [];
|
||||
for (const cam of ['nuwa', 'd435']) {
|
||||
const c = r[cam];
|
||||
if (!c || c.error) {
|
||||
parts.push(`${cam}: ${c?.error || 'no data'}`);
|
||||
continue;
|
||||
}
|
||||
buildOrUpdateCloud(cam, c.points);
|
||||
parts.push(`${cam}: ${c.n} pts`);
|
||||
}
|
||||
$('p3CloudStatus').textContent = parts.join(' · ');
|
||||
$('p3CloudStatus').style.color = 'var(--ok)';
|
||||
} catch (e) {
|
||||
$('p3CloudStatus').textContent = `errore: ${e.message}`;
|
||||
$('p3CloudStatus').style.color = 'var(--danger)';
|
||||
}
|
||||
}
|
||||
|
||||
function clearClouds() {
|
||||
for (const cam of ['nuwa', 'd435']) {
|
||||
const pts = state.cloud_points[cam];
|
||||
if (pts) {
|
||||
pts.parent?.remove(pts);
|
||||
pts.geometry.dispose();
|
||||
state.cloud_points[cam] = null;
|
||||
}
|
||||
}
|
||||
$('p3CloudStatus').textContent = '—';
|
||||
$('p3CloudStatus').style.color = '';
|
||||
}
|
||||
|
||||
function bindCloudControls() {
|
||||
$('p3CloudAcquireBtn')?.addEventListener('click', acquireClouds);
|
||||
$('p3CloudClearBtn')?.addEventListener('click', clearClouds);
|
||||
$('p3ShowNuwaCloud')?.addEventListener('change', applyCloudVisibility);
|
||||
$('p3ShowD435Cloud')?.addEventListener('change', applyCloudVisibility);
|
||||
// Mirror toggles: applicano lo scale in frame ottico senza ri-acquisire
|
||||
for (const tag of ['Nuwa', 'D435']) {
|
||||
const key = tag === 'Nuwa' ? 'nuwa' : 'd435';
|
||||
for (const axis of ['X', 'Y', 'Z']) {
|
||||
$(`p3Flip${tag}${axis}`)?.addEventListener('change', () => applyCloudTransform(key));
|
||||
}
|
||||
}
|
||||
// Swap modalita' D435i: riparenting + ricalcolo trasformazione
|
||||
$('p3D435UseHandeye')?.addEventListener('change', () => {
|
||||
const pts = state.cloud_points.d435;
|
||||
if (!pts) return;
|
||||
const newParent = getCloudParent('d435');
|
||||
if (newParent && pts.parent !== newParent) newParent.add(pts);
|
||||
applyCloudTransform('d435');
|
||||
});
|
||||
// Size live per camera (pixel size)
|
||||
for (const ev of ['input', 'change']) {
|
||||
$('p3NuwaCloudSize')?.addEventListener(ev, () => applyCloudSize('nuwa'));
|
||||
$('p3D435CloudSize')?.addEventListener(ev, () => applyCloudSize('d435'));
|
||||
}
|
||||
// Scale XY workaround D435i (intrinsics-mismatch fix)
|
||||
for (const ev of ['input', 'change']) {
|
||||
$('p3D435ScaleXY')?.addEventListener(ev, () => applyCloudTransform('d435'));
|
||||
}
|
||||
}
|
||||
|
||||
// ----- joint UCS debug toggle -----
|
||||
function applyJointUCSUIState() {
|
||||
const visible = $('p3ShowJointUCS')?.checked === true;
|
||||
const sizeMm = parseFloat($('p3JointUCSSize')?.value || '40');
|
||||
setJointUCSVisible(visible);
|
||||
setJointUCSScale((Number.isFinite(sizeMm) ? sizeMm : 40) / 1000);
|
||||
}
|
||||
|
||||
function bindJointUCSControls() {
|
||||
$('p3ShowJointUCS')?.addEventListener('change', applyJointUCSUIState);
|
||||
$('p3JointUCSSize')?.addEventListener('change', applyJointUCSUIState);
|
||||
$('p3JointUCSSize')?.addEventListener('input', applyJointUCSUIState);
|
||||
$('p3ShowD435CalibUCS')?.addEventListener('change', applyD435CalibUCSVisibility);
|
||||
$('p3ShowRobotBody')?.addEventListener('change', applyRobotBodyVisibility);
|
||||
}
|
||||
|
||||
// Toggla solo i mesh "nativi" dell'URDF, lasciando UCS giunti / sensor / Nuwa
|
||||
// (taggati con userData.isCustom=true) e label/sprite sempre visibili.
|
||||
function applyRobotBodyVisibility() {
|
||||
if (!state.robot) return;
|
||||
const visible = $('p3ShowRobotBody')?.checked !== false;
|
||||
state.robot.traverse((obj) => {
|
||||
if (obj.isMesh && !obj.userData.isCustom) {
|
||||
obj.visible = visible;
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// ----- Nuwa camera params (offset+rpy in wrist frame) -----
|
||||
const NUWA_DEFAULTS = { enabled: true, offset_mm: [0, 0, 100], rpy_deg: [0, 0, -90] };
|
||||
|
||||
function updateNuwaInputsFromCfg() {
|
||||
// Popola solo al primo poll. Poi l'utente controlla i campi finche' non
|
||||
// clicca Default (che bypassa questa funzione).
|
||||
if (state.nuwa_inputs_populated) return;
|
||||
const cfg = state.nuwa_cfg;
|
||||
if (!cfg) return;
|
||||
const enabled = cfg.enabled !== false;
|
||||
const [ox = 0, oy = 0, oz = 0.1] = cfg.offset_m || [];
|
||||
const [rd = 0, pd = 0, yd = 0] = cfg.rpy_deg || [];
|
||||
const elE = $('p3NuwaEnabled');
|
||||
if (elE) elE.checked = enabled;
|
||||
const fields = [
|
||||
['p3NuwaX', Math.round(ox * 1000)],
|
||||
['p3NuwaY', Math.round(oy * 1000)],
|
||||
['p3NuwaZ', Math.round(oz * 1000)],
|
||||
['p3NuwaR', Math.round(rd)],
|
||||
['p3NuwaP', Math.round(pd)],
|
||||
['p3NuwaW', Math.round(yd)],
|
||||
];
|
||||
for (const [id, val] of fields) {
|
||||
const el = $(id);
|
||||
if (el) el.value = String(val);
|
||||
}
|
||||
state.nuwa_inputs_populated = true;
|
||||
}
|
||||
|
||||
async function onNuwaApply() {
|
||||
const num = (id) => Number($(id).value);
|
||||
const offset_m = [num('p3NuwaX') / 1000, num('p3NuwaY') / 1000, num('p3NuwaZ') / 1000];
|
||||
const rpy_deg = [num('p3NuwaR'), num('p3NuwaP'), num('p3NuwaW')];
|
||||
const enabled = $('p3NuwaEnabled').checked;
|
||||
const patch = {
|
||||
pose_3d: {
|
||||
right_wrist_camera: { enabled, offset_m, rpy_deg },
|
||||
},
|
||||
};
|
||||
try {
|
||||
const r = await fetch('/api/config', {
|
||||
method: 'PUT',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(patch),
|
||||
});
|
||||
if (!r.ok) throw new Error(`HTTP ${r.status}`);
|
||||
// riapplica subito senza aspettare il prossimo poll
|
||||
state.nuwa_cfg = { ...state.nuwa_cfg, enabled, offset_m, rpy_deg };
|
||||
applyNuwaTransform();
|
||||
$('p3Status').textContent = '✓ Nuwa applicata';
|
||||
$('p3Status').className = 'p3-status ok';
|
||||
} catch (e) {
|
||||
showError(`Nuwa apply fallito: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
function onNuwaReset() {
|
||||
$('p3NuwaEnabled').checked = NUWA_DEFAULTS.enabled;
|
||||
$('p3NuwaX').value = String(NUWA_DEFAULTS.offset_mm[0]);
|
||||
$('p3NuwaY').value = String(NUWA_DEFAULTS.offset_mm[1]);
|
||||
$('p3NuwaZ').value = String(NUWA_DEFAULTS.offset_mm[2]);
|
||||
$('p3NuwaR').value = String(NUWA_DEFAULTS.rpy_deg[0]);
|
||||
$('p3NuwaP').value = String(NUWA_DEFAULTS.rpy_deg[1]);
|
||||
$('p3NuwaW').value = String(NUWA_DEFAULTS.rpy_deg[2]);
|
||||
}
|
||||
|
||||
function bindNuwaControls() {
|
||||
$('p3NuwaApplyBtn')?.addEventListener('click', onNuwaApply);
|
||||
$('p3NuwaResetBtn')?.addEventListener('click', onNuwaReset);
|
||||
}
|
||||
|
||||
// ----- D435i URDF-mode offset (offset_m + rpy_deg vs d435_link) -----
|
||||
const D435_DEFAULTS = { offset_mm: [0, 0, 0], rpy_deg: [-90, 90, 0] };
|
||||
|
||||
function updateD435InputsFromCfg() {
|
||||
// Popola solo al primo poll: dopo non si tocca, l'utente controlla.
|
||||
if (state.d435_inputs_populated) return;
|
||||
const cfg = state.clouds_cfg && state.clouds_cfg.d435;
|
||||
if (!cfg) return;
|
||||
const [ox = 0, oy = 0, oz = 0] = cfg.offset_m || [];
|
||||
const [rd = -90, pd = 90, yd = 0] = cfg.rpy_deg || [];
|
||||
const fields = [
|
||||
['p3D435X', Math.round(ox * 1000)],
|
||||
['p3D435Y', Math.round(oy * 1000)],
|
||||
['p3D435Z', Math.round(oz * 1000)],
|
||||
['p3D435R', Math.round(rd)],
|
||||
['p3D435P', Math.round(pd)],
|
||||
['p3D435W', Math.round(yd)],
|
||||
];
|
||||
for (const [id, val] of fields) {
|
||||
const el = $(id);
|
||||
if (el) el.value = String(val);
|
||||
}
|
||||
state.d435_inputs_populated = true;
|
||||
}
|
||||
|
||||
async function onD435Apply() {
|
||||
const num = (id) => Number($(id).value);
|
||||
const offset_m = [num('p3D435X') / 1000, num('p3D435Y') / 1000, num('p3D435Z') / 1000];
|
||||
const rpy_deg = [num('p3D435R'), num('p3D435P'), num('p3D435W')];
|
||||
const patch = {
|
||||
pose_3d: { clouds: { d435: { offset_m, rpy_deg } } },
|
||||
};
|
||||
try {
|
||||
const r = await fetch('/api/config', {
|
||||
method: 'PUT',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(patch),
|
||||
});
|
||||
if (!r.ok) throw new Error(`HTTP ${r.status}`);
|
||||
state.clouds_cfg = state.clouds_cfg || {};
|
||||
state.clouds_cfg.d435 = { ...state.clouds_cfg.d435, offset_m, rpy_deg };
|
||||
applyCloudTransform('d435');
|
||||
$('p3Status').textContent = '✓ D435i applicata';
|
||||
$('p3Status').className = 'p3-status ok';
|
||||
} catch (e) {
|
||||
showError(`D435i apply fallito: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
function onD435Reset() {
|
||||
$('p3D435X').value = String(D435_DEFAULTS.offset_mm[0]);
|
||||
$('p3D435Y').value = String(D435_DEFAULTS.offset_mm[1]);
|
||||
$('p3D435Z').value = String(D435_DEFAULTS.offset_mm[2]);
|
||||
$('p3D435R').value = String(D435_DEFAULTS.rpy_deg[0]);
|
||||
$('p3D435P').value = String(D435_DEFAULTS.rpy_deg[1]);
|
||||
$('p3D435W').value = String(D435_DEFAULTS.rpy_deg[2]);
|
||||
}
|
||||
|
||||
function bindD435Controls() {
|
||||
$('p3D435ApplyBtn')?.addEventListener('click', onD435Apply);
|
||||
$('p3D435ResetBtn')?.addEventListener('click', onD435Reset);
|
||||
}
|
||||
|
||||
// ----- init triggered on first activation -----
|
||||
let inited = false;
|
||||
function ensureInit() {
|
||||
@@ -984,6 +1710,10 @@ function ensureInit() {
|
||||
bindJogAutoApply();
|
||||
bindPoseButtons();
|
||||
bindUnlockButton();
|
||||
bindNuwaControls();
|
||||
bindD435Controls();
|
||||
bindJointUCSControls();
|
||||
bindCloudControls();
|
||||
bindReadoutInputs();
|
||||
pollState();
|
||||
pollTimer = setInterval(pollState, POLL_MS);
|
||||
|
||||
@@ -704,37 +704,6 @@ details.joint-temps summary { cursor: pointer; font-weight: 600; font-size: 12px
|
||||
.device-pill.err { background: rgba(231,76,60,0.15); color: var(--danger); border: 1px solid var(--danger); }
|
||||
.active-robot-pill { font-size: 12px; padding: 4px 10px; border-radius: 999px; background: rgba(77,163,255,0.15); color: var(--accent); border: 1px solid var(--accent); margin-left: 12px; }
|
||||
|
||||
/* ---------- Assistente AI tab ---------- */
|
||||
.agent-layout > div { display: flex; flex-direction: column; }
|
||||
.agent-chat {
|
||||
border: 1px solid var(--border);
|
||||
border-radius: 4px;
|
||||
padding: 10px;
|
||||
height: 420px;
|
||||
overflow-y: auto;
|
||||
background: rgba(0,0,0,0.15);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 8px;
|
||||
}
|
||||
.agent-msg { padding: 8px 12px; border-radius: 6px; max-width: 90%; line-height: 1.4; font-size: 13px; white-space: pre-wrap; word-wrap: break-word; }
|
||||
.agent-msg.user { align-self: flex-end; background: rgba(77,163,255,0.15); border: 1px solid var(--accent); }
|
||||
.agent-msg.assistant { align-self: flex-start; background: rgba(255,255,255,0.04); border: 1px solid var(--border); }
|
||||
.agent-msg.system { align-self: stretch; background: rgba(240,173,78,0.08); border: 1px dashed var(--warn); color: var(--dim); font-style: italic; }
|
||||
.agent-msg.error { align-self: stretch; background: rgba(231,76,60,0.10); border: 1px solid var(--danger); color: var(--danger); }
|
||||
.agent-msg .meta { font-size: 10px; color: var(--dim); margin-top: 4px; }
|
||||
.agent-tool-pill { display: inline-block; padding: 1px 8px; margin: 2px 2px 0 0; font-size: 11px; border-radius: 999px; background: rgba(62,207,142,0.12); border: 1px solid var(--ok); color: var(--ok); }
|
||||
.agent-tool-pill.err { background: rgba(231,76,60,0.12); border-color: var(--danger); color: var(--danger); }
|
||||
.agent-input-row { display: flex; gap: 8px; align-items: stretch; margin-top: 8px; }
|
||||
.agent-input-row textarea { flex: 1; resize: vertical; font-family: ui-monospace, monospace; font-size: 13px; padding: 8px; background: #0e1116; color: inherit; border: 1px solid var(--border); border-radius: 4px; }
|
||||
.agent-input-row button { white-space: nowrap; }
|
||||
.agent-task-list { list-style: none; padding: 0; margin: 8px 0; max-height: 260px; overflow-y: auto; }
|
||||
.agent-task-list li { display: flex; gap: 6px; align-items: center; padding: 6px 8px; border-bottom: 1px solid var(--border); font-size: 12px; }
|
||||
.agent-task-list li:hover { background: rgba(255,255,255,0.03); }
|
||||
.agent-task-list .task-name { font-weight: 600; flex: 0 0 auto; }
|
||||
.agent-task-list .task-desc { color: var(--dim); flex: 1; overflow: hidden; text-overflow: ellipsis; white-space: nowrap; }
|
||||
.agent-task-list button.link-btn { font-size: 11px; padding: 2px 6px; }
|
||||
|
||||
/* ----- Tab Braccio DX ----- */
|
||||
.ra-banner {
|
||||
padding: 8px 12px;
|
||||
|
||||
Reference in new issue
Block a user