diff --git a/config.yaml b/config.yaml index b33d2dd..31820c0 100644 --- a/config.yaml +++ b/config.yaml @@ -20,8 +20,8 @@ services: aruco: dictionary: DICT_4X4_50 wrist_marker: - id: 20 - size_mm: 103.0 + id: 10 + size_mm: 40.0 robot: wrist_side: right active: h1 @@ -53,5 +53,6 @@ chat: api_key_env: OPENROUTER_API_KEY referer: http://localhost:8890 title: presa-mano - user_prompt_addendum: L'operatore preferisce risposte molto telegrafiche, max - 3 righe se possibile. Misure sempre in mm interi. + 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. diff --git a/src/presa_mano/agent/prompt.py b/src/presa_mano/agent/prompt.py index 3fda7c2..99cc041 100644 --- a/src/presa_mano/agent/prompt.py +++ b/src/presa_mano/agent/prompt.py @@ -52,11 +52,25 @@ un robot umanoide (Unitree G1 o H1) con una camera depth Nuwa-HP60C fissa. - 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": diff --git a/src/presa_mano/agent/tools.py b/src/presa_mano/agent/tools.py index f4b24e9..c213fa9 100644 --- a/src/presa_mano/agent/tools.py +++ b/src/presa_mano/agent/tools.py @@ -89,7 +89,10 @@ def _h_set_active_robot(args: dict[str, Any]) -> dict[str, Any]: def _h_device_status(_: dict[str, Any]) -> dict[str, Any]: """Reuses the home_routes implementation to avoid duplicating the ping logic.""" from ..api.home_routes import status as _status - return _status() + try: + return _ok(**_status()) + except Exception as e: + return _err(str(e)) # ----- tool: wrist read/move ------------------------------------------------ @@ -118,11 +121,12 @@ def _h_move_wrist_absolute(args: dict[str, Any]) -> dict[str, Any]: 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", False)) result = _deps.g1(dry_run=dry_run).wrist_absolute( side=side, position=position, quaternion=quaternion, - wait=True, timeout_s=timeout_s, + wait=True, timeout_s=timeout_s, precise=precise, ) - return _ok(dry_run=dry_run, side=side, response=result) + 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: @@ -140,19 +144,34 @@ def _h_move_wrist_relative(args: dict[str, Any]) -> dict[str, Any]: 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", False)) 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, + wait=True, timeout_s=timeout_s, precise=precise, ) - return _ok(dry_run=dry_run, side=side, response=result) + 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() @@ -422,6 +441,7 @@ _HANDLERS: dict[str, Callable[[dict[str, Any]], dict[str, Any]]] = { "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, @@ -479,7 +499,13 @@ TOOL_DEFINITIONS: list[dict[str, Any]] = [ "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.", + "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"], @@ -490,6 +516,8 @@ TOOL_DEFINITIONS: list[dict[str, Any]] = [ "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": False, + "description": "true = controllo accurato (più lento)"}, "timeout_s": {"type": "number", "default": 5.0}, }, }, @@ -499,7 +527,11 @@ TOOL_DEFINITIONS: list[dict[str, Any]] = [ "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.", + "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"], @@ -508,11 +540,27 @@ TOOL_DEFINITIONS: list[dict[str, Any]] = [ "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": False}, "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": { diff --git a/src/presa_mano/clients/g1.py b/src/presa_mano/clients/g1.py index 3065245..5d4add3 100644 --- a/src/presa_mano/clients/g1.py +++ b/src/presa_mano/clients/g1.py @@ -70,8 +70,11 @@ class G1Client: def wrist_relative(self, side: str, delta_position: list[float] | None = None, delta_quaternion: list[float] | None = None, wait: bool = True, - timeout_s: float = 5.0) -> dict[str, Any]: - body: dict[str, Any] = {"side": side, "wait": wait, "timeout_s": timeout_s} + timeout_s: float = 5.0, precise: bool = False) -> dict[str, Any]: + # `precise` is forwarded but the bridge silently ignores it on + # /wrist/relative if not supported — safer to always send it. + body: dict[str, Any] = {"side": side, "wait": wait, "timeout_s": timeout_s, + "precise": precise} if delta_position is not None: body["delta_position"] = delta_position if delta_quaternion is not None: diff --git a/web/app.js b/web/app.js index 82eaee7..faec1c4 100644 --- a/web/app.js +++ b/web/app.js @@ -109,259 +109,6 @@ async function api(method, path, body = null) { return data; } -// ==================== CALIBRAZIONE ==================== -const calibCanvas = $('calibCanvas'); -const calibCtx = calibCanvas.getContext('2d'); -let displayedRgb = null; // last successfully loaded Image (stays on screen) -let rgbInFlight = false; -let previewInFlight = false; -let lastMarker = null; -let lastMarkerTs = 0; -const MARKER_STALE_MS = 1500; // keep overlay this long across hiccups - -function drawCalibFrame() { - if (displayedRgb) { - if (calibCanvas.width !== displayedRgb.naturalWidth) { - calibCanvas.width = displayedRgb.naturalWidth; - calibCanvas.height = displayedRgb.naturalHeight; - } - calibCtx.drawImage(displayedRgb, 0, 0); - } else { - calibCtx.fillStyle = '#000'; - calibCtx.fillRect(0, 0, calibCanvas.width, calibCanvas.height); - calibCtx.fillStyle = '#666'; - calibCtx.font = '14px sans-serif'; - calibCtx.fillText('no RGB', 20, 30); - } - const fresh = lastMarker && lastMarker.found && (performance.now() - lastMarkerTs < MARKER_STALE_MS); - if (fresh) { - const c = lastMarker.corners; - calibCtx.strokeStyle = '#3ecf8e'; - calibCtx.lineWidth = 3; - calibCtx.beginPath(); - calibCtx.moveTo(c[0][0], c[0][1]); - for (let i = 1; i < c.length; i++) calibCtx.lineTo(c[i][0], c[i][1]); - calibCtx.closePath(); - calibCtx.stroke(); - calibCtx.fillStyle = '#3ecf8e'; - c.forEach(p => { calibCtx.beginPath(); calibCtx.arc(p[0], p[1], 4, 0, 2 * Math.PI); calibCtx.fill(); }); - const cx = lastMarker.center_px[0], cy = lastMarker.center_px[1]; - calibCtx.font = 'bold 14px sans-serif'; - const d = lastMarker.pose_camera && lastMarker.pose_camera.distance_m; - calibCtx.fillText(`ID ${lastMarker.marker_id}` + (d ? ` · ${(d*100).toFixed(1)} cm` : ''), cx + 10, cy - 10); - } -} - -function refreshRgb() { - if (rgbInFlight) return; - rgbInFlight = true; - const img = new Image(); - img.onload = () => { - displayedRgb = img; - rgbInFlight = false; - drawCalibFrame(); - }; - img.onerror = () => { rgbInFlight = false; /* keep previous frame */ }; - img.src = `/api/calibration/rgb.jpg?t=${Date.now()}`; -} - -async function previewMarker() { - if (previewInFlight) return; - if (!$('livePreview').checked) { - if (lastMarker) { lastMarker = null; drawCalibFrame(); } - return; - } - previewInFlight = true; - try { - const body = { marker_id: +$('arucoId').value, size_mm: +$('arucoSize').value }; - const data = await api('POST', '/api/calibration/preview', body); - lastMarker = data; - lastMarkerTs = performance.now(); - const info = $('markerInfo'); - const t = fmtTimings(data.timings_ms); - if (data.found) { - const d = data.pose_camera && data.pose_camera.distance_m; - info.textContent = `marker ${data.marker_id} @ ${(d*100).toFixed(1)} cm · ${t}`; - info.style.color = 'var(--ok)'; - } else { - info.textContent = `marker ${data.marker_id} non trovato · ${t}`; - info.style.color = 'var(--warn)'; - } - } catch (e) { - // transient failure — don't wipe the overlay - $('markerInfo').textContent = `preview: ${e.message}`; - $('markerInfo').style.color = 'var(--danger)'; - } finally { - previewInFlight = false; - drawCalibFrame(); - } -} - -// Two independent loops so a slow preview doesn't starve the RGB redraw, -// and a slow RGB doesn't block the marker polling. -setInterval(() => { - if ($('tab-calibration').classList.contains('active')) refreshRgb(); -}, 150); -setInterval(() => { - if ($('tab-calibration').classList.contains('active')) previewMarker(); -}, 500); - -$('sampleBtn').addEventListener('click', async () => { - try { - setStatus('Acquisisco sample…'); - const body = { - marker_id: +$('arucoId').value, - size_mm: +$('arucoSize').value, - side: $('side').value, - }; - const r = await api('POST', '/api/calibration/sample', body); - setStatus(`Sample #${r.total} acquisito · ${fmtTimings(r.timings_ms)}`, 'ok'); - await loadSamples(); - } catch (e) { setStatus(e.message, 'err'); } -}); - -$('solveBtn').addEventListener('click', async () => { - try { - setStatus('Calcolo matrice…'); - const side = $('side').value; - const r = await api('POST', '/api/calibration/solve', { method: $('method').value, side }); - renderCalibResult(r); - setStatus(`Matrice calcolata (${side}) · ${fmtTimings(r.timings_ms)}`, 'ok'); - } catch (e) { setStatus(e.message, 'err'); } -}); - -$('clearSamplesBtn').addEventListener('click', async () => { - const side = $('side').value; - if (!confirm(`Rimuovere tutti i sample del braccio ${side}?`)) return; - await api('DELETE', `/api/calibration/samples?side=${encodeURIComponent(side)}`); - await loadSamples(); -}); - -$('saveConfigBtn').addEventListener('click', async () => { - const patch = { - aruco: { wrist_marker: { id: +$('arucoId').value, size_mm: +$('arucoSize').value } }, - robot: { wrist_side: $('side').value }, - calibration: { method: $('method').value }, - }; - try { - await api('PUT', '/api/config', patch); - setStatus('Configurazione salvata', 'ok'); - } catch (e) { setStatus(e.message, 'err'); } -}); - -// When the user changes the side dropdown, reload per-side samples/result. -$('side').addEventListener('change', async () => { - await loadSamples().catch(() => {}); - await loadCalibResult().catch(() => {}); -}); - -// index -> { reason, detail } for samples the last solve dropped -const lastDrops = new Map(); - -async function loadSamples() { - const side = $('side') ? $('side').value : 'right'; - const r = await api('GET', `/api/calibration/samples?side=${encodeURIComponent(side)}`); - $('sampleCount').textContent = r.total; - const ul = $('sampleList'); - ul.innerHTML = ''; - // Display newest first while keeping the server-side index (used in drop reports). - for (let i = r.samples.length - 1; i >= 0; i--) { - const s = r.samples[i]; - const t = s.T_base_wrist; - const p = [t[0][3], t[1][3], t[2][3]].map(v => v.toFixed(3)).join(', '); - const li = document.createElement('li'); - const drop = lastDrops.get(i); - let line = `#${i+1} side=${s.side} pos=[${p}] ${s.note || ''}`; - if (drop) { - line += ` · scartato (${drop.reason}: ${drop.detail})`; - li.classList.add('sample-dropped'); - } - li.textContent = line; - ul.appendChild(li); - } -} - -function renderCalibResult(r) { - const m = r.T_base_camera; - const fmt = (v) => (v >= 0 ? ' ' : '') + v.toFixed(4); - const rows = m.map(row => row.map(fmt).join(' ')); - $('calibMatrix').textContent = rows.join('\n'); - const meta = [ - `method=${r.method}`, - `n=${r.n_samples}`, - `residual: ${r.residual_translation_mm.toFixed(2)} mm / ${r.residual_rotation_deg.toFixed(2)}°`, - ]; - const sc = r.sanity_check; - lastDrops.clear(); - if (sc && sc.applied) { - const drops = []; - const pa = sc.dropped_by_pair_angle || []; - const wm = sc.dropped_by_wm_cluster || []; - const pq = sc.dropped_by_pose_quality || []; - for (const d of pq) { - const key = d.reprojection_px != null ? 'reproj' : 'ambiguity'; - const val = d.reprojection_px != null ? `${d.reprojection_px.toFixed(2)} px` : `amb ${(d.ambiguity_ratio*100).toFixed(0)}%`; - lastDrops.set(d.sample_index, { reason: key, detail: val }); - } - for (const d of pa) { - lastDrops.set(d.sample_index, - { reason: 'pair-angle', detail: `${d.pair_angle_err_deg.toFixed(1)}°` }); - } - for (const d of wm) { - // A sample can fail both checks; prefer the wm message (happens after solve). - lastDrops.set(d.sample_index, - { reason: 'T_wrist_marker', detail: `${d.wm_translation_dev_mm.toFixed(1)} mm` }); - } - if (pq.length) drops.push(`pose-quality: ${pq.map(d => `#${d.sample_index+1}(${d.reprojection_px != null ? d.reprojection_px.toFixed(2)+'px' : (d.ambiguity_ratio*100).toFixed(0)+'%'})`).join(', ')}`); - if (pa.length) drops.push(`pair-angle: ${pa.map(d => `#${d.sample_index+1}(${d.pair_angle_err_deg.toFixed(1)}°)`).join(', ')}`); - if (wm.length) drops.push(`T_wrist_marker: ${wm.map(d => `#${d.sample_index+1}(${d.wm_translation_dev_mm.toFixed(1)}mm)`).join(', ')}`); - const totalDropped = (sc.dropped_by_pose_quality || []).length + pa.length + wm.length; - if (drops.length) { - meta.push(`\n⚠ scartati ${totalDropped}/${sc.n_input}: ${drops.join(' · ')}`); - } else { - meta.push(`\n✓ nessun outlier (${sc.n_input} sample usati)`); - } - } - $('calibMeta').textContent = meta.join(' '); - $('calibMeta').style.whiteSpace = 'pre-wrap'; - // Refresh the list so dropped rows turn red (if already visible). - loadSamples().catch(() => {}); -} - -async function loadCalibResult() { - const side = $('side') ? $('side').value : 'right'; - try { renderCalibResult(await api('GET', `/api/calibration/result?side=${encodeURIComponent(side)}`)); } catch {} -} - -// auto-grid -$('runGridBtn').addEventListener('click', async () => { - const quat = $('gridQuat').value.split(',').map(Number); - if (quat.length !== 4) { setStatus('quat deve avere 4 valori', 'err'); return; } - const body = { - start_position: [+$('gridX0').value, +$('gridY0').value, +$('gridZ0').value], - orientation_quaternion_xyzw: quat, - step_x_m: +$('gridDX').value, - step_y_m: +$('gridDY').value, - step_z_m: +$('gridDZ').value, - n_x: 3, n_y: 3, n_z: 3, - min_z_m: +$('gridMinZ').value, - settle_s: +$('gridSettle').value, - dry_run: $('gridDryRun').checked, - auto_solve: true, - }; - $('gridLog').textContent = 'run in progress…'; - try { - const r = await api('POST', '/api/calibration/auto_grid', body); - $('gridLog').textContent = JSON.stringify(r, null, 2); - if (r.calibration) renderCalibResult(r.calibration); - await loadSamples(); - setStatus(body.dry_run ? 'auto-grid dry-run completato' : 'auto-grid completato', 'ok'); - } catch (e) { - $('gridLog').textContent = e.message; - setStatus(e.message, 'err'); - } -}); - // ==================== TROVA UTENSILE ==================== const graspCanvas = $('graspCanvas'); const graspCtx = graspCanvas.getContext('2d'); @@ -1374,15 +1121,9 @@ startHomePoll(); (async () => { try { const cfg = await api('GET', '/api/config'); - $('arucoId').value = cfg.aruco.wrist_marker.id; - $('arucoSize').value = cfg.aruco.wrist_marker.size_mm; - $('side').value = cfg.robot.wrist_side; - $('eeSide').value = cfg.robot.wrist_side; + if ($('eeSide')) $('eeSide').value = cfg.robot.wrist_side; } catch {} - await loadSamples(); - await loadCalibResult(); refreshWrist(); - refreshRgb(); })(); // ==================== ASSISTENTE AI ==================== diff --git a/web/index.html b/web/index.html index b78c969..efee306 100644 --- a/web/index.html +++ b/web/index.html @@ -13,7 +13,6 @@ - @@ -176,76 +175,6 @@ - -
-
-
-

Vista camera

-
- -
-
- - - - - - -
-
marker: —
-
-
-

Sample e matrice

-
- - - -
-
-
- Sample acquisiti - 0 -
-
    -
    -
    - Tbase ← camera - 
    (non ancora calcolata)
    -
    -
    -
    -
    - -
    - Auto-grid (future update) — 3×3 × 3 altezze -
    - - - - - - - - - - - -
    - 
    
-  
    
-
    
-
-