Adriano/Presa-Mano
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(pose_3d): rimuovi agent/chat, aggiungi cloud+diagnostics+camera polso

Browse Source 
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 2026-05-27 17:34:42 +02:00
1 parent 9d30073d57
commit 6c2452fe20
20 files changed
+152744 -2701
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

config.yaml
+64 -15
View File
@@ -48,7 +48,10 @@ tab_right_arm:
x: 1 x: 1
y: -1 y: -1
z: -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: camera_offset_wrist_mm:
x: 0.0 x: 0.0
y: 0.0 y: 0.0
@@ -79,7 +82,66 @@ tab_right_arm:
- 0 - 0
pose_3d: pose_3d:
live_max_step_m: 0.15 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: grasp:
approach_offset_m: 0.1 approach_offset_m: 0.1
pre_grasp_clearance_m: 0.05 pre_grasp_clearance_m: 0.05
@@ -103,16 +165,3 @@ calibration:
method: DANIILIDIS method: DANIILIDIS
file: data/calibration.yaml file: data/calibration.yaml
samples_file: data/samples.json 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.
docs/bug_d435i_intrinsics_mismatch.md
+144
View File
@@ -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"
```
src/presa_mano/agent/__init__.py
-10
View File
@@ -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
src/presa_mano/agent/prompt.py
-121
View File
@@ -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"
src/presa_mano/agent/runner.py
-174
View File
@@ -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()
src/presa_mano/agent/tools.py
-732
View File
@@ -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
src/presa_mano/api/__init__.py
+2 -4
View File
@@ -1,9 +1,8 @@
# Load `_deps` first so it is registered as an attribute of this package # 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 # before any of the route modules (which import `from ..api import _deps`)
# in turn imports `from ..api import _deps`) start executing. # start executing.
from . import _deps # noqa: F401 from . import _deps # noqa: F401
from . import ( from . import (
agent_routes,
calibration_routes, calibration_routes,
config_routes, config_routes,
grasp_routes, grasp_routes,
@@ -16,7 +15,6 @@ from . import (
) )
__all__ = [ __all__ = [
"agent_routes",
"calibration_routes", "calibration_routes",
"config_routes", "config_routes",
"grasp_routes", "grasp_routes",
src/presa_mano/api/agent_routes.py
-203
View File
@@ -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)}
src/presa_mano/api/pose_3d_routes.py
+75 -8
View File
@@ -12,6 +12,7 @@ from pydantic import BaseModel, Field
from ..clients import G1Unavailable from ..clients import G1Unavailable
from ..config import get_config, set_config_value from ..config import get_config, set_config_value
from ..geometry import normalize_quaternion from ..geometry import normalize_quaternion
from ..perception.pointcloud import nuwa_to_cv_frame
from ..timing import Stopwatch from ..timing import Stopwatch
from . import _deps from . import _deps
@@ -57,15 +58,14 @@ class GoReq(BaseModel):
@router.get("/state") @router.get("/state")
def get_state() -> dict[str, Any]: def get_state() -> dict[str, Any]:
sw = Stopwatch() sw = Stopwatch()
wrist_payload: dict[str, Any] | None = None
joints_payload: dict[str, float] | None = None
robot_error: str | None = None
try: try:
with sw.step("g1_wrist"): with sw.step("g1_wrist"):
w = _deps.g1().wrist() g1 = _deps.g1()
except G1Unavailable as e: w = g1.wrist()
raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}") wrist_payload = {
cfg = get_config().get("pose_3d", {}) or {}
return {
"robot_active": _deps.active_robot_key(),
"wrist": {
"left": { "left": {
"position": list(w["left_wrist"]["position"]), "position": list(w["left_wrist"]["position"]),
"quaternion": list(w["left_wrist"]["quaternion"]), "quaternion": list(w["left_wrist"]["quaternion"]),
@@ -74,9 +74,31 @@ def get_state() -> dict[str, Any]:
"position": list(w["right_wrist"]["position"]), "position": list(w["right_wrist"]["position"]),
"quaternion": list(w["right_wrist"]["quaternion"]), "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 {}, "poses": cfg.get("named_poses") or {},
"live_max_step_m": float(cfg.get("live_max_step_m", 0.15)), "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(), "timings_ms": sw.as_dict(),
} }
@@ -174,6 +196,51 @@ def delete_pose(name: str) -> dict[str, Any]:
return {"deleted": name} 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") @router.post("/unlock")
def post_unlock() -> dict[str, Any]: def post_unlock() -> dict[str, Any]:
"""Proxy a POST {bridge}/unlock. Recovery rapido da arms: error_timeout.""" """Proxy a POST {bridge}/unlock. Recovery rapido da arms: error_timeout."""
src/presa_mano/clients/g1.py
+7
View File
@@ -58,6 +58,13 @@ class G1Client:
"""Hand finger state. Shape: {'left': {'fingers': {pinky,ring,middle,index,thumb_bend,thumb_rotation}, 'age_s'}, 'right': {...}}.""" """Hand finger state. Shape: {'left': {'fingers': {pinky,ring,middle,index,thumb_bend,thumb_rotation}, 'age_s'}, 'right': {...}}."""
return self._get("/hand") 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 ----- # ----- mutating -----
def wrist_absolute(self, side: str, position: list[float], quaternion: list[float], def wrist_absolute(self, side: str, position: list[float], quaternion: list[float],
src/presa_mano/main.py
-2
View File
@@ -10,7 +10,6 @@ from fastapi.responses import HTMLResponse, JSONResponse
from fastapi.staticfiles import StaticFiles from fastapi.staticfiles import StaticFiles
from .api import ( from .api import (
agent_routes,
calibration_routes, calibration_routes,
config_routes, config_routes,
grasp_routes, grasp_routes,
@@ -39,7 +38,6 @@ app = FastAPI(
app.include_router(config_routes.router, prefix="/api/config", tags=["config"]) 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(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(calibration_routes.router, prefix="/api/calibration", tags=["calibration"])
app.include_router(perception_routes.router, prefix="/api/perception", tags=["perception"]) app.include_router(perception_routes.router, prefix="/api/perception", tags=["perception"])
app.include_router(grasp_routes.router, prefix="/api/grasp", tags=["grasp"]) app.include_router(grasp_routes.router, prefix="/api/grasp", tags=["grasp"])
tests/test_pose_3d_routes.py
+11 -2
View File
@@ -30,17 +30,26 @@ def test_state_returns_both_wrists():
assert body["wrist"]["right"]["position"] == [0.30, -0.20, 0.30] assert body["wrist"]["right"]["position"] == [0.30, -0.20, 0.30]
assert "poses" in body assert "poses" in body
assert "live_max_step_m" in body assert "live_max_step_m" in body
assert "right_wrist_camera" in body
assert body["robot_active"] in ("g1", "h1") 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 from presa_mano.clients import G1Unavailable
with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory: with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
g1 = MagicMock() g1 = MagicMock()
g1.wrist.side_effect = G1Unavailable("connection refused") g1.wrist.side_effect = G1Unavailable("connection refused")
g1_factory.return_value = g1 g1_factory.return_value = g1
r = client.get("/api/pose_3d/state") 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(): def test_apply_dry_run_calls_g1_with_dry_run_true():
web/app.js
-732
View File
@@ -642,392 +642,6 @@ $('unlockBtn').addEventListener('click', async () => {
} catch (e) { $('eeLog').textContent = e.message; } } 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 ==================== // ==================== HOME ====================
// Shows reachability + latency for every upstream microservice and lets the // Shows reachability + latency for every upstream microservice and lets the
// user switch the active robot (G1 ↔ H1). The orchestrator runs the actual // user switch the active robot (G1 ↔ H1). The orchestrator runs the actual
@@ -1136,349 +750,3 @@ startHomePoll();
} catch {} } catch {}
refreshWrist(); 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, '&amp;').replace(/</g, '&lt;').replace(/>/g, '&gt;');
}
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();
}
}));
web/assets/nuwa/NUWA-HP60C-20230222.stp
+151568
View File
File diff suppressed because it is too large. Load diff
web/assets/nuwa/nuwa-hp60c.stl
BIN
View File
Binary file not shown.
web/diagnostic3d.js
-479
View File
@@ -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(); }
}));
web/index.html
+109 -162
View File
@@ -12,11 +12,8 @@
<h1>presa-mano</h1> <h1>presa-mano</h1>
<nav class="tabs"> <nav class="tabs">
<button class="tab active" data-tab="home">Home</button> <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="workplane">Calibrazione Piano</button>
<button class="tab" data-tab="grasp">Cerca modello</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="right-arm">Braccio DX</button>
<button class="tab" data-tab="pose-3d">Pose 3D</button> <button class="tab" data-tab="pose-3d">Pose 3D</button>
<button class="tab" data-tab="ee">Movimento EE</button> <button class="tab" data-tab="ee">Movimento EE</button>
@@ -68,115 +65,6 @@
<pre id="homeLog" class="log small">—</pre> <pre id="homeLog" class="log small">—</pre>
</section> </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 ============================== --> <!-- ============================== CALIBRAZIONE PIANO ============================== -->
<section class="panel" id="tab-workplane"> <section class="panel" id="tab-workplane">
<div class="diag3d-toolbar"> <div class="diag3d-toolbar">
@@ -292,50 +180,6 @@
</div> </div>
</section> </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 ============================== --> <!-- ============================== BRACCIO DX ============================== -->
<section class="panel" id="tab-right-arm"> <section class="panel" id="tab-right-arm">
<div class="ra-grid"> <div class="ra-grid">
@@ -519,15 +363,118 @@
</div> </div>
<div class="p3-section"> <div class="p3-section">
<h3>Pose salvate</h3> <h3>Pose salvate — DX</h3>
<div class="p3-row"> <div class="p3-row">
<select id="p3PoseSelect"><option value="">(seleziona)</option></select> <select id="p3PoseSelectR"><option value="">(seleziona)</option></select>
</div> </div>
<div class="p3-row"> <div class="p3-row">
<button id="p3PoseGoBtn">Vai</button> <button id="p3PoseGoBtnR">Vai</button>
<input type="text" id="p3PoseName" placeholder="nome" maxlength="40"> <input type="text" id="p3PoseNameR" placeholder="nome" maxlength="40">
<button id="p3PoseSaveBtn">Salva</button> <button id="p3PoseSaveBtnR">Salva</button>
<button id="p3PoseDelBtn">✕</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>
</div> </div>
web/pose_3d.css
+8
View File
@@ -85,6 +85,14 @@
align-items: center; align-items: center;
gap: 4px; 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 { #tab-pose-3d .p3-readout input {
width: 60px; width: 60px;
background: #0d1014; background: #0d1014;
web/pose_3d.js
+756 -26
View File
@@ -47,6 +47,147 @@ function makeLabelSprite(text, color = '#ffffff') {
return sprite; 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) { function makeWristMarker(color) {
const group = new THREE.Group(); const group = new THREE.Group();
const sphere = new THREE.Mesh( const sphere = new THREE.Mesh(
@@ -76,6 +217,11 @@ const state = {
live_max_step_m: 0.15, live_max_step_m: 0.15,
robot: null, // URDF robot loaded via urdf-loader (null se non caricato) 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 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) { function checkWorkspace(side, position_m) {
@@ -149,6 +295,17 @@ async function loadG1Urdf(scene) {
state.arm_links_m = measureArmLinkLengths(robot); state.arm_links_m = measureArmLinkLengths(robot);
console.log('[pose_3d] G1 URDF caricato:', robot.name || 'g1', console.log('[pose_3d] G1 URDF caricato:', robot.name || 'g1',
'arm links (m):', state.arm_links_m); '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); resolve(robot);
}, },
undefined, 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) { function applyIKToUrdf(side, target_position, target_quaternion) {
// Aggiorna i joint dell'URDF G1 per posizionare il polso `side` al target. // Aggiorna i joint dell'URDF G1 per posizionare il polso `side` al target.
// No-op se l'URDF non e' caricato. // No-op se l'URDF non e' caricato.
@@ -325,11 +504,18 @@ function initScene() {
scene.add(wrist); scene.add(wrist);
state.arms[side] = wrist; state.arms[side] = wrist;
// Etichetta DX / SX sopra la sfera // Nuwa camera rigida sul polso destro (eye-in-hand): mesh STL, trasformata
const label = makeLabelSprite(side === 'right' ? 'DX' : 'SX', // letta da pose_3d.right_wrist_camera (offset+rpy in wrist frame).
side === 'right' ? '#4da3ff' : '#ff5e7e'); if (side === 'right') {
label.position.set(0, 0, 0.08); // 8cm sopra la sfera (locale al gruppo) loadNuwaMesh(wrist, '/static/assets/nuwa/nuwa-hp60c.stl')
wrist.add(label); .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 // Linea shoulder → wrist
const lineGeom = new THREE.BufferGeometry().setFromPoints([ const lineGeom = new THREE.BufferGeometry().setFromPoints([
@@ -427,15 +613,44 @@ function rpyDegToQuat(roll, pitch, yaw) {
return [qx, qy, qz, qw]; 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 ----- // ----- state apply to scene -----
function updateRealWrists() { function updateRealWrists() {
const tmpV = new THREE.Vector3();
const tmpQ = new THREE.Quaternion();
for (const side of ['right', 'left']) { 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]; const pose = state.current[side];
if (!pose) continue; if (!pose) continue;
const arm = state.arms[side];
arm.position.set(pose.position[0], pose.position[1], pose.position[2]); arm.position.set(pose.position[0], pose.position[1], pose.position[2]);
arm.quaternion.set(pose.quaternion[0], pose.quaternion[1], arm.quaternion.set(pose.quaternion[0], pose.quaternion[1],
pose.quaternion[2], pose.quaternion[3]); 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 // aggiorna line shoulder→wrist
const line = state.links[side]; const line = state.links[side];
const pts = [SHOULDER[side].clone(), arm.position.clone()]; const pts = [SHOULDER[side].clone(), arm.position.clone()];
@@ -547,10 +762,20 @@ let pollTimer = null;
async function pollState() { async function pollState() {
try { try {
const s = await jsonFetch('/api/pose_3d/state'); const s = await jsonFetch('/api/pose_3d/state');
if (s.wrist) {
state.current.right = s.wrist.right; state.current.right = s.wrist.right;
state.current.left = s.wrist.left; state.current.left = s.wrist.left;
}
state.saved_poses = s.poses || {}; state.saved_poses = s.poses || {};
state.live_max_step_m = s.live_max_step_m || 0.15; 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(); updateRealWrists();
syncGhostsToTargetOrReal(); syncGhostsToTargetOrReal();
updateReadout(); updateReadout();
@@ -563,16 +788,21 @@ async function pollState() {
} }
function refreshPoseSelect() { function refreshPoseSelect() {
const sel = $('p3PoseSelect'); // Popola i due dropdown filtrati per side (right → DX, left → SX).
if (!sel) return; for (const side of ['right', 'left']) {
const sel = $(POSE_UI[side].select);
if (!sel) continue;
const cur = sel.value; const cur = sel.value;
sel.innerHTML = '<option value="">(seleziona)</option>'; sel.innerHTML = '<option value="">(seleziona)</option>';
for (const name of Object.keys(state.saved_poses).sort()) { 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'); const opt = document.createElement('option');
opt.value = name; opt.textContent = name; opt.value = name; opt.textContent = name;
sel.appendChild(opt); sel.appendChild(opt);
} }
sel.value = cur; sel.value = cur;
}
} }
// ----- selezione polso via click ----- // ----- selezione polso via click -----
@@ -872,17 +1102,24 @@ function bindJogAutoApply() {
}); });
} }
// ----- named poses ----- // ----- named poses (sezioni separate DX e SX) -----
async function onSavePose() { const POSE_UI = {
const name = ($('p3PoseName').value || '').trim(); 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 (!name) { alert('Inserisci un nome'); return; }
if (!/^[A-Za-z0-9_-]{1,40}$/.test(name)) { if (!/^[A-Za-z0-9_-]{1,40}$/.test(name)) {
alert('Nome: solo [A-Za-z0-9_-], 1-40 caratteri'); alert('Nome: solo [A-Za-z0-9_-], 1-40 caratteri');
return; return;
} }
const side = state.active_side;
const pose = state.target[side] || state.current[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 (state.saved_poses[name]) {
if (!confirm(`Posa "${name}" esiste già — sovrascrivere?`)) return; if (!confirm(`Posa "${name}" esiste già — sovrascrivere?`)) return;
} }
@@ -893,19 +1130,20 @@ async function onSavePose() {
side, position: pose.position, quaternion: pose.quaternion, side, position: pose.position, quaternion: pose.quaternion,
}), }),
}); });
$('p3PoseName').value = ''; $(ui.name).value = '';
await pollState(); // refresh dropdown await pollState(); // refresh dropdown
$('p3PoseSelect').value = name; $(ui.select).value = name;
} catch (e) { } catch (e) {
showError(`Salvataggio fallito: ${e.message}`); showError(`Salvataggio fallito: ${e.message}`);
} }
} }
async function onGoPose() { async function onGoPose(side) {
const name = $('p3PoseSelect').value; const ui = POSE_UI[side];
if (!name) { alert('Seleziona una posa dal dropdown'); return; } const name = $(ui.select).value;
if (!name) { alert(`Seleziona una posa ${ui.label} dal dropdown`); return; }
if (state.mode === 'jog') { 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'); const dry_run = (state.mode === 'editor');
clearError(); clearError();
@@ -921,7 +1159,7 @@ async function onGoPose() {
if (r.reached_target === false || r.stalled === true) { if (r.reached_target === false || r.stalled === true) {
showError(`Vai a "${name}": residual ${(r.position_error_mm||0).toFixed(1)} mm`); showError(`Vai a "${name}": residual ${(r.position_error_mm||0).toFixed(1)} mm`);
} else { } else {
$('p3Status').textContent = `✓ Posa "${name}" applicata` + $('p3Status').textContent = `✓ Posa "${name}" (${ui.label}) applicata` +
(dry_run ? ' (simulato)' : ''); (dry_run ? ' (simulato)' : '');
$('p3Status').className = 'p3-status ok'; $('p3Status').className = 'p3-status ok';
const pose = state.saved_poses[name]; const pose = state.saved_poses[name];
@@ -932,9 +1170,10 @@ async function onGoPose() {
} }
} }
async function onDeletePose() { async function onDeletePose(side) {
const name = $('p3PoseSelect').value; const ui = POSE_UI[side];
if (!name) { alert('Seleziona una posa dal dropdown'); return; } const name = $(ui.select).value;
if (!name) { alert(`Seleziona una posa ${ui.label} dal dropdown`); return; }
if (!confirm(`Cancellare definitivamente la posa "${name}"?`)) return; if (!confirm(`Cancellare definitivamente la posa "${name}"?`)) return;
try { try {
await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, { await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
@@ -947,9 +1186,12 @@ async function onDeletePose() {
} }
function bindPoseButtons() { function bindPoseButtons() {
$('p3PoseSaveBtn').addEventListener('click', onSavePose); for (const side of ['right', 'left']) {
$('p3PoseGoBtn').addEventListener('click', onGoPose); const ui = POSE_UI[side];
$('p3PoseDelBtn').addEventListener('click', onDeletePose); $(ui.save).addEventListener('click', () => onSavePose(side));
$(ui.go).addEventListener('click', () => onGoPose(side));
$(ui.del).addEventListener('click', () => onDeletePose(side));
}
} }
async function onUnlock() { async function onUnlock() {
@@ -967,6 +1209,490 @@ function bindUnlockButton() {
$('p3UnlockBtn').addEventListener('click', onUnlock); $('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 ----- // ----- init triggered on first activation -----
let inited = false; let inited = false;
function ensureInit() { function ensureInit() {
@@ -984,6 +1710,10 @@ function ensureInit() {
bindJogAutoApply(); bindJogAutoApply();
bindPoseButtons(); bindPoseButtons();
bindUnlockButton(); bindUnlockButton();
bindNuwaControls();
bindD435Controls();
bindJointUCSControls();
bindCloudControls();
bindReadoutInputs(); bindReadoutInputs();
pollState(); pollState();
pollTimer = setInterval(pollState, POLL_MS); pollTimer = setInterval(pollState, POLL_MS);
web/style.css
-31
View File
@@ -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); } .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; } .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 ----- */ /* ----- Tab Braccio DX ----- */
.ra-banner { .ra-banner {
padding: 8px 12px; padding: 8px 12px;