Compare commits

..
21 Commits
Author SHA1 Message Date
AdrianoandClaude Opus 4.8 c383fd3143 feat(version): mostra build time nel badge (non solo tooltip); bump 1.0.2
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:49:22 +02:00
AdrianoandClaude Opus 4.8 054ad103df chore: bump VERSION 1.0.0 -> 1.0.1 (deploy)
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:44:25 +02:00
AdrianoandClaude Opus 4.8 7b4dc6ce8c docs: build.sh deploy workflow + badge versione (CLAUDE.md, README)
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:43:07 +02:00
AdrianoandClaude Opus 4.8 cf726b479e feat: badge versione build in GUI + build.sh con auto-bump
- VERSION (1.0.0) + build.sh: bump patch/minor/major e deploy via docker compose
- Dockerfile: ARG/ENV APP_VERSION + BUILD_TIME (iniettati come build-arg)
- version.build_info(): env in Docker, fallback file VERSION + '(local)' in dev
- GET /api/version; badge #appVersionPill nell'header (tooltip = build time)
- test_version.py: env vs fallback

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:40:52 +02:00
AdrianoandClaude Opus 4.8 a88a064aa8 docs: spec badge versione build in GUI
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:40:52 +02:00
AdrianoandClaude Opus 4.8 8084b87261 Merge: allineamento cross-camera + overlay tag multi-camera (Pose 3D)
- umeyama_similarity (geometry)
- GET /api/pose_3d/tags (Nuwa + D435i), rimosso /front_tags
- POST /api/pose_3d/align: fit Umeyama + persistenza pose_3d.cross_camera
- /state espone cross_camera
- overlay tag su entrambe le camere (Nuwa rosso, D435i ciano)
- UI Allinea cross-camera + render Nuwa nel frame D435i
- docs

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 13:58:06 +02:00
AdrianoandClaude Opus 4.8 0ee3ef244b chore(pose_3d): tieni config.yaml allineato a main (drift runtime non versionato)
Il commit della Task 1 aveva incluso per errore (via git add -u) drift runtime
di config.yaml (cloud_params size/flip) persistito dall'app live. Lo rimuovo dal
branch: config.yaml resta uguale a main. I valori live restano come modifica
non committata nel working tree.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:43:34 +02:00
AdrianoandClaude Sonnet 4.6 d9213ca19a docs(pose_3d): tags multi-camera + calibrazione cross-camera
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:41:12 +02:00
AdrianoandClaude Opus 4.8 47ae8a6b2c feat(pose_3d): doAlign + persistenza/caricamento calibrazione cross-camera
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:38:25 +02:00
AdrianoandClaude Opus 4.8 746662d534 feat(pose_3d): piazzamento Nuwa allineato nel frame D435i (matrice)
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:35:30 +02:00
AdrianoandClaude Sonnet 4.6 2f0dc69cdd feat(pose_3d): rinomina sezione Tags + controlli Allinea cross-camera
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:33:55 +02:00
AdrianoandClaude Sonnet 4.6 edc4430c67 feat(pose_3d): overlay tag su entrambe le camere (Nuwa rosso, D435i ciano)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:31:16 +02:00
AdrianoandClaude Sonnet 4.6 79f587567e feat(pose_3d): /state espone blocco cross_camera
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:28:52 +02:00
AdrianoandClaude Sonnet 4.6 5ab808accd feat(pose_3d): POST /align fit Umeyama cross-camera + persistenza
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:28:05 +02:00
AdrianoandClaude Sonnet 4.6 8b01ae123f feat(pose_3d): GET /tags multi-camera, rimuovi /front_tags
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:20:04 +02:00
AdrianoandClaude Sonnet 4.6 77fb0f8fc0 feat(geometry): umeyama_similarity (rot+trasl+scala uniforme)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:13:25 +02:00
AdrianoandClaude Opus 4.8 9189700f34 docs(pose_3d): piano implementazione allineamento cross-camera
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:06:25 +02:00
AdrianoandClaude Opus 4.8 80625cc281 docs(pose_3d): spec allineamento cross-camera + overlay tag multi-camera
Design per: detection tag su tutte le camere attive (Nuwa rosso + D435i),
rinomina "Tag camera frontale" -> "Tags", calibrazione persistente
Nuwa->frontale via tag condiviso (Umeyama similarity), flag+pulsante
"Allinea" nella sezione Cloud cross-camera.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:02:26 +02:00
AdrianoandClaude Opus 4.8 b4922fe01b fix(pose_3d): front-tag detection solo al cambio immagine, non in loop
L'overlay tag frontale (D435i) ri-eseguiva la detection ArUco ad ogni
tick di pollState() finche' il flag era attivo. Ora gira solo:
- al toggle on del checkbox
- a fine acquireClouds() (pulsante Acquisisci) se il flag e' attivo

Rimosso il piggyback in pollState() e lo stato front_tags.in_flight
ormai inutilizzato.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 10:05:51 +02:00
AdrianoandClaude Opus 4.7 fb0e9b9112 feat(pose_3d): tag locator save, front-tag overlay, sidebar tabs, cloud params persist
- Sidebar a 3 tab verticali (POSE / CAMERAS / MISC) per ridurre lo scroll
- Tag locator: POST /api/pose_3d/tag_offset/{save,} salva crocetta nel
  viewport con snapshot giunti, current.json + archive rotation
- Front-camera tag overlay (MISC):
  - GET /api/pose_3d/front_tags scansiona ArUco sul D435i e per ogni tag
    misura i 4 corner 3D via depth sensor (indipendente da solvePnP);
    outlier rejection ±20% dalla mediana per ignorare corner che catturano
    background sul bordo del marker
  - Rendering quad cyan + label compatta (id, edge_mm, distanza camera)
    agganciato al d435_link, segue la camera anche in modalita' hand-eye
  - Trigger via pollState() quando il flag e' on, niente timer separato
- Cloud cross-camera: memorizza step/max_depth/sizes/scale_xy/flip/handeye
  in pose_3d.cloud_params al click di Acquisisci, ripopola UI da config
  al primo poll (state.cloud_params_populated guard)
- D435i front cloud: applica nuwa_to_cv_frame (il servizio ritorna native,
  non OpenCV come si pensava)
