diff --git a/config.yaml b/config.yaml index 1311d2f..4285386 100644 --- a/config.yaml +++ b/config.yaml @@ -82,6 +82,8 @@ tab_right_arm: - 0 pose_3d: live_max_step_m: 0.15 + tag_offset: + save_dir: /home/harry/robot/logging/wrist-tag-offset right_wrist_camera: enabled: true mesh_url: /static/assets/nuwa/nuwa-hp60c.stl @@ -107,25 +109,25 @@ pose_3d: d435: offset_m: - -0.01 - - 0 - - 0 + - 0.015 + - -0.005 rpy_deg: - -90 - - 86 + - 90 - 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.03273746934835532 + - -0.9960477706629303 + - -0.0825657111522212 + - -0.00771413467718246 + - - -0.7734929889294071 + - 0.027067579810660346 + - -0.633226612043623 + - 0.37547513731967447 + - - 0.6329588092267278 + - 0.08459423550455474 + - -0.7695498431821574 + - 0.33337005199505626 - - 0.0 - 0.0 - 0.0 @@ -142,6 +144,21 @@ pose_3d: - -0.047703617194205114 - -0.005110044879619669 - 0.9988341941293186 + cloud_params: + step: 2 + max_depth_mm: 1000 + nuwa_size_mm: 5 + d435_size_mm: 5 + d435_scale_xy: 1 + flip_nuwa: + - false + - false + - false + flip_d435: + - false + - false + - false + d435_use_handeye: false grasp: approach_offset_m: 0.1 pre_grasp_clearance_m: 0.05 diff --git a/src/presa_mano/api/pose_3d_routes.py b/src/presa_mano/api/pose_3d_routes.py index e9b9200..c5b7e1e 100644 --- a/src/presa_mano/api/pose_3d_routes.py +++ b/src/presa_mano/api/pose_3d_routes.py @@ -1,8 +1,12 @@ """Endpoint per la tab "Pose 3D": editor visuale + jog dei polsi.""" from __future__ import annotations +import json import logging +import os import re +import time +from pathlib import Path from typing import Any, Literal, Optional import httpx @@ -99,6 +103,7 @@ def get_state() -> dict[str, Any]: "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 {}, + "cloud_params": cfg.get("cloud_params") or {}, "timings_ms": sw.as_dict(), } @@ -229,11 +234,14 @@ def get_clouds(step: int = 8, max_depth_mm: int = 2000) -> dict[str, Any]: except Exception as e: result["nuwa"] = {"error": f"{type(e).__name__}: {e}"} - # D435i: stesso client NuwaClient ma backend RealSense, gia' in OpenCV + # D435i: stesso client NuwaClient (interfaccia uniforme), il servizio + # ritorna però Nuwa-native (X right, Y up, Z back) come per la Nuwa. + # Quindi: stessa conversione → OpenCV optical (X right, Y down, Z forward). 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)} + pts_cv = nuwa_to_cv_frame(pts) + result["d435"] = {"n": int(pts_cv.shape[0]), "points": _pack(pts_cv)} except Exception as e: result["d435"] = {"error": f"{type(e).__name__}: {e}"} @@ -241,6 +249,234 @@ def get_clouds(step: int = 8, max_depth_mm: int = 2000) -> dict[str, Any]: return result +# --- Front-camera tag finder: detect + misura indipendente via depth ---- +# Scansiona ArUco DICT_4X4_50 sulla RGB del D435i frontale e per ogni tag +# trovato misura i 4 corner 3D usando il depth sensor (independent da pnp). +# Output in frame ottico OpenCV (X right, Y down, Z forward) — il client +# lo agganciera' al link URDF del d435 nella scena 3D. + +def _parse_id_range(spec: str) -> list[int]: + """Accetta "0-49" o "1,10,20,33".""" + spec = (spec or "").strip() + if not spec: + return list(range(50)) + out: list[int] = [] + for part in spec.split(","): + part = part.strip() + if "-" in part: + a, b = part.split("-", 1) + out.extend(range(int(a), int(b) + 1)) + else: + out.append(int(part)) + return sorted(set(out)) + + +@router.get("/front_tags") +def get_front_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str, Any]: + """Detect ArUco markers via front D435i + misura geometria 3D reale. + + Per ogni tag trovato chiama /api/measure ai 4 corner per ottenere i punti + 3D direttamente dal depth sensor (indipendente da solvePnP). Cosi' la + dimensione misurata non dipende dalla size assunta in input. + + `probe_size_mm` viene passato a marker-finder solo per soddisfare la sua + API (richiede un size per ogni id) ma non influisce sulla misura. + """ + sw = Stopwatch() + front = _deps.front_camera() + mk = _deps.marker() + + # 1) intrinsics RGB del D435i → passate a marker-finder per pose corrette + with sw.step("intrinsics"): + intr = front.intrinsics() + rgb = (intr or {}).get("rgb") or {} + if not rgb or "fx" not in rgb: + raise HTTPException(500, detail=f"front camera intrinsics missing rgb: {intr!r}") + K = { + "camera_matrix": [ + [float(rgb["fx"]), 0.0, float(rgb["cx"])], + [0.0, float(rgb["fy"]), float(rgb["cy"])], + [0.0, 0.0, 1.0], + ], + "dist_coeffs": [ + float((rgb.get("distortion") or {}).get(k, 0.0)) + for k in ("k1", "k2", "p1", "p2", "k3") + ], + } + + # 2) detect (passiamo intrinsics override per non usare il default Nuwa) + id_list = _parse_id_range(ids) + markers = [{"id": int(i), "size_mm": float(probe_size_mm)} for i in id_list] + with sw.step("detect"): + det = mk.detect_url(markers=markers, image_url=front.rgb_url(), intrinsics=K) + + # 3) per ogni found: depth ai 4 corner + center + tags_out: list[dict[str, Any]] = [] + with sw.step("measure_corners"): + for m in det.get("found", []): + corners = m.get("corners") or [] + if len(corners) != 4: + continue + pts3d: list[list[float]] = [] + depths: list[Optional[int]] = [] + try: + for (u, v) in corners: + md = front.measure(int(round(u)), int(round(v)), radius=2) + pt = (md or {}).get("point_m") or {} + d_mm = (md or {}).get("depth_mm") + if not pt or d_mm in (None, 0): + pts3d.append([float("nan")] * 3) + depths.append(None) + else: + pts3d.append([float(pt["x"]), float(pt["y"]), float(pt["z"])]) + depths.append(int(d_mm)) + cu = sum(c[0] for c in corners) / 4.0 + cv = sum(c[1] for c in corners) / 4.0 + cmd = front.measure(int(round(cu)), int(round(cv)), radius=4) + cpt = (cmd or {}).get("point_m") or {} + center3d = ( + [float(cpt["x"]), float(cpt["y"]), float(cpt["z"])] + if cpt else [float("nan")] * 3 + ) + except Exception as e: + log.warning("front_tags: measure failed for id=%s: %s", m.get("id"), e) + continue + + # Outlier rejection sulla depth: il pixel di un corner sul bordo + # del marker spesso prende la depth del background (es. 1358mm + # mentre gli altri 3 sono a 320mm). Rifiutiamo corner con depth + # > 20% off dalla mediana → li trattiamo come NaN nel calcolo edge. + valid_d = [d for d in depths if d is not None] + if len(valid_d) >= 3: + # mediana semplice (4 elementi → media dei 2 centrali) + sd = sorted(valid_d) + median_d = (sd[len(sd)//2 - 1] + sd[len(sd)//2]) / 2 if len(sd) % 2 == 0 else sd[len(sd)//2] + for i, d in enumerate(depths): + if d is None: + continue + if abs(d - median_d) / median_d > 0.20: + pts3d[i] = [float("nan")] * 3 # esclude da edge_mm + + # 3D edge lengths (mm) — robusti a un corner senza depth (NaN salta) + def _L(a: list[float], b: list[float]) -> Optional[float]: + if any(x != x for x in a + b): # NaN check + return None + return (sum((a[i] - b[i]) ** 2 for i in range(3))) ** 0.5 * 1000.0 + + edges_mm = [_L(pts3d[i], pts3d[(i + 1) % 4]) for i in range(4)] + valid_edges = [e for e in edges_mm if e is not None] + edge_mean = sum(valid_edges) / len(valid_edges) if valid_edges else None + + distance_m: Optional[float] = None + if all(x == x for x in center3d): + distance_m = (sum(c ** 2 for c in center3d)) ** 0.5 + + tags_out.append({ + "id": int(m["id"]), + "corners_pixel": [[float(c[0]), float(c[1])] for c in corners], + "corners_3d_optical_m": pts3d, + "corner_depths_mm": depths, + "center_3d_optical_m": center3d, + "edge_mm_per_side": edges_mm, + "edge_mm": edge_mean, + "distance_m": distance_m, + }) + + result = { + "tags": tags_out, + "not_found_count": int(len(det.get("not_found", []))), + "frame": "d435_rgb_optical", # OpenCV: X right, Y down, Z forward + "timings_ms": sw.as_dict(), + } + return result + + +# --- Tag locator: piazza crocetta nella scena 3D e salva la posa in pelvis --- +# Workflow utente (vedi web/pose_3d.js sezione "tag locator"): +# 1) seleziona tag da dropdown (id+size+label) +# 2) click nel viewport → crocetta a quel punto +# 3) "Salva" → POST qui → scrive current.json + ruota archive +# TODO(orientation): per ora salviamo solo CdM; orientamento non catturato. + +_TAG_OFFSET_DEFAULT_DIR = "/home/harry/robot/logging/wrist-tag-offset" + + +def _tag_offset_dir() -> Path: + """Letto da config.yaml pose_3d.tag_offset.save_dir; ricaricato ad ogni call + cosi' un PUT /api/config a caldo prende effetto subito.""" + cfg = (get_config().get("pose_3d", {}) or {}).get("tag_offset", {}) or {} + return Path(cfg.get("save_dir") or _TAG_OFFSET_DEFAULT_DIR) + + +class TagOffsetSaveReq(BaseModel): + tag_id: int + tag_size_mm: float + tag_label: Optional[str] = None + position_pelvis_m: list[float] = Field(min_length=3, max_length=3) + # Snapshot dei giunti reali al click (name → radianti). Null se G1 offline. + joints_at_save_rad: Optional[dict[str, float]] = None + note: Optional[str] = None + + +@router.post("/tag_offset/save") +def post_tag_offset_save(req: TagOffsetSaveReq) -> dict[str, Any]: + sw = Stopwatch() + ts = time.strftime("%Y%m%d_%H%M%S") + payload = { + "tag_id": int(req.tag_id), + "tag_size_mm": float(req.tag_size_mm), + "tag_label": req.tag_label, + "position_pelvis_m": [float(v) for v in req.position_pelvis_m], + "joints_at_save_rad": req.joints_at_save_rad, + "frame": "pelvis", + "method": "manual_3d_click", + "timestamp": ts, + "note": req.note, + } + base = _tag_offset_dir() + archive_dir = base / "archive" + current_path = base / "current.json" + archived_from: Optional[str] = None + try: + with sw.step("mkdir"): + archive_dir.mkdir(parents=True, exist_ok=True) + with sw.step("archive"): + if current_path.exists(): + old = json.loads(current_path.read_text()) + old_ts = old.get("timestamp") or ts + dest = archive_dir / f"{old_ts}.json" + # se collide, suffisso numerico + i = 1 + while dest.exists(): + dest = archive_dir / f"{old_ts}_{i}.json" + i += 1 + current_path.rename(dest) + archived_from = str(dest) + with sw.step("write"): + current_path.write_text(json.dumps(payload, indent=2)) + except OSError as e: + raise HTTPException(status_code=500, detail=f"tag_offset save fail: {e}") + return { + "ok": True, + "path": str(current_path), + "archived_from": archived_from, + "saved": payload, + "timings_ms": sw.as_dict(), + } + + +@router.get("/tag_offset") +def get_tag_offset() -> dict[str, Any]: + """Restituisce il current.json se esiste, altrimenti 404.""" + current_path = _tag_offset_dir() / "current.json" + if not current_path.exists(): + raise HTTPException(status_code=404, detail="no tag offset saved yet") + try: + return json.loads(current_path.read_text()) + except (OSError, json.JSONDecodeError) as e: + raise HTTPException(status_code=500, detail=f"read fail: {e}") + + @router.post("/unlock") def post_unlock() -> dict[str, Any]: """Proxy a POST {bridge}/unlock. Recovery rapido da arms: error_timeout.""" diff --git a/src/presa_mano/main.py b/src/presa_mano/main.py index b4487e4..7b3f455 100644 --- a/src/presa_mano/main.py +++ b/src/presa_mano/main.py @@ -107,7 +107,8 @@ def index() -> HTMLResponse: idx = WEB_DIR / "index.html" html = idx.read_text(encoding="utf-8") # Cache-bust app.js and style.css with their mtime so a rebuild forces refetch. - for name in ("app.js", "work_plane.js", "right_arm.js", "style.css"): + for name in ("app.js", "work_plane.js", "right_arm.js", "pose_3d.js", + "style.css", "pose_3d.css"): f = WEB_DIR / name if f.exists(): v = int(f.stat().st_mtime) diff --git a/web/index.html b/web/index.html index 662c436..86564cc 100644 --- a/web/index.html +++ b/web/index.html @@ -320,6 +320,7 @@
click polso = seleziona · gizmo: T sposta / R ruota · drag = muovi · orbit = wheel/drag fondo · pan = shift+drag
+ diff --git a/web/pose_3d.css b/web/pose_3d.css index 534eb87..898b512 100644 --- a/web/pose_3d.css +++ b/web/pose_3d.css @@ -1,11 +1,40 @@ #tab-pose-3d .p3-layout { display: grid; - grid-template-columns: 1fr 360px; + grid-template-columns: 1fr 360px 92px; gap: 16px; height: calc(100vh - 140px); min-height: 600px; } +#tab-pose-3d .p3-tabrail { + display: flex; + flex-direction: column; + gap: 8px; + align-items: stretch; +} +#tab-pose-3d .p3-tab { + background: #1a1f26; + border: 1px solid #2a2f36; + color: #aab; + padding: 14px 8px; + border-radius: 6px; + font-size: 12px; + font-weight: bold; + letter-spacing: 1px; + cursor: pointer; + writing-mode: horizontal-tb; + text-align: center; +} +#tab-pose-3d .p3-tab:hover { background: #232a33; color: #e0e0e0; } +#tab-pose-3d .p3-tab.active { + background: #1f3556; + border-color: #4da3ff; + color: #4da3ff; +} + +#tab-pose-3d .p3-group { display: contents; } +#tab-pose-3d .p3-group[hidden] { display: none; } + #tab-pose-3d .p3-viewport { position: relative; background: #0d1014; diff --git a/web/pose_3d.js b/web/pose_3d.js index 52c0927..b8f2d51 100644 --- a/web/pose_3d.js +++ b/web/pose_3d.js @@ -222,6 +222,15 @@ const state = { 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 + tag_locator: { // sezione "Tag locator" nella sidebar Pose 3D + marker: null, // THREE.LineSegments (crocetta XYZ) o null + position_m: null, // [x,y,z] in pelvis frame della crocetta corrente + joints_at_place: null, // snapshot giunti al click (può essere null se G1 offline) + }, + front_tags: { // overlay tag visibili dalla camera D435i frontale + group: null, // THREE.Group child del d435 parent (cloud-like transform) + in_flight: false, // evita richieste sovrapposte se il fetch supera il poll + }, }; function checkWorkspace(side, position_m) { @@ -773,8 +782,10 @@ async function pollState() { applyNuwaTransform(); updateNuwaInputsFromCfg(); updateD435InputsFromCfg(); + applyCloudParamsFromCfg(s.cloud_params); applyCalibratedD435UCS(); // Applica TUTTI i giunti reali sul modello URDF (29 motori G1) + state.joints = s.joints || null; // snapshot per tag locator save if (s.joints) applyJointsToUrdf(s.joints); updateRealWrists(); syncGhostsToTargetOrReal(); @@ -785,6 +796,12 @@ async function pollState() { // soft fail: log + segna lo stato console.warn('pose_3d /state fail:', e.message); } + // Front-camera tag overlay: piggyback sul ciclo poll dello state, no + // timer separato. Se il flag e' on aggiorniamo il rendering ad ogni tick. + if ($('p3ShowFrontTags')?.checked && !state.front_tags.in_flight) { + state.front_tags.in_flight = true; + pollFrontTags().finally(() => { state.front_tags.in_flight = false; }); + } } function refreshPoseSelect() { @@ -806,6 +823,203 @@ function refreshPoseSelect() { } // ----- selezione polso via click ----- +// ----- Tag locator ----- +// Workflow: +// 1) flagga "Modalità tag click" → entra in placement mode (click su URDF +// mesh piazza la crocetta, click su sfere polso disabilitato). +// 2) seleziona tag dal dropdown. +// 3) click sulla mesh URDF → crocetta arancione + gizmo XYZ agganciato per +// fine-tuning lungo gli assi pelvis. +// 4) bottone "Salva" → POST /api/pose_3d/tag_offset/save (pelvis frame). +// TODO(orientation): per ora salviamo solo CdM del tag. Estensione futura: +// catturare anche orientamento via gizmo di rotazione sulla crocetta, oppure +// con 3-click (normale + up). +const TAG_OPTIONS = { + '10:40:wrist_sx': { tag_id: 10, tag_size_mm: 40, tag_label: 'wrist_sx' }, + '1:20:nuwa_dx': { tag_id: 1, tag_size_mm: 20, tag_label: 'nuwa_dx' }, +}; + +function getActiveTagInfo() { + const v = $('p3TagSelect')?.value || '10:40:wrist_sx'; + return TAG_OPTIONS[v] || TAG_OPTIONS['10:40:wrist_sx']; +} + +function isTagPlacementMode() { + return $('p3TagPlaceMode')?.checked === true; +} + +function makeTagMarker(sizeMm) { + // 3 segmenti XYZ centrati. Lunghezza = tag_size così la crocetta scala col tag. + const halfM = (sizeMm / 1000) / 2; + const geom = new THREE.BufferGeometry(); + const verts = new Float32Array([ + -halfM, 0, 0, +halfM, 0, 0, + 0, -halfM, 0, 0, +halfM, 0, + 0, 0, -halfM, 0, 0, +halfM, + ]); + geom.setAttribute('position', new THREE.BufferAttribute(verts, 3)); + // Arancione brillante, renderOrder alto + depthTest off → sempre visibile. + const mat = new THREE.LineBasicMaterial({ color: 0xff8800, depthTest: false }); + const seg = new THREE.LineSegments(geom, mat); + seg.renderOrder = 999; + return seg; +} + +function attachGizmoToTagMarker() { + if (!state.tag_locator.marker) return; + const { transform } = state.three; + transform.setMode('translate'); + transform.attach(state.tag_locator.marker); +} + +function placeTagMarker(positionWorld, sizeMm) { + if (state.tag_locator.marker) { + if (state.three.transform.object === state.tag_locator.marker) { + state.three.transform.detach(); + } + state.three.scene.remove(state.tag_locator.marker); + state.tag_locator.marker.geometry.dispose(); + state.tag_locator.marker.material.dispose(); + } + const m = makeTagMarker(sizeMm); + m.position.copy(positionWorld); + state.three.scene.add(m); + state.tag_locator.marker = m; + // Snapshot giunti al momento del click (resta valido anche dopo gizmo-drag). + state.tag_locator.joints_at_place = state.joints ? { ...state.joints } : null; + updateTagMarkerReadout(); + const sv = $('p3TagSaveBtn'); if (sv) sv.disabled = false; + const cv = $('p3TagClearBtn'); if (cv) cv.disabled = false; + const st = $('p3TagStatus'); + if (st) { + const qInfo = state.tag_locator.joints_at_place + ? ` · q snapshot=${Object.keys(state.tag_locator.joints_at_place).length} giunti` + : ' · ⚠ G1 offline → niente q snapshot'; + st.textContent = 'crocetta piazzata' + qInfo + ' · gizmo per fine-tune · Salva quando ok'; + st.style.color = ''; + } + attachGizmoToTagMarker(); +} + +function updateTagMarkerReadout() { + if (!state.tag_locator.marker) return; + const p = state.tag_locator.marker.position; + state.tag_locator.position_m = [p.x, p.y, p.z]; + $('p3TagPosX').value = (p.x * 1000).toFixed(1); + $('p3TagPosY').value = (p.y * 1000).toFixed(1); + $('p3TagPosZ').value = (p.z * 1000).toFixed(1); +} + +function tryPlaceTagFromClick(raycaster) { + if (!state.robot) { + $('p3TagStatus').textContent = 'URDF non ancora caricata, riprova tra un attimo'; + $('p3TagStatus').style.color = 'var(--danger)'; + return false; + } + const hits = raycaster.intersectObject(state.robot, true); + const meshHit = hits.find(h => h.object && h.object.isMesh && h.object.visible); + if (!meshHit) { + $('p3TagStatus').textContent = 'click fuori dalla mesh del robot · clicca direttamente sul modello'; + $('p3TagStatus').style.color = 'var(--dim)'; + return false; + } + const info = getActiveTagInfo(); + placeTagMarker(meshHit.point, info.tag_size_mm); + return true; +} + +function clearTagMarker() { + if (!state.tag_locator.marker) return; + if (state.three.transform.object === state.tag_locator.marker) { + state.three.transform.detach(); + } + state.three.scene.remove(state.tag_locator.marker); + state.tag_locator.marker.geometry.dispose(); + state.tag_locator.marker.material.dispose(); + state.tag_locator.marker = null; + state.tag_locator.position_m = null; + state.tag_locator.joints_at_place = null; + for (const id of ['p3TagPosX','p3TagPosY','p3TagPosZ']) { const el = $(id); if (el) el.value = ''; } + $('p3TagSaveBtn').disabled = true; + $('p3TagClearBtn').disabled = true; + $('p3TagStatus').textContent = isTagPlacementMode() + ? 'click sul modello 3D per piazzare la crocetta' + : 'attiva la modalità tag click sopra, poi clicca sul modello 3D'; + $('p3TagStatus').style.color = ''; +} + +async function onTagSave() { + if (!state.tag_locator.position_m) return; + const info = getActiveTagInfo(); + const sv = $('p3TagSaveBtn'); + const st = $('p3TagStatus'); + if (sv) sv.disabled = true; + if (st) { st.textContent = 'salvataggio…'; st.style.color = 'var(--dim)'; } + try { + const res = await jsonFetch('/api/pose_3d/tag_offset/save', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ + tag_id: info.tag_id, + tag_size_mm: info.tag_size_mm, + tag_label: info.tag_label, + position_pelvis_m: state.tag_locator.position_m, + joints_at_save_rad: state.tag_locator.joints_at_place, + }), + }); + if (st) { + const arch = res.archived_from ? ' · prev → archive' : ''; + const qBit = state.tag_locator.joints_at_place + ? ` · q=${Object.keys(state.tag_locator.joints_at_place).length} giunti` + : ' · ⚠ niente q (G1 offline)'; + st.textContent = `salvato${qBit}${arch}`; + st.style.color = 'var(--ok)'; + } + } catch (e) { + if (st) { st.textContent = `errore: ${e.message}`; st.style.color = 'var(--danger)'; } + } finally { + if (sv) sv.disabled = false; + } +} + +function setupTagLocator() { + // Toggle modalita' tag click → aggiorna stato + status testo, e aggancia + // / sgancia il gizmo dalla crocetta esistente. + $('p3TagPlaceMode')?.addEventListener('change', () => { + if (isTagPlacementMode()) { + $('p3TagStatus').textContent = state.tag_locator.marker + ? 'modalità attiva · click per spostare la crocetta · gizmo per fine-tuning' + : 'modalità attiva · click sul modello 3D per piazzare'; + $('p3TagStatus').style.color = 'var(--ok)'; + if (state.tag_locator.marker) attachGizmoToTagMarker(); + } else { + $('p3TagStatus').textContent = 'modalità disattivata · attiva sopra per piazzare'; + $('p3TagStatus').style.color = ''; + // se il gizmo era sulla crocetta, lascialo libero (l'utente magari vuole + // tornare a editare il polso senza dover ri-cliccare). + if (state.three.transform.object === state.tag_locator.marker) { + state.three.transform.detach(); + } + } + }); + // Cambio tag → ricreo crocetta con nuova dimensione, mantenendo link. + $('p3TagSelect')?.addEventListener('change', () => { + if (state.tag_locator.marker && state.tag_locator.position_m) { + const info = getActiveTagInfo(); + const pos = new THREE.Vector3(...state.tag_locator.position_m); + placeTagMarker(pos, info.tag_size_mm, state.tag_locator.link_object); + } + }); + $('p3TagSaveBtn')?.addEventListener('click', onTagSave); + $('p3TagClearBtn')?.addEventListener('click', clearTagMarker); + // Listener gizmo: aggiorna lettura X/Y/Z quando trascini la crocetta lungo gli assi. + state.three.transform.addEventListener('change', () => { + if (state.three.transform.object === state.tag_locator.marker) { + updateTagMarkerReadout(); + } + }); +} + function setupSelection() { const { renderer, camera, transform } = state.three; const raycaster = new THREE.Raycaster(); @@ -819,7 +1033,14 @@ function setupSelection() { mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1; mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1; raycaster.setFromCamera(mouse, camera); - // Verifica intersezione con sphere destra o sinistra + // Tag-click mode ATTIVA → click su URDF mesh piazza la crocetta. + // In questa modalita' i click sulle sfere polso restano disabilitati per + // non confondere il workflow (l'utente sa che e' in placement mode). + if (isTagPlacementMode()) { + tryPlaceTagFromClick(raycaster); + return; + } + // Modalita' normale: click sulle sfere polso → attiva il gizmo sul ghost const candidates = [state.arms.right.userData.sphere, state.arms.left.userData.sphere]; const hits = raycaster.intersectObjects(candidates, false); @@ -894,6 +1115,12 @@ function setGizmoMode(mode) { function bindGizmoEvents() { const { transform } = state.three; transform.addEventListener('change', () => { + // Guardia: questo listener gestisce solo le pose dei polsi (ghost). + // Quando il gizmo e' attaccato alla crocetta tag locator, c'e' un + // listener separato in setupTagLocator() che fa l'update giusto. + const attached = transform.object; + if (!attached) return; + if (attached !== state.ghosts.right && attached !== state.ghosts.left) return; const side = state.active_side; const ghost = state.ghosts[side]; if (!ghost) return; @@ -1453,6 +1680,64 @@ function readCloudScaleVec(camKey) { return [f[0] * sc[0], f[1] * sc[1], f[2] * sc[2]]; } +// Snapshot dei parametri UI Cloud cross-camera. Mantenuto separato da +// acquireClouds() perche' usato anche dal save persistente (memorizza la +// configurazione attiva all'ultima Acquisisci). +function readCloudParamsFromUI() { + return { + step: parseInt($('p3CloudStep').value, 10) || 8, + max_depth_mm: parseInt($('p3CloudMaxDepth').value, 10) || 2000, + nuwa_size_mm: parseFloat($('p3NuwaCloudSize').value) || 5, + d435_size_mm: parseFloat($('p3D435CloudSize').value) || 5, + d435_scale_xy: parseFloat($('p3D435ScaleXY').value) || 1.0, + flip_nuwa: [ + $('p3FlipNuwaX').checked, + $('p3FlipNuwaY').checked, + $('p3FlipNuwaZ').checked, + ], + flip_d435: [ + $('p3FlipD435X').checked, + $('p3FlipD435Y').checked, + $('p3FlipD435Z').checked, + ], + d435_use_handeye: $('p3D435UseHandeye').checked, + }; +} + +async function saveCloudParamsToConfig() { + const params = readCloudParamsFromUI(); + try { + const r = await fetch('/api/config', { + method: 'PUT', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ pose_3d: { cloud_params: params } }), + }); + if (!r.ok) throw new Error(`HTTP ${r.status}`); + } catch (e) { + console.warn('[pose_3d] save cloud_params failed:', e?.message || e); + } +} + +// Popola UI Cloud cross-camera dai params salvati. Eseguito una volta sola +// (state.cloud_params_populated) per non sovrascrivere edit utente in corso. +function applyCloudParamsFromCfg(cloud_params) { + if (state.cloud_params_populated) return; + if (!cloud_params || Object.keys(cloud_params).length === 0) return; + const set = (id, val) => { const el = $(id); if (el && val !== undefined && val !== null) el.value = String(val); }; + const setCb = (id, val) => { const el = $(id); if (el && typeof val === 'boolean') el.checked = val; }; + set('p3CloudStep', cloud_params.step); + set('p3CloudMaxDepth', cloud_params.max_depth_mm); + set('p3NuwaCloudSize', cloud_params.nuwa_size_mm); + set('p3D435CloudSize', cloud_params.d435_size_mm); + set('p3D435ScaleXY', cloud_params.d435_scale_xy); + const fn = cloud_params.flip_nuwa || []; + setCb('p3FlipNuwaX', fn[0]); setCb('p3FlipNuwaY', fn[1]); setCb('p3FlipNuwaZ', fn[2]); + const fd = cloud_params.flip_d435 || []; + setCb('p3FlipD435X', fd[0]); setCb('p3FlipD435Y', fd[1]); setCb('p3FlipD435Z', fd[2]); + setCb('p3D435UseHandeye', cloud_params.d435_use_handeye); + state.cloud_params_populated = true; +} + async function acquireClouds() { // Clamp dei parametri: step oltre ~32 fa 422 sul server pointcloud. let step = parseInt($('p3CloudStep').value, 10); @@ -1481,6 +1766,9 @@ async function acquireClouds() { } $('p3CloudStatus').textContent = parts.join(' · '); $('p3CloudStatus').style.color = 'var(--ok)'; + // Memorizza i parametri usati in questa acquisizione (best-effort, + // non blocca il rendering del cloud se il save fallisce). + saveCloudParamsToConfig(); } catch (e) { $('p3CloudStatus').textContent = `errore: ${e.message}`; $('p3CloudStatus').style.color = 'var(--danger)'; @@ -1500,6 +1788,163 @@ function clearClouds() { $('p3CloudStatus').style.color = ''; } +// ----- Front-camera tag finder ----- +// Overlay sulla scena 3D dei marker ArUco visibili dal D435i frontale. +// Per ogni tag: LineLoop dei 4 corner + sprite label con id, edge_mm +// (misurato dal depth sensor, non da pnp) e distanza camera→centro. +// Stesso parent + transform del d435 cloud, cosi' l'overlay segue la +// camera in modalita' URDF link e in modalita' hand-eye. + +const FRONT_TAG_COLOR = 0x4dd0e1; +const FRONT_TAG_LABEL_COLOR = '#4dd0e1'; + +// Label compatta auto-sized: canvas largo quanto il testo, font piccolo, +// sprite proporzionato al canvas. Evita il clipping di makeLabelSprite che +// usa canvas fisso 128x64 con font 36px (i nostri "id=20 · 101.4mm · ..." +// uscivano dal box). +function makeCompactLabel(text, color = FRONT_TAG_LABEL_COLOR) { + const fontPx = 24; + const padX = 8, padY = 4; + // Misura testo su un canvas temporaneo + const probe = document.createElement('canvas').getContext('2d'); + probe.font = `${fontPx}px sans-serif`; + const textW = Math.ceil(probe.measureText(text).width); + const w = textW + padX * 2; + const h = fontPx + padY * 2; + const canvas = document.createElement('canvas'); + canvas.width = w; canvas.height = h; + const ctx = canvas.getContext('2d'); + ctx.fillStyle = 'rgba(0,0,0,0.65)'; + ctx.fillRect(0, 0, w, h); + ctx.fillStyle = color; + ctx.font = `${fontPx}px sans-serif`; + ctx.textAlign = 'center'; + ctx.textBaseline = 'middle'; + ctx.fillText(text, w / 2, h / 2); + const tex = new THREE.CanvasTexture(canvas); + const mat = new THREE.SpriteMaterial({ map: tex, depthTest: false }); + const sprite = new THREE.Sprite(mat); + // Scala in metri: ~4 cm di altezza, larghezza proporzionale all'aspect + const targetH = 0.04; + sprite.scale.set(targetH * (w / h), targetH, 1); + return sprite; +} + +function applyCloudLikeTransform(obj, camKey) { + const sv = readCloudScaleVec(camKey); + if (camKey === 'd435' && useD435Handeye()) { + const M = getD435HandeyeMatrix(); + if (M) { + obj.matrixAutoUpdate = false; + const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]); + obj.matrix.copy(M).multiply(S); + obj.matrixWorldNeedsUpdate = true; + return; + } + } + obj.matrixAutoUpdate = true; + const t = getCloudTransform(camKey); + obj.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', + ); + obj.quaternion.setFromEuler(e); + obj.scale.set(sv[0], sv[1], sv[2]); +} + +function clearFrontTagsGroup() { + const grp = state.front_tags.group; + if (!grp) return; + if (grp.parent) grp.parent.remove(grp); + grp.traverse(o => { + if (o.geometry) o.geometry.dispose(); + if (o.material) { + if (Array.isArray(o.material)) o.material.forEach(m => m.dispose()); + else o.material.dispose(); + } + }); + state.front_tags.group = null; +} + +function addOneFrontTag(grp, t) { + const c = t.corners_3d_optical_m; + if (!Array.isArray(c) || c.length !== 4) return; + if (c.some(p => !p || p.some(v => !Number.isFinite(v)))) return; + + // LineLoop (5 verts = close) + const v = new Float32Array(5 * 3); + for (let i = 0; i < 4; i++) { v[3*i]=c[i][0]; v[3*i+1]=c[i][1]; v[3*i+2]=c[i][2]; } + v[12] = c[0][0]; v[13] = c[0][1]; v[14] = c[0][2]; + const geom = new THREE.BufferGeometry(); + geom.setAttribute('position', new THREE.BufferAttribute(v, 3)); + const mat = new THREE.LineBasicMaterial({ color: FRONT_TAG_COLOR }); + grp.add(new THREE.Line(geom, mat)); + + // Label "id=X · NNmm · d=DDDmm" leggermente offset dal centro + const edge_s = Number.isFinite(t.edge_mm) ? `${t.edge_mm.toFixed(1)}mm` : '?'; + const dist_s = Number.isFinite(t.distance_m) ? `${(t.distance_m * 1000).toFixed(0)}mm` : '?'; + const sprite = makeCompactLabel(`id=${t.id} · ${edge_s} · d=${dist_s}`); + const ctr = t.center_3d_optical_m; + if (ctr && ctr.every(Number.isFinite)) { + // offset in -Y ottico = sopra (Y down nella convenzione OpenCV) + sprite.position.set(ctr[0], ctr[1] - 0.04, ctr[2]); + } + grp.add(sprite); +} + +function renderFrontTags(tags) { + clearFrontTagsGroup(); + if (!tags || tags.length === 0) return; + const parent = getCloudParent('d435'); + if (!parent) { + console.warn('[pose_3d] front_tags: parent d435 non disponibile'); + return; + } + const grp = new THREE.Group(); + grp.name = 'front_tags_group'; + applyCloudLikeTransform(grp, 'd435'); + parent.add(grp); + state.front_tags.group = grp; + for (const t of tags) addOneFrontTag(grp, t); +} + +async function pollFrontTags() { + const ids = ($('p3FrontTagIds')?.value || '0-49').trim(); + const status = $('p3FrontTagStatus'); + try { + const r = await jsonFetch(`/api/pose_3d/front_tags?ids=${encodeURIComponent(ids)}`); + const tags = r.tags || []; + renderFrontTags(tags); + if (status) { + const ms = r.timings_ms?.total; + status.textContent = tags.length + ? `${tags.length} tag · ids: ${tags.map(t => t.id).join(',')}${ms != null ? ' · ' + ms.toFixed(0) + 'ms' : ''}` + : 'nessun tag visibile'; + } + } catch (e) { + if (status) status.textContent = `errore: ${e?.message || e}`; + } +} + +function bindFrontTagFinder() { + const cb = $('p3ShowFrontTags'); + if (!cb) return; + cb.addEventListener('change', () => { + if (cb.checked) { + // Refresh immediato senza aspettare il prossimo tick di pollState + pollFrontTags(); + } else { + clearFrontTagsGroup(); + const s = $('p3FrontTagStatus'); + if (s) s.textContent = '—'; + } + }); +} + + function bindCloudControls() { $('p3CloudAcquireBtn')?.addEventListener('click', acquireClouds); $('p3CloudClearBtn')?.addEventListener('click', clearClouds); @@ -1693,6 +2138,22 @@ function bindD435Controls() { $('p3D435ResetBtn')?.addEventListener('click', onD435Reset); } +// Tab rail a destra: 3 voci (POSE/CAMERAS/MISC) che mostrano un gruppo +// alla volta. Solo presentazione: i listener interni alle sezioni nascoste +// restano agganciati, quindi switchare tab non perde stato. +function bindTabRail() { + const tabs = document.querySelectorAll('#tab-pose-3d .p3-tab'); + const groups = document.querySelectorAll('#tab-pose-3d .p3-group'); + if (!tabs.length || !groups.length) return; + for (const tab of tabs) { + tab.addEventListener('click', () => { + const target = tab.dataset.target; + for (const t of tabs) t.classList.toggle('active', t === tab); + for (const g of groups) g.hidden = (g.dataset.group !== target); + }); + } +} + // ----- init triggered on first activation ----- let inited = false; function ensureInit() { @@ -1701,6 +2162,7 @@ function ensureInit() { inited = true; initScene(); setupSelection(); + setupTagLocator(); bindSideControls(); bindGizmoControls(); bindGizmoEvents(); @@ -1715,6 +2177,8 @@ function ensureInit() { bindJointUCSControls(); bindCloudControls(); bindReadoutInputs(); + bindTabRail(); + bindFrontTagFinder(); pollState(); pollTimer = setInterval(pollState, POLL_MS); }