- Cache-bust per pose_3d.js e pose_3d.css

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-06-01 16:53:26 +02:00
Adriano Dal PastroandClaude Opus 4.7 ae8e609b7e docs: aggiorna README.md e CLAUDE.md con tab Braccio DX, Pose 3D, cloud, diagnostics
Allineati alla rimozione del modulo agent e alle nuove funzionalità:
- Tab Braccio DX e Pose 3D documentate in GUI
- Servizi front_camera (D435i) e G1 /diagnostics nella tabella upstream
- Endpoint right_arm/* e pose_3d/* nella lista API
- Convenzioni Pose 3D (graceful offline, clouds, joints mirroring) in CLAUDE.md
- Router list e _deps aggiornati (front_camera(), 12 router modules)

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-05-27 18:01:28 +02:00
22 changed files with 2749 additions and 36 deletions

No files matched your search

+15 -5
View File
@@ -9,6 +9,8 @@ Build & run (Docker):
docker compose up -d presa-mano # serves GUI + API on http://localhost:8890 docker compose up -d presa-mano # serves GUI + API on http://localhost:8890
``` ```
**Deploy changes — use `./build.sh`.** `src/` and `web/` are baked into the image (only `config.yaml` + `data/` are bind-mounted), so **code edits are invisible until the image is rebuilt**. `./build.sh` auto-bumps `VERSION` (patch by default; `./build.sh minor|major` for the others), injects version + build time as build-args, rebuilds, and restarts the container. The running version is shown as a badge in the GUI header (and at `GET /api/version`) so a stale deployment is visible at a glance. The `VERSION` bump is left uncommitted — commit it with your changes.
Local dev (uses `uv`, same as the Dockerfile): Local dev (uses `uv`, same as the Dockerfile):
```bash ```bash
uv sync # install deps from pyproject.toml uv sync # install deps from pyproject.toml
@@ -17,39 +19,43 @@ uv run uvicorn presa_mano.main:app --host 0.0.0.0 --port 8890 --reload
Useful endpoints (all under `http://localhost:8890`): Useful endpoints (all under `http://localhost:8890`):
- `GET /apidocs` — Swagger UI; `GET /redoc` — ReDoc. - `GET /apidocs` — Swagger UI; `GET /redoc` — ReDoc.
- `GET /health` — liveness; `GET /api/version` — `{version, build_time}` baked at build by `build.sh` (falls back to the `VERSION` file + `(local)` under `uv run`).
- `GET/PUT /api/config` — read/patch runtime config (deep-merged into `config.yaml` and persisted). - `GET/PUT /api/config` — read/patch runtime config (deep-merged into `config.yaml` and persisted).
- Calibration: `POST /api/calibration/sample`, `GET/DELETE /api/calibration/samples`, `POST /api/calibration/solve`, `GET /api/calibration/result`, `POST /api/calibration/preview`. - Calibration: `POST /api/calibration/sample`, `GET/DELETE /api/calibration/samples`, `POST /api/calibration/solve`, `GET /api/calibration/result`, `POST /api/calibration/preview`.
- Perception: `POST /api/perception/detect`, `POST /api/perception/object_points`, `GET /api/perception/measure`. - Perception: `POST /api/perception/detect`, `POST /api/perception/object_points`, `GET /api/perception/measure`.
- Grasp: `POST /api/grasp/plan`, `POST /api/grasp/execute` (set `dry_run` to gate G1 mutations). - Grasp: `POST /api/grasp/plan`, `POST /api/grasp/execute` (set `dry_run` to gate G1 mutations).
- Wrist/EE: `GET /api/wrist`, `POST /api/wrist/{absolute,relative,hand,emergency_stop,unlock}`. - Wrist/EE: `GET /api/wrist`, `POST /api/wrist/{absolute,relative,hand,emergency_stop,unlock}`.
- Right arm: `GET /api/right_arm/state`, `POST /api/right_arm/detect_{front,wrist}`, `POST /api/right_arm/move_toward`, `POST /api/right_arm/{home,custom_pose}`.
- Pose 3D: `GET /api/pose_3d/state` (wrist + joints + config, incl. `cross_camera` calibration), `POST /api/pose_3d/apply`, `GET /api/pose_3d/clouds` (Nuwa + D435i point clouds), `GET /api/pose_3d/tags` (ArUco su Nuwa polso + D435i frontale), `POST /api/pose_3d/align?align_id=N` (fit Umeyama Nuwa→frontale, persiste in `pose_3d.cross_camera`), `CRUD /api/pose_3d/poses/{name}`.
Tests live under `tests/` and run with `uv run pytest`. No linter is configured. Tests live under `tests/` and run with `uv run pytest`. No linter is configured.
## Architecture ## Architecture
This is a **FastAPI orchestrator** that glues together four external microservices to implement an eye-to-hand calibration + object-grasping pipeline for a Unitree G1 humanoid using a fixed Nuwa-HP60C depth camera. It owns no hardware itself — every action is an HTTP call. This is a **FastAPI orchestrator** that glues together external microservices to implement an eye-to-hand calibration + object-grasping + 3D visualization pipeline for a Unitree G1 humanoid. It owns no hardware itself — every action is an HTTP call.
Upstream services (defaults in `config.yaml > services`): Upstream services (defaults in `config.yaml > services`):
| Service | Default URL | Role | | Service | Default URL | Role |
|---------|------------|------| |---------|------------|------|
| Nuwa-HP60C | `:8877` | RGB, depth PNG, point cloud (full + ROI), intrinsics, pixel→3D measure | | Nuwa-HP60C | `:8877` | RGB, depth PNG, point cloud (full + ROI), intrinsics, pixel→3D measure. Eye-in-hand sul polso DX |
| Front camera (D435i) | `services.front_camera` | Point cloud frontale fissa sul torso; stessa interfaccia NuwaClient |
| marker-finder | `:8082` | ArUco detection (returns `corners`, `position_m`, `rotation_matrix`) | | marker-finder | `:8082` | ArUco detection (returns `corners`, `position_m`, `rotation_matrix`) |
| YOLO server | `:8879` | Object bbox detection from an image URL | | YOLO server | `:8879` | Object bbox detection from an image URL |
| G1 API | `:8765` | Wrist pose read/write, hand open/close, e-stop | | G1 API | `:8765` | Wrist pose read/write, hand open/close, e-stop, `/diagnostics` (29 joint positions) |
| SAM21 (NuwaSeg) | `:8002` | Promptable segmentation: rgb+depth+intrinsics+bbox → 3D point cloud of the object (replaces the bbox-only ROI cloud in the grasp pipeline) | | SAM21 (NuwaSeg) | `:8002` | Promptable segmentation: rgb+depth+intrinsics+bbox → 3D point cloud of the object (replaces the bbox-only ROI cloud in the grasp pipeline) |
### Layered package layout (`src/presa_mano/`) ### Layered package layout (`src/presa_mano/`)
- `main.py` — FastAPI app; mounts `web/` at `/static`, serves `web/index.html` at `/`, registers routers under `/api/*`. - `main.py` — FastAPI app; mounts `web/` at `/static`, serves `web/index.html` at `/`, registers routers under `/api/*`.
- `config.py` — Thread-safe singleton config loader. `load_config()` reads YAML once; `update_config(patch)` deep-merges and persists. Path comes from `PRESA_MANO_CONFIG` env (Dockerfile sets `/app/config.yaml`); data files live under `PRESA_MANO_DATA` (default `/app/data`). - `config.py` — Thread-safe singleton config loader. `load_config()` reads YAML once; `update_config(patch)` deep-merges and persists. Path comes from `PRESA_MANO_CONFIG` env (Dockerfile sets `/app/config.yaml`); data files live under `PRESA_MANO_DATA` (default `/app/data`).
- `api/` — Routers, one per concern. **Build clients per-request via `api/_deps.py`** (`nuwa()`, `marker()`, `yolo()`, `sam21()`, `g1(dry_run=...)`) so runtime `PUT /api/config` changes take effect without restart. Don't cache clients at module level. - `api/` — Routers, one per concern (calibration, config, grasp, home, models, perception, pose_3d, posture, right_arm, robot, work_plane, wrist). **Build clients per-request via `api/_deps.py`** (`nuwa()`, `front_camera()`, `marker()`, `yolo()`, `sam21()`, `g1(dry_run=...)`) so runtime `PUT /api/config` changes take effect without restart. Don't cache clients at module level.
- `clients/` — Thin `httpx` wrappers, one per upstream. Errors are normalized to service-specific exceptions (e.g. `G1Unavailable`, `NuwaError`). - `clients/` — Thin `httpx` wrappers, one per upstream. Errors are normalized to service-specific exceptions (e.g. `G1Unavailable`, `NuwaError`).
- `calibration/` — `sampler.py` builds one (T_base_wrist, T_cam_marker) sample; `handeye.py` runs `cv2.calibrateHandEye` and computes residuals; `store.py` persists samples (JSON) and the result (YAML). - `calibration/` — `sampler.py` builds one (T_base_wrist, T_cam_marker) sample; `handeye.py` runs `cv2.calibrateHandEye` and computes residuals; `store.py` persists samples (JSON) and the result (YAML).
- `perception/` — `detector.py` calls YOLO and normalizes responses across multiple envelope shapes; `pointcloud.py` handles frame conversion + outlier filtering. - `perception/` — `detector.py` calls YOLO and normalizes responses across multiple envelope shapes; `pointcloud.py` handles frame conversion + outlier filtering.
- `grasp/` — `axis.py` PCA → `ObjectAxes`; `planner.py` builds a wrist pose from those axes. - `grasp/` — `axis.py` PCA → `ObjectAxes`; `planner.py` builds a wrist pose from those axes.
- `geometry.py` — pure-numpy SE(3) helpers (quat↔R, rvec↔R, invert_T, transform_points). - `geometry.py` — pure-numpy SE(3) helpers (quat↔R, rvec↔R, invert_T, transform_points).
- `timing.py` — `Stopwatch` with `step(name)` context manager; every endpoint annotates its response with `timings_ms`. - `timing.py` — `Stopwatch` with `step(name)` context manager; every endpoint annotates its response with `timings_ms`.
- `web/` — vanilla HTML/JS/CSS GUI (3 tabs: Calibration, Grasp, Movimento EE), served as static files; talks only to `/api/*`. - `web/` — vanilla HTML/JS/CSS GUI served as static files; talks only to `/api/*`. Tab principali: Calibrazione, Calibrazione Piano, Cerca Modello, Movimento EE, Braccio DX, Pose 3D. Il viewport 3D (pose_3d.js) usa Three.js + URDFLoader per il modello robot.
### Core data flow: hand-eye calibration (eye-to-hand) ### Core data flow: hand-eye calibration (eye-to-hand)
@@ -88,6 +94,10 @@ Marker-finder caveat: it uses **D435i intrinsics baked into its config** that do
- **Dry-run semantics:** any G1-mutating endpoint takes a `dry_run` flag; `G1Client._post` short-circuits and logs `[DRY-RUN]` instead of hitting the network. Never silently strip dry-run from a mutating path. - **Dry-run semantics:** any G1-mutating endpoint takes a `dry_run` flag; `G1Client._post` short-circuits and logs `[DRY-RUN]` instead of hitting the network. Never silently strip dry-run from a mutating path.
- **Per-request client construction** (via `api/_deps.py`) is intentional — runtime `PUT /api/config` updates would otherwise be invisible. - **Per-request client construction** (via `api/_deps.py`) is intentional — runtime `PUT /api/config` updates would otherwise be invisible.
- **SAM21 vs bbox frame convention:** the grasp pipeline has two parallel paths in `_object_points_in_base`. The SAM21 path returns points already in the OpenCV RGB-camera frame and **must NOT call `nuwa_to_cv_frame`**; the legacy bbox path (via `_points_via_bbox`) still calls it because Nuwa's ROI cloud uses a different convention. Don't accidentally add the flip to the SAM21 branch — coordinates would double-flip. - **SAM21 vs bbox frame convention:** the grasp pipeline has two parallel paths in `_object_points_in_base`. The SAM21 path returns points already in the OpenCV RGB-camera frame and **must NOT call `nuwa_to_cv_frame`**; the legacy bbox path (via `_points_via_bbox`) still calls it because Nuwa's ROI cloud uses a different convention. Don't accidentally add the flip to the SAM21 branch — coordinates would double-flip.
- **Pose 3D graceful offline:** `GET /api/pose_3d/state` returns 200 even when G1 is unreachable — `wrist` and `joints` are `null`, `robot_error` carries the message. The tab must remain usable for editing config / named poses / viewing the URDF model without a live robot.
- **Pose 3D clouds:** `GET /api/pose_3d/clouds` acquires from Nuwa (eye-in-hand, polso DX) and D435i (front camera, torso) in parallel. Both are returned in **OpenCV optical frame** (X right, Y down, Z forward). The client attaches them to URDF links with configurable offset/rpy from `config.yaml > pose_3d.clouds`.
- **Pose 3D joints mirroring:** `G1Client.diagnostics()` → `motors[].{name, q}` gives real joint positions in radians. The frontend uses these to pose the URDF model directly, bypassing client-side IK.
- **Pose 3D cross-camera alignment:** `POST /api/pose_3d/align` fits an Umeyama similarity (rotation + translation + uniform scale) mapping Nuwa-optical → D435-optical from a tag (`align_id`) seen by both cameras, and persists it to `config.yaml > pose_3d.cross_camera`. The frontend renders the Nuwa cloud through the D435i transform pipeline (`d435LocalMatrix()` · alignMatrix) when `cross_camera.enabled`. If the tag isn't seen by both cameras, the saved matrix is NOT overwritten and the Nuwa cloud falls back to URDF placement. Tag detection (`GET /tags`) runs on both cameras; the GUI draws Nuwa tags red, D435i tags cyan.
- Endpoints return `{ ..., "timings_ms": {step: ms, ..., total: ms} }`. New endpoints should follow this with `Stopwatch().step(name)`. - Endpoints return `{ ..., "timings_ms": {step: ms, ..., total: ms} }`. New endpoints should follow this with `Stopwatch().step(name)`.
- Calibration paths in config (`calibration.file`, `calibration.samples_file`) are resolved against `PRESA_MANO_DATA` when relative — see `_deps.calibration_file_path()` / `samples_file_path()`. - Calibration paths in config (`calibration.file`, `calibration.samples_file`) are resolved against `PRESA_MANO_DATA` when relative — see `_deps.calibration_file_path()` / `samples_file_path()`.
- Italian is fine in user-facing copy (GUI, README, comments) — it's the project default. - Italian is fine in user-facing copy (GUI, README, comments) — it's the project default.
+8 -1
View File
@@ -12,13 +12,20 @@ COPY pyproject.toml README.md ./
COPY src/ src/ COPY src/ src/
RUN uv sync --no-dev RUN uv sync --no-dev
COPY config.yaml ./ COPY config.yaml VERSION ./
COPY web/ web/ COPY web/ web/
ENV PRESA_MANO_CONFIG=/app/config.yaml ENV PRESA_MANO_CONFIG=/app/config.yaml
ENV PRESA_MANO_DATA=/app/data ENV PRESA_MANO_DATA=/app/data
ENV PRESA_MANO_PORT=8890 ENV PRESA_MANO_PORT=8890
# Versione build: iniettata da build.sh come --build-arg. Cambia ad ogni build
# (il valore diverso invalida solo questi due layer finali, non le dipendenze).
ARG APP_VERSION=dev
ARG BUILD_TIME=unknown
ENV APP_VERSION=${APP_VERSION}
ENV BUILD_TIME=${BUILD_TIME}
EXPOSE 8890 EXPOSE 8890
# Shell form so $PRESA_MANO_PORT viene espanso a runtime: cambi la porta in # Shell form so $PRESA_MANO_PORT viene espanso a runtime: cambi la porta in
+32 -6
View File
@@ -8,17 +8,21 @@ Microservizio Python (FastAPI) che orchestra:
nuvola di punti dell'oggetto. nuvola di punti dell'oggetto.
3. **Calibrazione piano di lavoro** + filtro "sotto piano" applicato a ogni cloud. 3. **Calibrazione piano di lavoro** + filtro "sotto piano" applicato a ogni cloud.
4. **Pianificazione presa**: asse dell'oggetto (PCA) + posa polso per il robot attivo. 4. **Pianificazione presa**: asse dell'oggetto (PCA) + posa polso per il robot attivo.
5. **Visualizzazione 3D** (tab Pose 3D): viewport Three.js con URDF del robot
specchiato sui giunti reali, point cloud live (Nuwa eye-in-hand + D435i front
camera), gizmo di editing pose, named poses salvabili.
## Microservizi usati ## Microservizi usati
| Servizio | URL (default) | Scopo | | Servizio | URL (default) | Scopo |
|-------------------|-----------------------|----------------------------------------------------| |-------------------|-----------------------|----------------------------------------------------|
| Nuwa-HP60C | http://localhost:8877 | RGB, depth, point cloud, intrinsics | | Nuwa-HP60C | `:8877` | RGB, depth, point cloud, intrinsics (eye-in-hand polso DX) |
| Marker-finder | http://localhost:8082 | Detection 2D ArUco (corners + pose) | | Front camera (D435i) | config `services.front_camera` | Point cloud frontale fissa sul torso |
| YOLO Server | http://localhost:8879 | Bbox oggetto da prendere | | Marker-finder | `:8082` | Detection 2D ArUco (corners + pose) |
| SAM21 (NuwaSeg) | http://localhost:8002 | Segmentazione promptable: bbox → cloud 3D filtrata | | YOLO Server | `:8879` | Bbox oggetto da prendere |
| G1 API Server | http://localhost:8765 | Controllo polso/mano G1 (dry-run se down) | | SAM21 (NuwaSeg) | `:8002` | Segmentazione promptable: bbox → cloud 3D filtrata |
| H1 API Server | http://localhost:8767 | Controllo polso/mano H1 (network host, 4 DoF) | | G1 API Server | `:8765` | Controllo polso/mano G1, diagnostics giunti (dry-run se down) |
| H1 API Server | `:8767` | Controllo polso/mano H1 (network host, 4 DoF) |
Il servizio **presa-mano** espone GUI + API su **http://localhost:8890**. Il robot attivo Il servizio **presa-mano** espone GUI + API su **http://localhost:8890**. Il robot attivo
è scelto da `config.yaml > robot.active` (`g1` | `h1`); l'orchestrator fa routing è scelto da `config.yaml > robot.active` (`g1` | `h1`); l'orchestrator fa routing
@@ -31,6 +35,18 @@ docker compose up -d presa-mano
# Apri http://localhost:8890 # Apri http://localhost:8890
``` ```
## Build & deploy delle modifiche
`src/` e `web/` sono inclusi (baked) nell'immagine: le modifiche al codice **non si vedono finché non ricostruisci l'immagine**. Usa lo script:
```bash
./build.sh # patch++ (1.0.1 -> 1.0.2), rebuild + restart
./build.sh minor # 1.0.2 -> 1.1.0
./build.sh major # 1.1.0 -> 2.0.0
```
Lo script bumpa il file `VERSION`, inietta versione + ora di build nell'immagine e riavvia il container. La versione attiva è mostrata nel **badge in alto a destra** nella GUI (tooltip con l'ora di build) ed è esposta da `GET /api/version` — così capisci subito se il container gira l'ultima build. Il bump di `VERSION` resta non committato: committalo con le tue modifiche.
## GUI ## GUI
Tab principali: Tab principali:
@@ -47,6 +63,16 @@ Tab principali:
se SAM21 è down), visualizza bbox + asse, esegui (toggle dry-run). se SAM21 è down), visualizza bbox + asse, esegui (toggle dry-run).
- **Movimento EE**: lettura pose correnti, invio pose assolute/relative, apri/chiudi mano, - **Movimento EE**: lettura pose correnti, invio pose assolute/relative, apri/chiudi mano,
emergency stop. emergency stop.
- **Braccio DX**: detection via YOLO su front camera + camera polso, move-toward
iterativo, pose salvabili (home, custom), verifica tag ArUco.
- **Pose 3D**: viewport Three.js con URDF G1 specchiato sui giunti reali
(`/diagnostics`), point cloud Nuwa + D435i, modello 3D camera sul polso,
gizmo traslazione/rotazione, editor pose (Apply/Jog), named poses salvabili.
Overlay tag ArUco su entrambe le camere (Nuwa = rosso, D435i = ciano).
Calibrazione cross-camera: pulsante *Allinea* esegue fit Umeyama Nuwa→D435i
da un tag comune, persiste in `config.yaml > pose_3d.cross_camera`.
La tab resta utilizzabile anche senza robot (mostra il modello URDF, config,
pose salvate).
## Pipeline cloud-from-bbox ## Pipeline cloud-from-bbox
+1
View File
@@ -0,0 +1 @@
1.0.2
Executable
+40
View File
@@ -0,0 +1,40 @@
#!/usr/bin/env bash
# Build + deploy di presa-mano con auto-bump della versione.
#
# ./build.sh → patch++ (1.0.1 -> 1.0.2)
# ./build.sh minor → minor++ (1.0.2 -> 1.1.0)
# ./build.sh major → major++ (1.1.0 -> 2.0.0)
#
# La nuova versione + l'ora di build vengono iniettate nell'immagine come
# build-arg e mostrate nel badge in GUI. Il file VERSION viene riscritto ma NON
# committato automaticamente (lo committi tu con le altre modifiche).
set -euo pipefail
SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
COMPOSE_DIR="$(dirname "$SCRIPT_DIR")" # docker-compose.yml sta un livello sopra
VERSION_FILE="$SCRIPT_DIR/VERSION"
[ -f "$VERSION_FILE" ] || echo "1.0.0" > "$VERSION_FILE"
cur="$(tr -d '[:space:]' < "$VERSION_FILE")"
IFS='.' read -r major minor patch <<< "$cur"
major="${major:-1}"; minor="${minor:-0}"; patch="${patch:-0}"
case "${1:-patch}" in
major) major=$((major + 1)); minor=0; patch=0 ;;
minor) minor=$((minor + 1)); patch=0 ;;
patch) patch=$((patch + 1)) ;;
*) echo "uso: $0 [major|minor|patch]" >&2; exit 1 ;;
esac
new="${major}.${minor}.${patch}"
echo "$new" > "$VERSION_FILE"
build_time="$(date -u +"%Y-%m-%d %H:%M UTC")"
echo "→ build V${new} (${build_time})"
cd "$COMPOSE_DIR"
docker compose build \
--build-arg APP_VERSION="$new" \
--build-arg BUILD_TIME="$build_time" \
presa-mano
docker compose up -d presa-mano
echo "✓ deployed V${new} (${build_time})"
+32 -15
View File
@@ -82,6 +82,8 @@ tab_right_arm:
- 0 - 0
pose_3d: pose_3d:
live_max_step_m: 0.15 live_max_step_m: 0.15
tag_offset:
save_dir: /home/harry/robot/logging/wrist-tag-offset
right_wrist_camera: right_wrist_camera:
enabled: true enabled: true
mesh_url: /static/assets/nuwa/nuwa-hp60c.stl mesh_url: /static/assets/nuwa/nuwa-hp60c.stl
@@ -107,25 +109,25 @@ pose_3d:
d435: d435:
offset_m: offset_m:
- -0.01 - -0.01
- 0 - 0.015
- 0 - -0.005
rpy_deg: rpy_deg:
- -90 - -90
- 86 - 90
- 0 - 0
transform_pelvis_4x4: transform_pelvis_4x4:
- - -0.011344227963137242 - - 0.03273746934835532
- -0.9974094742471121 - -0.9960477706629303
- -0.07103273311663486 - -0.0825657111522212
- 0.007340882385689307 - -0.00771413467718246
- - -0.5556320632728338 - - -0.7734929889294071
- 0.06534842452974676 - 0.027067579810660346
- -0.8288561960163252 - -0.633226612043623
- 0.3109738415126284 - 0.37547513731967447
- - 0.8313508998943191 - - 0.6329588092267278
- 0.03006533042523672 - 0.08459423550455474
- -0.5549340115286931 - -0.7695498431821574
- 0.2869260834772309 - 0.33337005199505626
- - 0.0 - - 0.0
- 0.0 - 0.0
- 0.0 - 0.0
@@ -142,6 +144,21 @@ pose_3d:
- -0.047703617194205114 - -0.047703617194205114
- -0.005110044879619669 - -0.005110044879619669
- 0.9988341941293186 - 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: grasp:
approach_offset_m: 0.1 approach_offset_m: 0.1
pre_grasp_clearance_m: 0.05 pre_grasp_clearance_m: 0.05
File diff suppressed because it is too large. Load diff
@@ -0,0 +1,153 @@
# Cross-camera tag overlay + Nuwa→frontale alignment calibration
**Date:** 2026-06-03
**Status:** approved (design)
**Area:** Pose 3D tab — `web/pose_3d.js`, `web/index.html`, `src/presa_mano/api/pose_3d_routes.py`, `src/presa_mano/geometry.py`
## Problem
The Pose 3D tab detects ArUco tags only on the **front D435i** camera (`GET /api/pose_3d/front_tags`) and draws them as an overlay. We want:
1. Tag detection on **all active cameras** (Nuwa wrist eye-in-hand + D435i front), not just the front one. Nuwa tags drawn in a **distinct red** color; D435i keeps its current color.
2. The "Tag camera frontale" UI block renamed to **"Tags"** (it now covers every active camera).
3. A **cross-camera alignment calibration**: a configurable `align_id` tag that must be seen by **both** cameras lets us fit a similarity transform that maps the Nuwa point cloud onto the D435i cloud (rotation + translation + uniform scale). This is a **persistent calibration** (saved, reused across sessions), gated by an enable/disable flag in the "Cloud cross-camera" section.
## Decisions (locked)
- **Recompute trigger:** dedicated **"Allinea"** button. The fitted matrix is frozen until the button is pressed again. Acquisisci does *not* recompute it.
- **Persistence:** the matrix is a **persistent calibration**, saved to `config.yaml > pose_3d.cross_camera` and reloaded on startup (like a hand-eye calibration). Session memory mirrors it.
- **Transform type:** **Umeyama similarity** — rotation + translation + single uniform scale.
- **Tag detection scope:** when "Mostra tag" is on, detection runs on **both** cameras always (independent of which clouds are shown).
- **Fallback when the align tag is not seen by both cameras at calibration time:** the `POST /align` call returns `found_in_both:false` and an error and **does not overwrite** the saved matrix. At render time, if the flag is ON but there is no valid saved matrix, the Nuwa cloud falls back to normal URDF/hand-eye placement and a warning is shown.
- **`align_id` default:** `0`. It must be a scene/table tag visible to both the wrist (Nuwa) and torso (D435i) cameras simultaneously — not a robot-mounted tag.
- **`/front_tags` removed** (the GUI is the only consumer) and replaced by the generalized endpoints below.
## Architecture
### Backend — `src/presa_mano/`
**`geometry.py` — new pure helper**
```python
def umeyama_similarity(src: np.ndarray, dst: np.ndarray
) -> tuple[float, np.ndarray, np.ndarray, float]:
"""Fit dst ≈ s·R·src + t (Umeyama 1991, with scale).
src, dst: (N,3) corresponding points. Returns (scale, R(3x3), t(3,), rmse_meters).
Raises ValueError if N < 3 or the point set is degenerate (rank-deficient covariance)."""
```
Numpy-only, unit-testable in isolation. Standard Umeyama: center both sets, `H = dst_c.T @ src_c / N`, SVD `U,S,Vt = svd(H)`, `D = diag(1,1,sign(det(U@Vt)))` to forbid reflections, `R = U @ D @ Vt`, `scale = trace(diag(S)@D) / var(src_c)`, `t = mean(dst) - scale·R·mean(src)`. `rmse` = RMS residual of `s·R·src + t` vs `dst`.
**`pose_3d_routes.py` — generalize detection**
Extract the per-camera body of the current `get_front_tags` into a camera-agnostic helper:
```python
def _detect_tags_on(client, frame_label: str, ids: str,
probe_size_mm: float = 40.0) -> list[dict]:
"""Detect ArUco on `client` RGB + measure 3D corners/center via `client.measure`.
`client` is any NuwaClient (Nuwa wrist cam or D435i). Returns the per-tag list
(id, corners_pixel, corners_3d_optical_m, corner_depths_mm, center_3d_optical_m,
edge_mm_per_side, edge_mm, distance_m) — identical schema to today's front_tags."""
```
The current intrinsics→K override, corner measure, depth-outlier rejection, edge/distance math move verbatim into this helper.
**`GET /api/pose_3d/tags?ids=0-49&probe_size_mm=40`** — detect on both cameras:
```json
{ "cameras": {
"nuwa": { "tags": [ ...tag schema... ], "frame": "nuwa_rgb_optical" },
"d435": { "tags": [ ... ], "frame": "d435_rgb_optical" } },
"timings_ms": { "nuwa": ..., "d435": ..., "total": ... } }
```
Each camera detected independently; a per-camera failure yields `{ "tags": [], "error": "<msg>", "frame": ... }` instead of a 500, so one offline camera doesn't kill the other (mirrors the `/clouds` graceful pattern).
**`POST /api/pose_3d/align?ids=0-49&align_id=0&probe_size_mm=40`** — compute + persist the calibration:
1. `_detect_tags_on` both cameras.
2. Find `align_id` in both `nuwa` and `d435` tag lists. If missing in either → return `{ "found_in_both": false, "error": "tag <id> non visto da <cam(s)>", ... }` **without touching the saved matrix**.
3. Build the 4 corner correspondences (`corners_3d_optical_m`), dropping any corner that is NaN in *either* camera. Require ≥3 valid corners else `found_in_both:false`, error.
4. `scale, R, t, rmse = umeyama_similarity(nuwa_corners, d435_corners)`.
5. Persist via `update_config({"pose_3d": {"cross_camera": {...}}})`:
```json
{ "align_id": 0, "scale": 0.98, "R": [[...3x3...]], "t": [x,y,z],
"rmse_mm": 3.2, "n_corners": 4 }
```
The `enabled` flag lives in the same `cross_camera` block but is written by the JS save path (see below), not overwritten here.
6. Return the same object plus `found_in_both:true`, `timings_ms`. Frame note: maps **Nuwa optical → D435 optical** (both OpenCV X-right/Y-down/Z-forward).
**`GET /api/pose_3d/state`** — extend the returned dict with `cross_camera`: the saved `{ enabled, align_id, scale, R, t, rmse_mm, n_corners }` (or `null` if never calibrated). The frontend uses it to repopulate the UI and apply the matrix on load.
### Frontend — `web/pose_3d.js` + `web/index.html`
**Tag overlay generalization.** `state.front_tags` → `state.tags = { nuwa: {group:null}, d435: {group:null} }`. Add `NUWA_TAG_COLOR = 0xff3b30` (red); D435i keeps `FRONT_TAG_COLOR`.
- `pollFrontTags()` → `pollTags()`: `GET /api/pose_3d/tags?ids=...`, then for each camera `renderTags(camKey, tags, color)`.
- `renderFrontTags(tags)` → `renderTags(camKey, tags, color)`: same geometry/label logic, but `clearTagsGroup(camKey)`, parent = `getCloudParent(camKey)` (so Nuwa tags ride the Nuwa cloud node, D435i tags the D435i node), color parameterized.
- `addOneFrontTag(grp, t)` → `addOneTag(grp, t, color)`.
- `clearFrontTagsGroup()` → `clearTagsGroup(camKey)` (per-camera).
- Trigger points unchanged in spirit: on the "Mostra tag" checkbox toggle-on, and at the end of `acquireClouds()` when the flag is on (the loop in `pollState()` stays removed). The IDs field `#p3FrontTagIds` is reused (the single shared range for both cameras).
**Cross-camera alignment.** New `state.align = { enabled:false, id:0, matrix:null /*THREE.Matrix4*/, info:null }`.
UI added to the existing **"Cloud cross-camera"** section in `index.html`:
```
[ ] Allinea Nuwa→frontale tag id [ 0 ] [Allinea]
<status line: scala / rmse / errore>
```
- `#p3AlignEnabled` (checkbox), `#p3AlignTagId` (number), `#p3AlignBtn` (button), `#p3AlignStatus` (status div).
Handlers:
- **`doAlign()`** (`#p3AlignBtn`): `POST /api/pose_3d/align?ids=<p3FrontTagIds>&align_id=<p3AlignTagId>`. On `found_in_both`: build `THREE.Matrix4` from `scale,R,t` → `state.align.matrix`, status shows `scala=… rmse=…mm`, then `applyNuwaCloudPlacement()`. On failure: leave `state.align.matrix` as-is, status shows the error in the warning color.
- **`#p3AlignEnabled` change**: update `state.align.enabled`, persist (see below), `applyNuwaCloudPlacement()`.
- On load, `applyCrossCameraFromCfg(s.cross_camera)` (called from `pollState`, guarded like `applyCloudParamsFromCfg` so it doesn't clobber in-progress edits): sets the checkbox/id inputs and, if `R/t/scale` present, builds `state.align.matrix`.
**Nuwa cloud placement.** Refactor the Nuwa branch of `applyCloudTransform`/`buildOrUpdateCloud` into `applyNuwaCloudPlacement()`:
- **Aligned mode** (`state.align.enabled && state.align.matrix`): re-parent the Nuwa `THREE.Points` to `getCloudParent('d435')` (same parent as the D435i cloud), set `matrixAutoUpdate=false`, and set
`nuwa.matrix = D435LocalMatrix · AlignMatrix`,
where `D435LocalMatrix` is exactly the matrix the D435i cloud uses relative to that parent (hand-eye `M·S`, or URDF `T(offset)·R(rpy)·S(scaleVec)` — extracted into a shared `d435LocalMatrix()` helper so both clouds stay in lockstep). A raw Nuwa optical point `p` then renders at `parent · D435Local · Align · p`, i.e. mapped into D435i optical space and pushed through D435i's own pipeline → guaranteed overlay regardless of URDF/hand-eye error.
- **Normal mode** (flag off, or no matrix): current behavior — parent `right_wrist_yaw_link`, `T·R·S` from `getCloudTransform('nuwa')` + `readCloudScaleVec('nuwa')`. If the flag is ON but `matrix` is null, also surface a warning in `#p3AlignStatus`.
`applyNuwaCloudPlacement()` is invoked from `buildOrUpdateCloud('nuwa')`, after `doAlign()`, and on the enable-flag toggle.
**Persistence (JS).** Extend `readCloudParamsFromUI`/`saveCloudParamsToConfig` path (or a sibling `saveCrossCameraFlag()`) to PUT `{ pose_3d: { cross_camera: { enabled: <bool>, align_id: <int> } } }`. The matrix fields are written server-side by `/align`; JS only owns `enabled` + `align_id`.
## Config schema
`config.yaml > pose_3d.cross_camera`:
```yaml
pose_3d:
cross_camera:
enabled: false
align_id: 0
scale: 1.0 # written by POST /align
R: [[1,0,0],[0,1,0],[0,0,1]]
t: [0.0, 0.0, 0.0]
rmse_mm: null
n_corners: 0
```
Reads tolerate a missing block (defaults). Writes go through `update_config` deep-merge.
## Error handling
- One camera offline → `/tags` and `/align` still report the other; `/align` with the dead camera missing the tag → `found_in_both:false`, no save.
- `umeyama_similarity` raises `ValueError` on <3 / degenerate corners → `/align` catches → `found_in_both:false`, error string.
- All new endpoints keep the `timings_ms` convention via `Stopwatch().step(...)`.
- Render-time: flag ON + null matrix → URDF fallback + visible warning (never a blank/crashed cloud).
## Testing
- **`umeyama_similarity`** (`tests/test_geometry.py` or new): apply a known `(s,R,t)` to random points, recover within tolerance (scale ~1e-6, R ~1e-6, t ~1e-6); collinear/<3 points → `ValueError`.
- **`POST /api/pose_3d/align`** (mock marker + both camera clients): tag in both → matrix returned + persisted; tag missing in one → `found_in_both:false`, saved matrix untouched. Use the **config snapshot/restore fixture** (conftest) because `update_config` reformats `config.yaml`.
- **`GET /api/pose_3d/tags`** (mock): both cameras' tags returned with correct `frame` labels; one camera raising → that camera reports `error`, the other still returns tags.
- **`GET /api/pose_3d/state`**: includes `cross_camera` (saved or null).
## Out of scope (YAGNI)
- Per-axis/anisotropic scale (rejected — uniform only).
- Continuous/live re-alignment (rejected — button-driven).
- Aligning D435i onto Nuwa (direction is fixed Nuwa→frontale).
- A clear/reset-calibration button (can be added later; not required now).
```
@@ -0,0 +1,95 @@
# GUI build-version badge
**Date:** 2026-06-03
**Status:** approved (design)
**Area:** `VERSION`, `build.sh`, `Dockerfile`, `src/presa_mano/version.py`, `src/presa_mano/main.py`, `web/index.html`, `web/app.js`
## Problem
`src/` and `web/` are baked into the Docker image (no bind-mount), so code changes only take effect after an image rebuild. There is no visible indicator of which build the running container serves, so a stale deployment is invisible — exactly the confusion behind "non vedo modifiche alla GUI". We want a version shown in the GUI header that increments on every build, plus the build time.
## Decisions (locked)
- **Version string:** `V<major>.<minor>.<patch>` (e.g. `V1.0.1`), **patch auto-increments every build**; build time shown too.
- **Injection:** a build wrapper script passes the version + build time as Docker build args.
- **Placement:** a pill badge in the GUI header, build time in its tooltip.
## Components
### `VERSION` (tracked file, repo root `presa-mano/VERSION`)
Plain text `major.minor.patch`, initialized to `1.0.0`. Single source of truth for the number; persists across builds via the file on disk. The build script rewrites it (left as an uncommitted working-tree edit — the script does NOT auto-commit).
### `build.sh` (repo root, executable)
```
1. cd to script dir; ensure VERSION exists (else seed 1.0.0)
2. parse current major.minor.patch
3. bump: default patch++; arg `minor` → minor++/patch=0; `major` → major++/minor=patch=0
4. write new VERSION
5. BUILD_TIME = $(date -u +"%Y-%m-%d %H:%M UTC")
6. (cd compose dir) docker compose build --build-arg APP_VERSION=<new> --build-arg BUILD_TIME="<ts>" presa-mano
7. docker compose up -d presa-mano
8. echo deployed version
```
The compose file lives one level up (`/opt/docker/robotserver/docker-compose.yml`); the script resolves that dir and runs compose there. Usage: `./build.sh [patch|minor|major]` (default patch).
### `Dockerfile`
Add late (before `CMD`), so changing them doesn't bust the dependency layers:
```
ARG APP_VERSION=dev
ARG BUILD_TIME=unknown
ENV APP_VERSION=${APP_VERSION}
ENV BUILD_TIME=${BUILD_TIME}
```
Also `COPY VERSION ./` (cheap, makes the file fallback work inside the container too).
### `src/presa_mano/version.py`
```python
def build_info() -> dict:
"""{'version', 'build_time'}. Prefer env (set at Docker build); fall back to
the VERSION file + '(local)' for `uv run` dev."""
```
- `version` = `APP_VERSION` env if set, else `<VERSION file contents> (local)` (or `0.0.0 (local)` if the file is unreadable).
- `build_time` = `BUILD_TIME` env if set, else `(local)`.
- `VERSION` file resolved as `Path(__file__).resolve().parents[2] / "VERSION"`.
### `src/presa_mano/main.py`
```python
from .version import build_info
@app.get("/api/version", tags=["meta"])
def version() -> dict:
return build_info()
```
### `web/index.html`
In the header, next to `#activeRobotPill`, add:
```html
<span class="active-robot-pill" id="appVersionPill" title="versione build">V—</span>
```
(reuses the existing pill styling).
### `web/app.js`
Add and call once at startup (alongside `refreshHomeRobotSelector()`):
```javascript
async function refreshAppVersion() {
try {
const r = await api('GET', '/api/version');
const pill = $('appVersionPill');
if (pill) { pill.textContent = `V${r.version}`; pill.title = `build ${r.build_time}`; }
} catch {}
}
refreshAppVersion();
```
## Testing
- Unit test `tests/test_version.py` for `build_info()`:
- env set (`APP_VERSION`, `BUILD_TIME` via monkeypatch) → returns exactly those.
- env unset (monkeypatch.delenv) → `version` ends with `(local)` and contains the `VERSION` file content; `build_time == "(local)"`.
- Frontend: manual — after `./build.sh` the badge increments and the tooltip shows the new build time.
## Out of scope (YAGNI)
- git SHA / dirty flag (user chose version+time only).
- Auto-commit of the VERSION bump.
- Frontend-vs-backend version mismatch warning.
+306 -4
View File
@@ -1,17 +1,22 @@
"""Endpoint per la tab "Pose 3D": editor visuale + jog dei polsi.""" """Endpoint per la tab "Pose 3D": editor visuale + jog dei polsi."""
from __future__ import annotations from __future__ import annotations
import json
import logging import logging
import os
import re import re
import time
from pathlib import Path
from typing import Any, Literal, Optional from typing import Any, Literal, Optional
import httpx import httpx
import numpy as np
from fastapi import APIRouter, HTTPException from fastapi import APIRouter, HTTPException
from pydantic import BaseModel, Field 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, update_config
from ..geometry import normalize_quaternion from ..geometry import normalize_quaternion, umeyama_similarity
from ..perception.pointcloud import nuwa_to_cv_frame from ..perception.pointcloud import nuwa_to_cv_frame
from ..timing import Stopwatch from ..timing import Stopwatch
from . import _deps from . import _deps
@@ -99,6 +104,8 @@ def get_state() -> dict[str, Any]:
"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 {}, "right_wrist_camera": cfg.get("right_wrist_camera") or {},
"clouds": cfg.get("clouds") or {}, "clouds": cfg.get("clouds") or {},
"cloud_params": cfg.get("cloud_params") or {},
"cross_camera": cfg.get("cross_camera") or {},
"timings_ms": sw.as_dict(), "timings_ms": sw.as_dict(),
} }
@@ -229,11 +236,14 @@ def get_clouds(step: int = 8, max_depth_mm: int = 2000) -> dict[str, Any]:
except Exception as e: except Exception as e:
result["nuwa"] = {"error": f"{type(e).__name__}: {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: try:
with sw.step("d435_cloud"): with sw.step("d435_cloud"):
pts = _deps.front_camera().pointcloud(step=step, max_depth_mm=max_depth_mm) 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: except Exception as e:
result["d435"] = {"error": f"{type(e).__name__}: {e}"} result["d435"] = {"error": f"{type(e).__name__}: {e}"}
@@ -241,6 +251,298 @@ def get_clouds(step: int = 8, max_depth_mm: int = 2000) -> dict[str, Any]:
return result return result
# --- Multi-camera tag finder: detect + misura indipendente via depth ----
# Scansiona ArUco DICT_4X4_50 su tutte le camere attive (Nuwa polso DX e
# D435i frontale). Per ogni tag trovato misura i 4 corner 3D via depth sensor
# (indipendente da solvePnP). Output in frame ottico OpenCV per ciascuna camera.
def _edge_len_mm(a: list[float], b: list[float]) -> "Optional[float]":
"""Lunghezza 3D in mm tra due corner; None se un corner ha NaN."""
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
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))
def _detect_tags_on(client, frame_label: str, ids: str,
probe_size_mm: float = 40.0) -> list[dict]:
"""Detect ArUco markers su `client` (qualsiasi NuwaClient) e misura i 4
corner + centro in 3D via depth sensor (indipendente da solvePnP).
Ritorna una lista di dict per tag (id, corners_pixel, corners_3d_optical_m,
corner_depths_mm, center_3d_optical_m, edge_mm_per_side, edge_mm, distance_m).
`frame_label` è solo per i log; i punti sono nel frame ottico OpenCV della camera."""
mk = _deps.marker()
intr = client.intrinsics()
rgb = (intr or {}).get("rgb") or {}
if not rgb or "fx" not in rgb:
raise RuntimeError(f"intrinsics rgb mancanti: {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")
],
}
id_list = _parse_id_range(ids)
markers = [{"id": int(i), "size_mm": float(probe_size_mm)} for i in id_list]
det = mk.detect_url(markers=markers, image_url=client.rgb_url(), intrinsics=K)
tags_out: list[dict[str, Any]] = []
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 = client.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 = client.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("tags(%s): measure failed for id=%s: %s",
frame_label, m.get("id"), e)
continue
valid_d = [d for d in depths if d is not None]
if len(valid_d) >= 3:
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
edges_mm = [_edge_len_mm(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,
})
return tags_out
# Map: camera key → (frame_label, _deps attribute name).
# Usiamo il nome stringa dell'attributo su _deps e lo risolviamo via getattr
# a ogni richiesta: cosi' patch("...pose_3d_routes._deps.nuwa") viene visto.
_TAG_CAMERAS: dict[str, tuple[str, str]] = {
"nuwa": ("nuwa_rgb_optical", "nuwa"),
"d435": ("d435_rgb_optical", "front_camera"),
}
@router.get("/tags")
def get_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str, Any]:
"""Detect ArUco su tutte le camere attive (Nuwa polso + D435i frontale).
Una camera offline riporta tags=[] + error, non fa fallire l'altra."""
sw = Stopwatch()
cameras: dict[str, Any] = {}
for key, (frame_label, dep_name) in _TAG_CAMERAS.items():
try:
factory = getattr(_deps, dep_name)
with sw.step(key):
tags = _detect_tags_on(factory(), frame_label, ids, probe_size_mm)
cameras[key] = {"tags": tags, "frame": frame_label}
except Exception as e:
cameras[key] = {"tags": [], "frame": frame_label,
"error": f"{type(e).__name__}: {e}"}
return {"cameras": cameras, "timings_ms": sw.as_dict()}
@router.post("/align")
def post_align(ids: str = "0-49", align_id: int = 0,
probe_size_mm: float = 40.0) -> dict[str, Any]:
"""Calibrazione cross-camera: trova `align_id` su Nuwa e D435i, fitta una
similarita' (rot+trasl+scala) Nuwa-optical -> D435-optical e la PERSISTE in
config.yaml > pose_3d.cross_camera. Se il tag non e' visto da entrambe, NON
sovrascrive la matrice salvata."""
sw = Stopwatch()
found: dict[str, Optional[dict]] = {}
cam_errors: dict[str, str] = {}
for key, (frame_label, dep_name) in _TAG_CAMERAS.items():
try:
factory = getattr(_deps, dep_name)
with sw.step(key):
tags = _detect_tags_on(factory(), frame_label, ids, probe_size_mm)
found[key] = next((t for t in tags if int(t["id"]) == int(align_id)), None)
except Exception as e:
log.warning("align: camera %s failed: %s", key, e)
found[key] = None
cam_errors[key] = f"{type(e).__name__}: {e}"
missing = [k for k, v in found.items() if v is None]
if missing:
if cam_errors:
detail = "; ".join(f"{k}: {msg}" for k, msg in cam_errors.items())
err = f"camera non disponibile -> {detail}"
else:
err = f"tag {align_id} non visto da: {', '.join(missing)}"
return {"found_in_both": False, "align_id": int(align_id),
"error": err, "timings_ms": sw.as_dict()}
# I corner sono accoppiati posizionalmente: stesso marker-finder + stessa
# geometria ArUco -> ordine corner deterministico e identico tra le camere.
src, dst = [], []
for cn, cd in zip(found["nuwa"]["corners_3d_optical_m"],
found["d435"]["corners_3d_optical_m"]):
if any(x != x for x in cn) or any(x != x for x in cd):
continue
src.append(cn)
dst.append(cd)
if len(src) < 3:
return {"found_in_both": False, "align_id": int(align_id),
"error": f"corner validi insufficienti ({len(src)}/4)",
"timings_ms": sw.as_dict()}
try:
with sw.step("umeyama"):
scale, R, t, rmse = umeyama_similarity(np.array(src), np.array(dst))
except ValueError as e:
return {"found_in_both": False, "align_id": int(align_id),
"error": f"fit fallito: {e}", "timings_ms": sw.as_dict()}
payload = {
"align_id": int(align_id),
"scale": float(scale),
"R": [[float(x) for x in row] for row in R],
"t": [float(x) for x in t],
"rmse_mm": float(rmse * 1000.0),
"n_corners": len(src),
}
update_config({"pose_3d": {"cross_camera": payload}})
return {"found_in_both": True, **payload, "timings_ms": sw.as_dict()}
# --- 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") @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."""
+38
View File
@@ -142,3 +142,41 @@ def quaternion_xyzw_to_rpy_deg(quat_xyzw) -> tuple[float, float, float]:
rpy_rad = R_to_euler_xyz(R) rpy_rad = R_to_euler_xyz(R)
rpy_deg = np.degrees(rpy_rad) rpy_deg = np.degrees(rpy_rad)
return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2]) return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2])
def umeyama_similarity(src: np.ndarray, dst: np.ndarray) -> tuple[float, np.ndarray, np.ndarray, float]:
"""Fit dst ≈ s·R·src + t (Umeyama 1991, con scala uniforme).
src, dst: (N,3) array di punti corrispondenti.
Ritorna (scale: float, R: (3,3), t: (3,), rmse: float in unità di dst).
Solleva ValueError se N<3 o il set è degenere (varianza nulla).
"""
src = np.asarray(src, dtype=np.float64).reshape(-1, 3)
dst = np.asarray(dst, dtype=np.float64).reshape(-1, 3)
if src.shape != dst.shape:
raise ValueError(f"shape mismatch: {src.shape} vs {dst.shape}")
n = src.shape[0]
if n < 3:
raise ValueError(f"servono almeno 3 punti, ricevuti {n}")
mu_src = src.mean(axis=0)
mu_dst = dst.mean(axis=0)
src_c = src - mu_src
dst_c = dst - mu_dst
var_src = (src_c ** 2).sum() / n
if var_src < 1e-12:
raise ValueError("punti src degeneri (varianza nulla)")
cov = (dst_c.T @ src_c) / n
U, D, Vt = np.linalg.svd(cov)
S = np.eye(3)
if np.linalg.det(U) * np.linalg.det(Vt) < 0:
S[2, 2] = -1.0 # vieta riflessioni
R = U @ S @ Vt
scale = float((D * np.diag(S)).sum() / var_src)
t = mu_dst - scale * (R @ mu_src)
pred = (scale * (R @ src.T)).T + t
rmse = float(np.sqrt(((pred - dst) ** 2).sum(axis=1).mean()))
return scale, R, t, rmse
+8 -1
View File
@@ -25,6 +25,7 @@ from .api import (
) )
from .clients import G1StatusError from .clients import G1StatusError
from .config import load_config from .config import load_config
from .version import build_info
logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s") logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s")
@@ -102,12 +103,18 @@ def health() -> dict:
return {"status": "ok"} return {"status": "ok"}
@app.get("/api/version", tags=["meta"])
def version() -> dict:
return build_info()
@app.get("/", include_in_schema=False) @app.get("/", include_in_schema=False)
def index() -> HTMLResponse: def index() -> HTMLResponse:
idx = WEB_DIR / "index.html" idx = WEB_DIR / "index.html"
html = idx.read_text(encoding="utf-8") html = idx.read_text(encoding="utf-8")
# Cache-bust app.js and style.css with their mtime so a rebuild forces refetch. # 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 f = WEB_DIR / name
if f.exists(): if f.exists():
v = int(f.stat().st_mtime) v = int(f.stat().st_mtime)
+35
View File
@@ -0,0 +1,35 @@
"""Versione build dell'app, mostrata nel badge in GUI.
In Docker i valori arrivano da env (`APP_VERSION`, `BUILD_TIME`) iniettati come
build-arg da `build.sh`. In dev locale (`uv run`) gli env non ci sono: si
ripiega sul contenuto del file `VERSION` nella root del repo + suffisso
`(local)`.
"""
from __future__ import annotations
import os
from pathlib import Path
_VERSION_FILE = Path(__file__).resolve().parents[2] / "VERSION"
def _file_version() -> str:
try:
return _VERSION_FILE.read_text(encoding="utf-8").strip() or "0.0.0"
except OSError:
return "0.0.0"
def build_info() -> dict:
"""Ritorna {'version', 'build_time'}.
Preferisce gli env settati al build dell'immagine; in loro assenza ripiega
sul file VERSION marcando la versione come `(local)`.
"""
version = os.environ.get("APP_VERSION")
build_time = os.environ.get("BUILD_TIME")
if not version:
version = f"{_file_version()} (local)"
if not build_time:
build_time = "(local)"
return {"version": version, "build_time": build_time}
+49
View File
@@ -0,0 +1,49 @@
"""Unit test per umeyama_similarity (absolute orientation con scala)."""
from __future__ import annotations
import numpy as np
import pytest
from presa_mano.geometry import umeyama_similarity, rpy_deg_to_quaternion_xyzw, quat_xyzw_to_R
def _known_transform():
# scala 1.5, rotazione nota, traslazione nota
R = quat_xyzw_to_R(rpy_deg_to_quaternion_xyzw(10.0, -20.0, 35.0))
s = 1.5
t = np.array([0.3, -0.1, 0.2])
return s, R, t
def test_recovers_known_similarity():
s, R, t = _known_transform()
rng = np.random.default_rng(0)
src = rng.normal(size=(8, 3))
dst = (s * (R @ src.T)).T + t
s_hat, R_hat, t_hat, rmse = umeyama_similarity(src, dst)
assert abs(s_hat - s) < 1e-6
assert np.allclose(R_hat, R, atol=1e-6)
assert np.allclose(t_hat, t, atol=1e-6)
assert rmse < 1e-6
def test_forbids_reflection():
rng = np.random.default_rng(1)
src = rng.normal(size=(5, 3))
dst = rng.normal(size=(5, 3))
_, R_hat, _, _ = umeyama_similarity(src, dst)
assert np.linalg.det(R_hat) > 0
def test_raises_on_too_few_points():
with pytest.raises(ValueError):
umeyama_similarity(np.zeros((2, 3)), np.zeros((2, 3)))
def test_raises_on_degenerate_points():
src = np.ones((4, 3))
dst = np.ones((4, 3)) * 2.0
with pytest.raises(ValueError):
umeyama_similarity(src, dst)
+85
View File
@@ -0,0 +1,85 @@
"""Test POST /api/pose_3d/align: fit Umeyama + persistenza config."""
from __future__ import annotations
from unittest.mock import patch, MagicMock
import numpy as np
from fastapi.testclient import TestClient
from presa_mano.main import app
from presa_mano.config import get_config
client = TestClient(app)
_CORNERS_PX = [[100, 100], [140, 100], [140, 140], [100, 140]]
def _cam_with_points(points_by_pixel):
cam = MagicMock()
cam.intrinsics.return_value = {"rgb": {"fx": 600.0, "fy": 600.0, "cx": 320.0, "cy": 240.0}}
cam.rgb_url.return_value = "http://cam/rgb.png"
def _measure(u, v, radius=2):
key = (int(u), int(v))
if key in points_by_pixel:
x, y, z = points_by_pixel[key]
else:
xs = [p[0] for p in points_by_pixel.values()]
ys = [p[1] for p in points_by_pixel.values()]
zs = [p[2] for p in points_by_pixel.values()]
x, y, z = sum(xs)/len(xs), sum(ys)/len(ys), sum(zs)/len(zs)
return {"point_m": {"x": x, "y": y, "z": z}, "depth_mm": int(z * 1000)}
cam.measure.side_effect = _measure
return cam
def _marker(found_ids):
mk = MagicMock()
mk.detect_url.return_value = {
"found": [{"id": i, "corners": _CORNERS_PX} for i in found_ids],
"not_found": [],
}
return mk
def test_align_fits_and_persists():
nuwa_pts = {
(100, 100): (0.10, 0.10, 0.30),
(140, 100): (0.14, 0.10, 0.30),
(140, 140): (0.14, 0.14, 0.30),
(100, 140): (0.10, 0.14, 0.30),
}
d435_pts = {k: (v[0]*2, v[1]*2, v[2]*2) for k, v in nuwa_pts.items()}
with patch("presa_mano.api.pose_3d_routes._deps.nuwa") as nuwa_f, \
patch("presa_mano.api.pose_3d_routes._deps.front_camera") as front_f, \
patch("presa_mano.api.pose_3d_routes._deps.marker") as mk_f:
nuwa_f.return_value = _cam_with_points(nuwa_pts)
front_f.return_value = _cam_with_points(d435_pts)
mk_f.return_value = _marker([7])
r = client.post("/api/pose_3d/align?ids=0-9&align_id=7")
assert r.status_code == 200
body = r.json()
assert body["found_in_both"] is True
assert abs(body["scale"] - 2.0) < 1e-3
assert body["align_id"] == 7
cc = get_config()["pose_3d"]["cross_camera"]
assert abs(cc["scale"] - 2.0) < 1e-3
assert cc["align_id"] == 7
assert len(cc["R"]) == 3 and len(cc["t"]) == 3
def test_align_tag_missing_in_one_camera_does_not_save():
pts = {(100,100):(0.1,0.1,0.3),(140,100):(0.14,0.1,0.3),
(140,140):(0.14,0.14,0.3),(100,140):(0.1,0.14,0.3)}
with patch("presa_mano.api.pose_3d_routes._deps.nuwa") as nuwa_f, \
patch("presa_mano.api.pose_3d_routes._deps.front_camera") as front_f, \
patch("presa_mano.api.pose_3d_routes._deps.marker") as mk_f:
nuwa_f.return_value = _cam_with_points(pts)
front_f.return_value = _cam_with_points(pts)
mk_f.return_value = _marker([1]) # nessun tag id=99
r = client.post("/api/pose_3d/align?ids=0-9&align_id=99")
assert r.status_code == 200
body = r.json()
assert body["found_in_both"] is False
assert body.get("error")
assert "cross_camera" not in get_config().get("pose_3d", {})
+10
View File
@@ -208,3 +208,13 @@ def test_unlock_proxies_to_bridge():
r = client.post("/api/pose_3d/unlock") r = client.post("/api/pose_3d/unlock")
assert r.status_code == 200 assert r.status_code == 200
assert r.json()["ok"] is True assert r.json()["ok"] is True
def test_state_includes_cross_camera_block():
with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
g1 = MagicMock()
g1.wrist.return_value = _fake_wrist_pair()
g1_factory.return_value = g1
r = client.get("/api/pose_3d/state")
assert r.status_code == 200
assert "cross_camera" in r.json() # {} se mai calibrato
+75
View File
@@ -0,0 +1,75 @@
"""Test del nuovo endpoint multi-camera GET /api/pose_3d/tags."""
from __future__ import annotations
from unittest.mock import patch, MagicMock
from fastapi.testclient import TestClient
from presa_mano.main import app
client = TestClient(app)
def _fake_camera(corner_depth_m=0.32):
cam = MagicMock()
cam.intrinsics.return_value = {
"rgb": {"fx": 600.0, "fy": 600.0, "cx": 320.0, "cy": 240.0}
}
cam.rgb_url.return_value = "http://cam/rgb.png"
def _measure(u, v, radius=2):
return {"point_m": {"x": u / 1000.0, "y": v / 1000.0, "z": corner_depth_m},
"depth_mm": int(corner_depth_m * 1000)}
cam.measure.side_effect = _measure
return cam
def _fake_marker(found_ids):
mk = MagicMock()
found = [{
"id": i,
"corners": [[100, 100], [140, 100], [140, 140], [100, 140]],
} for i in found_ids]
mk.detect_url.return_value = {"found": found, "not_found": []}
return mk
def test_tags_returns_both_cameras():
with patch("presa_mano.api.pose_3d_routes._deps.nuwa") as nuwa_f, \
patch("presa_mano.api.pose_3d_routes._deps.front_camera") as front_f, \
patch("presa_mano.api.pose_3d_routes._deps.marker") as mk_f:
nuwa_f.return_value = _fake_camera()
front_f.return_value = _fake_camera()
mk_f.return_value = _fake_marker([5])
r = client.get("/api/pose_3d/tags?ids=0-9")
assert r.status_code == 200
body = r.json()
assert set(body["cameras"].keys()) == {"nuwa", "d435"}
assert body["cameras"]["nuwa"]["frame"] == "nuwa_rgb_optical"
assert body["cameras"]["d435"]["frame"] == "d435_rgb_optical"
assert body["cameras"]["nuwa"]["tags"][0]["id"] == 5
assert body["cameras"]["d435"]["tags"][0]["id"] == 5
tag = body["cameras"]["nuwa"]["tags"][0]
assert tag["edge_mm"] is not None
assert 35.0 < tag["edge_mm"] < 45.0 # ~40mm lato del quad
assert tag["distance_m"] is not None
assert abs(tag["distance_m"] - 0.362) < 0.02 # sqrt(.12^2+.12^2+.32^2)
assert all(len(c) == 3 for c in tag["corners_3d_optical_m"])
def test_tags_one_camera_offline_reports_error():
with patch("presa_mano.api.pose_3d_routes._deps.nuwa") as nuwa_f, \
patch("presa_mano.api.pose_3d_routes._deps.front_camera") as front_f, \
patch("presa_mano.api.pose_3d_routes._deps.marker") as mk_f:
nuwa = MagicMock()
nuwa.intrinsics.side_effect = RuntimeError("nuwa down")
nuwa_f.return_value = nuwa
front_f.return_value = _fake_camera()
mk_f.return_value = _fake_marker([5])
r = client.get("/api/pose_3d/tags?ids=0-9")
assert r.status_code == 200
body = r.json()
assert body["cameras"]["nuwa"]["tags"] == []
assert "error" in body["cameras"]["nuwa"]
assert body["cameras"]["d435"]["tags"][0]["id"] == 5
+25
View File
@@ -0,0 +1,25 @@
"""Unit test per build_info(): env (Docker) vs fallback VERSION file (local)."""
from __future__ import annotations
from pathlib import Path
from presa_mano.version import build_info
_VERSION_FILE = Path(__file__).resolve().parents[1] / "VERSION"
def test_build_info_prefers_env(monkeypatch):
monkeypatch.setenv("APP_VERSION", "1.2.3")
monkeypatch.setenv("BUILD_TIME", "2026-06-03 14:22 UTC")
info = build_info()
assert info == {"version": "1.2.3", "build_time": "2026-06-03 14:22 UTC"}
def test_build_info_falls_back_to_version_file(monkeypatch):
monkeypatch.delenv("APP_VERSION", raising=False)
monkeypatch.delenv("BUILD_TIME", raising=False)
info = build_info()
file_ver = _VERSION_FILE.read_text(encoding="utf-8").strip()
assert file_ver in info["version"]
assert info["version"].endswith("(local)")
assert info["build_time"] == "(local)"
+10
View File
@@ -742,6 +742,16 @@ async function refreshHomeRobotSelector() {
refreshHomeRobotSelector(); refreshHomeRobotSelector();
startHomePoll(); startHomePoll();
// Badge versione build (header). Si aggiorna ad ogni rebuild via build.sh.
async function refreshAppVersion() {
try {
const r = await api('GET', '/api/version');
const pill = $('appVersionPill');
if (pill) { pill.textContent = `V${r.version} · ${r.build_time}`; pill.title = `versione ${r.version} · build ${r.build_time}`; }
} catch {}
}
refreshAppVersion();
// ---------- init ---------- // ---------- init ----------
(async () => { (async () => {
try { try {
+56
View File
@@ -20,6 +20,7 @@
</nav> </nav>
<div class="status-strip" id="statusStrip"></div> <div class="status-strip" id="statusStrip"></div>
<span class="active-robot-pill" id="activeRobotPill" title="Robot attualmente collegato">robot: —</span> <span class="active-robot-pill" id="activeRobotPill" title="Robot attualmente collegato">robot: —</span>
<span class="active-robot-pill" id="appVersionPill" title="versione build">V—</span>
</header> </header>
<main> <main>
@@ -320,6 +321,7 @@
<div class="p3-help">click polso = seleziona · gizmo: T sposta / R ruota · drag = muovi · orbit = wheel/drag fondo · pan = shift+drag</div> <div class="p3-help">click polso = seleziona · gizmo: T sposta / R ruota · drag = muovi · orbit = wheel/drag fondo · pan = shift+drag</div>
</div> </div>
<aside class="p3-sidebar"> <aside class="p3-sidebar">
<div class="p3-group" data-group="pose">
<div class="p3-section"> <div class="p3-section">
<h3>Modalità</h3> <h3>Modalità</h3>
<div class="p3-row"> <div class="p3-row">
@@ -388,6 +390,9 @@
</div> </div>
</div> </div>
</div><!-- /p3-group pose -->
<div class="p3-group" data-group="cameras" hidden>
<div class="p3-section"> <div class="p3-section">
<h3>Camera Nuwa (polso DX)</h3> <h3>Camera Nuwa (polso DX)</h3>
<div class="p3-row"> <div class="p3-row">
@@ -445,6 +450,12 @@
<input id="p3D435ScaleXY" type="number" min="0.1" max="3" step="0.01" value="0.65" style="width:60px"> <input id="p3D435ScaleXY" type="number" min="0.1" max="3" step="0.01" value="0.65" style="width:60px">
</label> </label>
</div> </div>
<div class="p3-row" title="Calibrazione persistente Nuwa→frontale via tag condiviso (Umeyama). Premi Allinea con il tag visto da entrambe le camere.">
<label><input type="checkbox" id="p3AlignEnabled"> Allinea Nuwa→frontale</label>
<label>tag id <input id="p3AlignTagId" type="number" min="0" max="999" value="0" style="width:55px"></label>
<button id="p3AlignBtn">Allinea</button>
</div>
<div class="marker-info" id="p3AlignStatus">—</div>
<div class="p3-row"> <div class="p3-row">
<label>step <input id="p3CloudStep" type="number" min="1" max="32" value="8" style="width:50px"></label> <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> <label>max <input id="p3CloudMaxDepth" type="number" min="100" max="5000" step="100" value="2000" style="width:70px"> mm</label>
@@ -460,6 +471,34 @@
<div class="marker-info" id="p3CloudStatus">—</div> <div class="marker-info" id="p3CloudStatus">—</div>
</div> </div>
</div><!-- /p3-group cameras -->
<div class="p3-group" data-group="misc" hidden>
<div class="p3-section">
<h3>Tag locator</h3>
<div class="p3-row">
<label><input type="checkbox" id="p3TagPlaceMode"> <b>Modalità tag click</b> (click sul modello = piazza crocetta)</label>
</div>
<div class="p3-row">
<label>Tag
<select id="p3TagSelect">
<option value="10:40:wrist_sx">wrist sx (id=10, 40mm)</option>
<option value="1:20:nuwa_dx">nuwa dx (id=1, 20mm)</option>
</select>
</label>
</div>
<div class="p3-row">
<label>x <input id="p3TagPosX" type="number" readonly style="width:70px"> mm</label>
<label>y <input id="p3TagPosY" type="number" readonly style="width:70px"> mm</label>
<label>z <input id="p3TagPosZ" type="number" readonly style="width:70px"> mm</label>
</div>
<div class="p3-row">
<button id="p3TagSaveBtn" disabled>Salva</button>
<button id="p3TagClearBtn" disabled>✕ rimuovi</button>
</div>
<div class="marker-info" id="p3TagStatus">attiva la modalità tag click sopra, poi clicca sul modello 3D</div>
</div>
<div class="p3-section"> <div class="p3-section">
<h3>Debug</h3> <h3>Debug</h3>
<div class="p3-row"> <div class="p3-row">
@@ -478,6 +517,17 @@
</div> </div>
</div> </div>
<div class="p3-section">
<h3>Tags</h3>
<div class="p3-row">
<label><input type="checkbox" id="p3ShowFrontTags"> Mostra tag visibili (quad + dim + dist)</label>
</div>
<div class="p3-row">
<label>IDs <input id="p3FrontTagIds" type="text" value="0-49" style="width:90px"></label>
</div>
<div class="marker-info" id="p3FrontTagStatus">—</div>
</div>
<div class="p3-section"> <div class="p3-section">
<button id="p3UnlockBtn">Sblocca braccia</button> <button id="p3UnlockBtn">Sblocca braccia</button>
</div> </div>
@@ -485,7 +535,13 @@
<div class="p3-section" id="p3ErrorBox" style="display:none"> <div class="p3-section" id="p3ErrorBox" style="display:none">
<div class="p3-error" id="p3ErrorText"></div> <div class="p3-error" id="p3ErrorText"></div>
</div> </div>
</div><!-- /p3-group misc -->
</aside> </aside>
<nav class="p3-tabrail" aria-label="Pose 3D groups">
<button type="button" class="p3-tab active" data-target="pose">POSE</button>
<button type="button" class="p3-tab" data-target="cameras">CAMERAS</button>
<button type="button" class="p3-tab" data-target="misc">MISC</button>
</nav>
</div> </div>
</section> </section>
+30 -1
View File
@@ -1,11 +1,40 @@
#tab-pose-3d .p3-layout { #tab-pose-3d .p3-layout {
display: grid; display: grid;
grid-template-columns: 1fr 360px; grid-template-columns: 1fr 360px 92px;
gap: 16px; gap: 16px;
height: calc(100vh - 140px); height: calc(100vh - 140px);
min-height: 600px; 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 { #tab-pose-3d .p3-viewport {
position: relative; position: relative;
background: #0d1014; background: #0d1014;
+587 -3
View File
@@ -222,6 +222,16 @@ const state = {
joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug) joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug)
clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config
cloud_points: { nuwa: null, d435: null }, // THREE.Points obj per camera cloud_points: { nuwa: null, d435: null }, // THREE.Points obj per camera
align: { enabled: false, id: 0, matrix: null /* THREE.Matrix4 */ },
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)
},
tags: { // overlay tag per camera (gruppi THREE child del cloud node)
nuwa: { group: null },
d435: { group: null },
},
}; };
function checkWorkspace(side, position_m) { function checkWorkspace(side, position_m) {
@@ -773,8 +783,11 @@ async function pollState() {
applyNuwaTransform(); applyNuwaTransform();
updateNuwaInputsFromCfg(); updateNuwaInputsFromCfg();
updateD435InputsFromCfg(); updateD435InputsFromCfg();
applyCloudParamsFromCfg(s.cloud_params);
applyCrossCameraFromCfg(s.cross_camera);
applyCalibratedD435UCS(); applyCalibratedD435UCS();
// Applica TUTTI i giunti reali sul modello URDF (29 motori G1) // 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); if (s.joints) applyJointsToUrdf(s.joints);
updateRealWrists(); updateRealWrists();
syncGhostsToTargetOrReal(); syncGhostsToTargetOrReal();
@@ -785,6 +798,9 @@ async function pollState() {
// soft fail: log + segna lo stato // soft fail: log + segna lo stato
console.warn('pose_3d /state fail:', e.message); console.warn('pose_3d /state fail:', e.message);
} }
// NB: il front-tag overlay NON viene aggiornato qui. La detection ArUco
// gira solo al cambio dell'immagine (pulsante Acquisisci -> acquireClouds)
// e al toggle on del flag, non ad ogni tick del poll dello state.
} }
function refreshPoseSelect() { function refreshPoseSelect() {
@@ -806,6 +822,203 @@ function refreshPoseSelect() {
} }
// ----- selezione polso via click ----- // ----- 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() { function setupSelection() {
const { renderer, camera, transform } = state.three; const { renderer, camera, transform } = state.three;
const raycaster = new THREE.Raycaster(); const raycaster = new THREE.Raycaster();
@@ -819,7 +1032,14 @@ function setupSelection() {
mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1; mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1;
mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1; mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1;
raycaster.setFromCamera(mouse, camera); 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, const candidates = [state.arms.right.userData.sphere,
state.arms.left.userData.sphere]; state.arms.left.userData.sphere];
const hits = raycaster.intersectObjects(candidates, false); const hits = raycaster.intersectObjects(candidates, false);
@@ -894,6 +1114,12 @@ function setGizmoMode(mode) {
function bindGizmoEvents() { function bindGizmoEvents() {
const { transform } = state.three; const { transform } = state.three;
transform.addEventListener('change', () => { 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 side = state.active_side;
const ghost = state.ghosts[side]; const ghost = state.ghosts[side];
if (!ghost) return; if (!ghost) return;
@@ -1381,8 +1607,12 @@ function buildOrUpdateCloud(camKey, pointsFlat) {
`Y[${ymin.toFixed(2)},${ymax.toFixed(2)}]`, `Y[${ymin.toFixed(2)},${ymax.toFixed(2)}]`,
`Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`); `Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`);
// (Ri-)attacca al parent corretto e applica transform optical→link // (Ri-)attacca al parent corretto e applica transform optical→link
if (pts.parent !== parent) parent.add(pts); if (camKey === 'nuwa') {
applyCloudTransform(camKey); applyNuwaCloudPlacement();
} else {
if (pts.parent !== parent) parent.add(pts);
applyCloudTransform(camKey);
}
applyCloudVisibility(); applyCloudVisibility();
return pts; return pts;
} }
@@ -1397,6 +1627,24 @@ function applyCloudVisibility() {
// Applica position+rpy+mirror sui punti del cloud `camKey`. Il mirror (sign // 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 // 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). // (lo scale e' la prima operazione applicata al punto locale).
// Matrix4 locale (rispetto al parent getCloudParent('d435')) usata dal cloud
// D435i: hand-eye (M·S) oppure URDF (T·R·S). Riusata per piazzare la Nuwa
// allineata nello stesso frame del D435i.
function d435LocalMatrix() {
const sv = readCloudScaleVec('d435');
const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]);
if (useD435Handeye()) {
const M = getD435HandeyeMatrix();
if (M) return M.clone().multiply(S);
}
const t = getCloudTransform('d435');
const T = new THREE.Matrix4().makeTranslation(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');
const R = new THREE.Matrix4().makeRotationFromEuler(e);
return T.multiply(R).multiply(S);
}
function applyCloudTransform(camKey) { function applyCloudTransform(camKey) {
const pts = state.cloud_points[camKey]; const pts = state.cloud_points[camKey];
if (!pts) return; if (!pts) return;
@@ -1428,6 +1676,34 @@ function applyCloudTransform(camKey) {
pts.scale.set(sv[0], sv[1], sv[2]); pts.scale.set(sv[0], sv[1], sv[2]);
} }
// Piazza la nuvola Nuwa: in modalità allineata la renderizza nel frame del
// D435i (parent + pipeline D435i · matrice di allineamento); altrimenti usa il
// piazzamento URDF normale.
function applyNuwaCloudPlacement() {
const pts = state.cloud_points.nuwa;
if (!pts) return;
const aligned = state.align.enabled && state.align.matrix;
if (aligned) {
const parent = getCloudParent('d435');
if (parent) {
if (pts.parent !== parent) parent.add(pts);
pts.matrixAutoUpdate = false;
pts.matrix.copy(d435LocalMatrix()).multiply(state.align.matrix);
pts.matrixWorldNeedsUpdate = true;
return;
}
}
if (state.align.enabled && !state.align.matrix) {
const s = $('p3AlignStatus');
if (s) { s.textContent = 'nessuna calibrazione: premi Allinea'; s.style.color = 'var(--dim)'; }
}
// fallback URDF normale
const parent = getCloudParent('nuwa');
if (parent && pts.parent !== parent) parent.add(pts);
pts.matrixAutoUpdate = true;
applyCloudTransform('nuwa');
}
function readCloudFlip(camKey) { function readCloudFlip(camKey) {
const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435'; const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435';
return [ return [
@@ -1453,6 +1729,64 @@ function readCloudScaleVec(camKey) {
return [f[0] * sc[0], f[1] * sc[1], f[2] * sc[2]]; 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() { async function acquireClouds() {
// Clamp dei parametri: step oltre ~32 fa 422 sul server pointcloud. // Clamp dei parametri: step oltre ~32 fa 422 sul server pointcloud.
let step = parseInt($('p3CloudStep').value, 10); let step = parseInt($('p3CloudStep').value, 10);
@@ -1481,6 +1815,12 @@ async function acquireClouds() {
} }
$('p3CloudStatus').textContent = parts.join(' · '); $('p3CloudStatus').textContent = parts.join(' · ');
$('p3CloudStatus').style.color = 'var(--ok)'; $('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();
// Nuova immagine acquisita: se l'overlay tag e' attivo ri-esegui la
// detection una volta sola, sul frame appena acquisito.
if ($('p3ShowFrontTags')?.checked) pollTags();
} catch (e) { } catch (e) {
$('p3CloudStatus').textContent = `errore: ${e.message}`; $('p3CloudStatus').textContent = `errore: ${e.message}`;
$('p3CloudStatus').style.color = 'var(--danger)'; $('p3CloudStatus').style.color = 'var(--danger)';
@@ -1500,8 +1840,233 @@ function clearClouds() {
$('p3CloudStatus').style.color = ''; $('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 NUWA_TAG_COLOR = 0xff3b30; // rosso: tag visti dalla camera Nuwa (polso)
const TAG_COLORS = { nuwa: NUWA_TAG_COLOR, d435: FRONT_TAG_COLOR };
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 clearTagsGroup(camKey) {
const grp = state.tags[camKey].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.tags[camKey].group = null;
}
function addOneTag(grp, t, color) {
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;
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));
grp.add(new THREE.Line(geom, new THREE.LineBasicMaterial({ color })));
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)) sprite.position.set(ctr[0], ctr[1] - 0.04, ctr[2]);
grp.add(sprite);
}
function renderTags(camKey, tags, color) {
clearTagsGroup(camKey);
if (!tags || tags.length === 0) return;
const parent = getCloudParent(camKey);
if (!parent) { console.warn(`[pose_3d] tags: parent ${camKey} non disponibile`); return; }
const grp = new THREE.Group();
grp.name = `${camKey}_tags_group`;
applyCloudLikeTransform(grp, camKey);
parent.add(grp);
state.tags[camKey].group = grp;
for (const t of tags) addOneTag(grp, t, color);
}
async function pollTags() {
const ids = ($('p3FrontTagIds')?.value || '0-49').trim();
const status = $('p3FrontTagStatus');
try {
const r = await jsonFetch(`/api/pose_3d/tags?ids=${encodeURIComponent(ids)}`);
const cams = r.cameras || {};
const parts = [];
for (const camKey of ['nuwa', 'd435']) {
const c = cams[camKey] || {};
renderTags(camKey, c.tags || [], TAG_COLORS[camKey]);
if (c.error) parts.push(`${camKey}: err`);
else parts.push(`${camKey}: ${(c.tags || []).length}`);
}
if (status) status.textContent = parts.join(' · ');
} catch (e) {
if (status) status.textContent = `errore: ${e?.message || e}`;
}
}
// Costruisce una THREE.Matrix4 (row-major via .set) da {scale,R,t}: p' = s·R·p + t.
function matrixFromAlign(cc) {
if (!cc || !Array.isArray(cc.R) || cc.R.length !== 3 || !Array.isArray(cc.t)) return null;
const s = Number.isFinite(cc.scale) ? cc.scale : 1;
const R = cc.R, t = cc.t;
return new THREE.Matrix4().set(
s*R[0][0], s*R[0][1], s*R[0][2], t[0],
s*R[1][0], s*R[1][1], s*R[1][2], t[1],
s*R[2][0], s*R[2][1], s*R[2][2], t[2],
0, 0, 0, 1,
);
}
async function doAlign() {
const ids = ($('p3FrontTagIds')?.value || '0-49').trim();
const alignId = parseInt($('p3AlignTagId')?.value, 10) || 0;
const status = $('p3AlignStatus');
if (status) { status.textContent = 'allineamento…'; status.style.color = 'var(--dim)'; }
try {
const r = await jsonFetch(
`/api/pose_3d/align?ids=${encodeURIComponent(ids)}&align_id=${alignId}`,
{ method: 'POST' });
if (!r.found_in_both) {
if (status) { status.textContent = r.error || 'tag non visto da entrambe'; status.style.color = 'var(--danger)'; }
return;
}
state.align.matrix = matrixFromAlign(r);
state.align.id = r.align_id;
if (status) {
status.textContent = `scala=${r.scale.toFixed(3)} · rmse=${r.rmse_mm.toFixed(1)}mm · n=${r.n_corners}`;
status.style.color = 'var(--ok)';
}
applyNuwaCloudPlacement();
} catch (e) {
if (status) { status.textContent = `errore: ${e?.message || e}`; status.style.color = 'var(--danger)'; }
}
}
// Persistenza del solo flag + align_id (la matrice la salva il backend in /align).
async function saveCrossCameraFlag() {
try {
await jsonFetch('/api/config', {
method: 'PUT',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ pose_3d: { cross_camera: {
enabled: $('p3AlignEnabled')?.checked === true,
align_id: parseInt($('p3AlignTagId')?.value, 10) || 0,
} } }),
});
} catch (e) { console.warn('[pose_3d] save cross_camera failed:', e?.message || e); }
}
// Ripopola UI + matrice dalla config (guardato come applyCloudParamsFromCfg).
function applyCrossCameraFromCfg(cc) {
if (state.cross_camera_populated) return;
if (!cc || Object.keys(cc).length === 0) { state.cross_camera_populated = true; return; }
const en = $('p3AlignEnabled'); if (en) en.checked = cc.enabled === true;
const id = $('p3AlignTagId'); if (id && Number.isFinite(cc.align_id)) id.value = String(cc.align_id);
state.align.enabled = cc.enabled === true;
state.align.id = cc.align_id || 0;
const m = matrixFromAlign(cc);
if (m) state.align.matrix = m;
const status = $('p3AlignStatus');
if (status && Number.isFinite(cc.rmse_mm)) {
status.textContent = `caricata · scala=${(cc.scale||1).toFixed(3)} · rmse=${cc.rmse_mm.toFixed(1)}mm`;
}
state.cross_camera_populated = true;
}
function bindFrontTagFinder() {
const cb = $('p3ShowFrontTags');
if (!cb) return;
cb.addEventListener('change', () => {
if (cb.checked) {
pollTags();
} else {
clearTagsGroup('nuwa');
clearTagsGroup('d435');
const s = $('p3FrontTagStatus');
if (s) s.textContent = '—';
}
});
}
function bindCloudControls() { function bindCloudControls() {
$('p3CloudAcquireBtn')?.addEventListener('click', acquireClouds); $('p3CloudAcquireBtn')?.addEventListener('click', acquireClouds);
$('p3AlignBtn')?.addEventListener('click', doAlign);
$('p3AlignEnabled')?.addEventListener('change', () => {
state.align.enabled = $('p3AlignEnabled').checked === true;
saveCrossCameraFlag();
applyNuwaCloudPlacement();
});
$('p3AlignTagId')?.addEventListener('change', saveCrossCameraFlag);
$('p3CloudClearBtn')?.addEventListener('click', clearClouds); $('p3CloudClearBtn')?.addEventListener('click', clearClouds);
$('p3ShowNuwaCloud')?.addEventListener('change', applyCloudVisibility); $('p3ShowNuwaCloud')?.addEventListener('change', applyCloudVisibility);
$('p3ShowD435Cloud')?.addEventListener('change', applyCloudVisibility); $('p3ShowD435Cloud')?.addEventListener('change', applyCloudVisibility);
@@ -1693,6 +2258,22 @@ function bindD435Controls() {
$('p3D435ResetBtn')?.addEventListener('click', onD435Reset); $('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 ----- // ----- init triggered on first activation -----
let inited = false; let inited = false;
function ensureInit() { function ensureInit() {
@@ -1701,6 +2282,7 @@ function ensureInit() {
inited = true; inited = true;
initScene(); initScene();
setupSelection(); setupSelection();
setupTagLocator();
bindSideControls(); bindSideControls();
bindGizmoControls(); bindGizmoControls();
bindGizmoEvents(); bindGizmoEvents();
@@ -1715,6 +2297,8 @@ function ensureInit() {
bindJointUCSControls(); bindJointUCSControls();
bindCloudControls(); bindCloudControls();
bindReadoutInputs(); bindReadoutInputs();
bindTabRail();
bindFrontTagFinder();
pollState(); pollState();
pollTimer = setInterval(pollState, POLL_MS); pollTimer = setInterval(pollState, POLL_MS);
} }