# Tab "Braccio DX" Implementation Plan > **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. **Goal:** Aggiungere una tab "Braccio DX" alla GUI per operare il braccio destro di G1 con due camere (fronte fissa + Nuwa eye-in-hand sul polso): visione YOLO/SAM21 on-demand, move-toward con offset configurabile, home + custom pose. **Architecture:** Un nuovo router `/api/right_arm/*` + un nuovo modulo JS lato GUI; nessun client nuovo (la front camera è Nuwa-like, riuso `NuwaClient`); logica geometrica del delta isolata in un modulo puro (`right_arm/move.py`) per testabilità. **Tech Stack:** FastAPI, Pydantic v2, httpx, NumPy, OpenCV, pytest + `unittest.mock` (no respx — coerente con i test esistenti), vanilla HTML/JS/CSS (no librerie nuove). **Spec di riferimento:** `docs/superpowers/specs/2026-05-20-right-arm-tab-design.md` --- ## File Structure **Da creare**: - `src/presa_mano/right_arm/__init__.py` — package marker - `src/presa_mano/right_arm/move.py` — pure functions: `compute_delta_wrist`, `apply_z_floor`, schema RPY conversions - `src/presa_mano/api/right_arm_routes.py` — router `/api/right_arm/*` - `web/right_arm.js` — modulo JS della tab - `tests/test_geometry_rpy.py` — unit test conversioni RPY↔quaternion - `tests/test_right_arm_move.py` — unit test math del delta + z-floor - `tests/test_right_arm_routes.py` — integration test endpoint (con `unittest.mock`) **Da modificare**: - `config.yaml` — aggiunte `services.front_camera` e blocco `tab_right_arm` - `src/presa_mano/geometry.py` — aggiungere `rpy_deg_to_quaternion_xyzw` e `quaternion_xyzw_to_rpy_deg` - `src/presa_mano/api/_deps.py` — aggiungere `front_camera()` e helper `tab_right_arm_cfg()` - `src/presa_mano/main.py` — include `right_arm_routes.router` + aggiungere `right_arm.js` al cache-busting - `web/index.html` — nuova tab `right-arm` + markup pannello + script include --- ## Task 1: Config — aggiungere `services.front_camera` e `tab_right_arm` **Files:** - Modify: `config.yaml` - Test: `tests/test_config_right_arm.py` (nuovo) - [ ] **Step 1: Write the failing test** Create `tests/test_config_right_arm.py`: ```python """Verifica che config.yaml contenga le chiavi attese per la tab Braccio DX.""" from __future__ import annotations import yaml from pathlib import Path CFG = Path(__file__).resolve().parents[1] / "config.yaml" def _cfg() -> dict: return yaml.safe_load(CFG.read_text()) def test_front_camera_service_present(): c = _cfg() fc = c["services"]["front_camera"] assert fc["base_url"].startswith("http") assert isinstance(fc.get("timeout_s"), (int, float)) def test_tab_right_arm_block_present(): c = _cfg() t = c["tab_right_arm"] assert t["yolo_class"] == "pestello" assert 0 < t["yolo_conf_front"] <= 1 assert 0 < t["yolo_conf_wrist"] <= 1 off = t["move_offset_camera_mm"] assert set(off.keys()) == {"x", "y", "z"} sgn = t["axis_sign_cam_to_wrist"] assert all(sgn[k] in (-1, 1) for k in ("x", "y", "z")) assert isinstance(t["max_step_m"], (int, float)) assert "z_floor_m" in t for k in ("home_pose", "custom_pose"): p = t[k] assert len(p["position"]) == 3 assert len(p["rpy_deg"]) == 3 ``` - [ ] **Step 2: Run test to verify it fails** Run: `uv run pytest tests/test_config_right_arm.py -v` Expected: FAIL (`KeyError: 'front_camera'`) - [ ] **Step 3: Edit `config.yaml`** Add under `services:`: ```yaml front_camera: base_url: http://192.168.123.99:8878 timeout_s: 5.0 ``` Add a new top-level block: ```yaml tab_right_arm: yolo_class: pestello yolo_conf_front: 0.4 yolo_conf_wrist: 0.4 move_offset_camera_mm: { x: 0, y: 0, z: 100 } axis_sign_cam_to_wrist: { x: 1, y: -1, z: -1 } max_step_m: 0.15 z_floor_m: null home_pose: position: [0.30, -0.20, 0.30] rpy_deg: [0, 0, 0] custom_pose: position: [0.30, -0.20, 0.30] rpy_deg: [0, 0, 0] ``` - [ ] **Step 4: Run test to verify it passes** Run: `uv run pytest tests/test_config_right_arm.py -v` Expected: PASS (3 test) - [ ] **Step 5: Commit** ```bash git add config.yaml tests/test_config_right_arm.py git commit -m "feat(config): aggiungi services.front_camera e blocco tab_right_arm" ``` --- ## Task 2: Geometry — `rpy_deg_to_quaternion_xyzw` + `quaternion_xyzw_to_rpy_deg` **Files:** - Modify: `src/presa_mano/geometry.py` - Test: `tests/test_geometry_rpy.py` (nuovo) - [ ] **Step 1: Write the failing test** Create `tests/test_geometry_rpy.py`: ```python """Roundtrip RPY (deg) ↔ quaternion xyzw.""" from __future__ import annotations import numpy as np import pytest from presa_mano.geometry import ( quaternion_xyzw_to_rpy_deg, rpy_deg_to_quaternion_xyzw, ) def test_zero_rpy_is_identity_quat(): q = rpy_deg_to_quaternion_xyzw(0.0, 0.0, 0.0) assert np.allclose(q, [0.0, 0.0, 0.0, 1.0], atol=1e-9) def test_quat_identity_is_zero_rpy(): rpy = quaternion_xyzw_to_rpy_deg([0.0, 0.0, 0.0, 1.0]) assert np.allclose(rpy, [0.0, 0.0, 0.0], atol=1e-6) @pytest.mark.parametrize("roll,pitch,yaw", [ (10.0, 20.0, 30.0), (-45.0, 0.0, 90.0), (0.0, 60.0, -30.0), (15.0, -25.0, 80.0), ]) def test_roundtrip_rpy_quat_rpy(roll, pitch, yaw): q = rpy_deg_to_quaternion_xyzw(roll, pitch, yaw) r2, p2, y2 = quaternion_xyzw_to_rpy_deg(q) assert abs(r2 - roll) < 1e-4 assert abs(p2 - pitch) < 1e-4 assert abs(y2 - yaw) < 1e-4 def test_quat_unit_norm(): q = rpy_deg_to_quaternion_xyzw(33.0, -17.0, 91.5) n = float(np.linalg.norm(q)) assert abs(n - 1.0) < 1e-9 ``` - [ ] **Step 2: Run test to verify it fails** Run: `uv run pytest tests/test_geometry_rpy.py -v` Expected: FAIL (`ImportError: cannot import name 'rpy_deg_to_quaternion_xyzw'`) - [ ] **Step 3: Edit `src/presa_mano/geometry.py`** Append to the end of `geometry.py`: ```python def rpy_deg_to_quaternion_xyzw(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray: """Extrinsic XYZ Euler (deg) -> quaternion xyzw. Inverse of `quaternion_xyzw_to_rpy_deg`. Convenzione coerente con `R_to_euler_xyz` (XYZ extrinsic, in radianti).""" cr, sr = np.cos(np.radians(roll_deg) * 0.5), np.sin(np.radians(roll_deg) * 0.5) cp, sp = np.cos(np.radians(pitch_deg) * 0.5), np.sin(np.radians(pitch_deg) * 0.5) cy, sy = np.cos(np.radians(yaw_deg) * 0.5), np.sin(np.radians(yaw_deg) * 0.5) qw = cr * cp * cy + sr * sp * sy qx = sr * cp * cy - cr * sp * sy qy = cr * sp * cy + sr * cp * sy qz = cr * cp * sy - sr * sp * cy return np.array([qx, qy, qz, qw], dtype=np.float64) def quaternion_xyzw_to_rpy_deg(quat_xyzw) -> tuple[float, float, float]: """Quaternion xyzw -> extrinsic XYZ Euler (deg). Convention matches `R_to_euler_xyz`.""" R = quat_xyzw_to_R(np.asarray(quat_xyzw, dtype=np.float64)) rpy_rad = R_to_euler_xyz(R) rpy_deg = np.degrees(rpy_rad) return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2]) ``` - [ ] **Step 4: Run test to verify it passes** Run: `uv run pytest tests/test_geometry_rpy.py -v` Expected: PASS (8 test cases including parametrize) - [ ] **Step 5: Commit** ```bash git add src/presa_mano/geometry.py tests/test_geometry_rpy.py git commit -m "feat(geometry): aggiungi conversioni RPY (deg) ↔ quaternion xyzw" ``` --- ## Task 3: Aggiungere `front_camera()` e `tab_right_arm_cfg()` in `_deps.py` **Files:** - Modify: `src/presa_mano/api/_deps.py` - Test: `tests/test_deps_right_arm.py` (nuovo) - [ ] **Step 1: Write the failing test** Create `tests/test_deps_right_arm.py`: ```python """Verifica che _deps esponga front_camera() e tab_right_arm_cfg().""" from __future__ import annotations from presa_mano.api import _deps from presa_mano.clients import NuwaClient def test_front_camera_returns_nuwa_client_with_config_base_url(): cam = _deps.front_camera() assert isinstance(cam, NuwaClient) # base_url da config.yaml (stripped della trailing slash dal client) assert cam.base_url.startswith("http") # Deve essere DIVERSO dall'URL della nuwa principale assert cam.base_url != _deps.nuwa().base_url def test_tab_right_arm_cfg_returns_dict_with_required_keys(): cfg = _deps.tab_right_arm_cfg() assert cfg["yolo_class"] == "pestello" for key in ("yolo_conf_front", "yolo_conf_wrist", "move_offset_camera_mm", "axis_sign_cam_to_wrist", "max_step_m", "z_floor_m", "home_pose", "custom_pose"): assert key in cfg ``` - [ ] **Step 2: Run test to verify it fails** Run: `uv run pytest tests/test_deps_right_arm.py -v` Expected: FAIL (`AttributeError: module 'presa_mano.api._deps' has no attribute 'front_camera'`) - [ ] **Step 3: Edit `src/presa_mano/api/_deps.py`** After the existing `nuwa()` function, add: ```python def front_camera() -> NuwaClient: """Camera depth montata sulla fronte del robot (servizio Nuwa-like in docker).""" c = _svc(get_config(), "front_camera") return NuwaClient(c["base_url"], timeout_s=c.get("timeout_s", 5.0)) def tab_right_arm_cfg() -> dict[str, Any]: """Blocco di config per la tab Braccio DX. Restituisce {} se mancante.""" return get_config().get("tab_right_arm", {}) or {} ``` - [ ] **Step 4: Run test to verify it passes** Run: `uv run pytest tests/test_deps_right_arm.py -v` Expected: PASS (2 test) - [ ] **Step 5: Commit** ```bash git add src/presa_mano/api/_deps.py tests/test_deps_right_arm.py git commit -m "feat(deps): aggiungi front_camera() e tab_right_arm_cfg()" ``` --- ## Task 4: Pure math module `right_arm/move.py` **Files:** - Create: `src/presa_mano/right_arm/__init__.py` - Create: `src/presa_mano/right_arm/move.py` - Test: `tests/test_right_arm_move.py` - [ ] **Step 1: Write the failing test** Create `tests/test_right_arm_move.py`: ```python """Unit test della logica pura del move-toward (no I/O).""" from __future__ import annotations import numpy as np import pytest from presa_mano.right_arm.move import ( DeltaOversize, apply_z_floor, compute_delta_wrist, ) def test_zero_offset_returns_centroid_with_signs(): delta = compute_delta_wrist( centroid_camera_m=np.array([0.0, 0.0, 1.0]), offset_camera_m=np.array([0.0, 0.0, 0.0]), axis_signs=(1, 1, 1), ) assert np.allclose(delta, [0.0, 0.0, 1.0]) def test_offset_subtracted_before_signs(): # centroid=(0,0,1) offset=(0,0,0.1) sign=(1,-1,-1) → (0, 0, -0.9) delta = compute_delta_wrist( centroid_camera_m=np.array([0.0, 0.0, 1.0]), offset_camera_m=np.array([0.0, 0.0, 0.1]), axis_signs=(1, -1, -1), ) assert np.allclose(delta, [0.0, 0.0, -0.9]) def test_signs_applied_componentwise(): delta = compute_delta_wrist( centroid_camera_m=np.array([0.05, 0.10, 0.20]), offset_camera_m=np.array([0.00, 0.00, 0.00]), axis_signs=(1, -1, -1), ) assert np.allclose(delta, [0.05, -0.10, -0.20]) def test_oversize_raises_when_not_forced(): with pytest.raises(DeltaOversize): compute_delta_wrist( centroid_camera_m=np.array([0.0, 0.0, 0.5]), offset_camera_m=np.array([0.0, 0.0, 0.0]), axis_signs=(1, 1, 1), max_step_m=0.15, force=False, ) def test_oversize_passes_when_forced(): delta = compute_delta_wrist( centroid_camera_m=np.array([0.0, 0.0, 0.5]), offset_camera_m=np.array([0.0, 0.0, 0.0]), axis_signs=(1, 1, 1), max_step_m=0.15, force=True, ) assert np.allclose(delta, [0.0, 0.0, 0.5]) def test_z_floor_clips_when_target_below(): # wrist_before.z=0.10, delta.z=-0.30, target=-0.20, floor=0.05 → clip a 0.05 # → delta.z = 0.05 - 0.10 = -0.05 new_delta, clipped = apply_z_floor( wrist_z_m=0.10, delta=np.array([0.01, -0.02, -0.30]), z_floor_m=0.05, ) assert clipped is True assert new_delta[2] == pytest.approx(-0.05) # X/Y invariati assert new_delta[0] == pytest.approx(0.01) assert new_delta[1] == pytest.approx(-0.02) def test_z_floor_noop_when_target_above(): new_delta, clipped = apply_z_floor( wrist_z_m=0.30, delta=np.array([0.0, 0.0, -0.10]), z_floor_m=0.05, ) assert clipped is False assert np.allclose(new_delta, [0.0, 0.0, -0.10]) def test_z_floor_none_disables_clip(): new_delta, clipped = apply_z_floor( wrist_z_m=0.10, delta=np.array([0.0, 0.0, -0.50]), z_floor_m=None, ) assert clipped is False assert np.allclose(new_delta, [0.0, 0.0, -0.50]) ``` - [ ] **Step 2: Run test to verify it fails** Run: `uv run pytest tests/test_right_arm_move.py -v` Expected: FAIL (`ModuleNotFoundError: No module named 'presa_mano.right_arm'`) - [ ] **Step 3: Create `src/presa_mano/right_arm/__init__.py`** ```python """Logica della tab Braccio DX: math puro + endpoint.""" ``` - [ ] **Step 4: Create `src/presa_mano/right_arm/move.py`** ```python """Pure math: delta wrist a partire da centroide camera + offset + sign flips.""" from __future__ import annotations import numpy as np class DeltaOversize(ValueError): """Sollevata quando ||delta|| supera max_step_m e force=False.""" def __init__(self, delta_norm_m: float, max_step_m: float): super().__init__(f"delta {delta_norm_m*1000:.1f}mm > max {max_step_m*1000:.1f}mm") self.delta_norm_m = float(delta_norm_m) self.max_step_m = float(max_step_m) def compute_delta_wrist( centroid_camera_m: np.ndarray, offset_camera_m: np.ndarray, axis_signs: tuple[int, int, int], max_step_m: float | None = None, force: bool = False, ) -> np.ndarray: """Delta da applicare al polso (frame polso) per portare la camera sull'offset. Modello: si assume assi camera ≈ assi polso a meno dei segni. delta_wrist = sign * (centroid_camera - offset_camera) Se `max_step_m` è settato e ||delta|| > max_step_m e `force` è False, solleva DeltaOversize. """ centroid = np.asarray(centroid_camera_m, dtype=np.float64).reshape(3) offset = np.asarray(offset_camera_m, dtype=np.float64).reshape(3) sign = np.asarray(axis_signs, dtype=np.float64).reshape(3) delta = sign * (centroid - offset) if max_step_m is not None and not force: n = float(np.linalg.norm(delta)) if n > max_step_m: raise DeltaOversize(n, max_step_m) return delta def apply_z_floor( wrist_z_m: float, delta: np.ndarray, z_floor_m: float | None, ) -> tuple[np.ndarray, bool]: """Se la Z target finirebbe sotto `z_floor_m`, clippa il delta.Z. Ritorna (nuovo_delta, was_clipped). `z_floor_m=None` disabilita il clip.""" delta = np.asarray(delta, dtype=np.float64).reshape(3).copy() if z_floor_m is None: return delta, False target_z = float(wrist_z_m) + float(delta[2]) if target_z >= float(z_floor_m): return delta, False delta[2] = float(z_floor_m) - float(wrist_z_m) return delta, True ``` - [ ] **Step 5: Run test to verify it passes** Run: `uv run pytest tests/test_right_arm_move.py -v` Expected: PASS (8 test) - [ ] **Step 6: Commit** ```bash git add src/presa_mano/right_arm/ tests/test_right_arm_move.py git commit -m "feat(right_arm): math puro per delta-toward (sign flips + z-floor)" ``` --- ## Task 5: Router skeleton + `/state` + `/activate` **Files:** - Create: `src/presa_mano/api/right_arm_routes.py` - Modify: `src/presa_mano/main.py` - Test: `tests/test_right_arm_routes.py` - [ ] **Step 1: Write the failing test** Create `tests/test_right_arm_routes.py`: ```python """Integration test del router /api/right_arm/*.""" 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_wrist_response(): return { "left_wrist": {"position": [0.10, 0.20, 0.30], "quaternion": [0, 0, 0, 1]}, "right_wrist": {"position": [0.30, -0.20, 0.30], "quaternion": [0, 0, 0, 1]}, } def test_state_returns_wrist_pose_and_config(): with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_factory: g1 = MagicMock() g1.wrist.return_value = _fake_wrist_response() g1_factory.return_value = g1 r = client.get("/api/right_arm/state") assert r.status_code == 200 body = r.json() assert body["wrist"]["position"] == [0.30, -0.20, 0.30] assert "rpy_deg" in body["wrist"] assert body["side"] == "right" or body["side"] in ("left", "right") assert "home_pose" in body assert "custom_pose" in body assert "offset_camera_mm" in body assert "axis_signs" in body assert "max_step_m" in body assert "z_floor_m" in body assert "robot_active" in body assert "timings_ms" in body def test_state_502_when_g1_unavailable(): from presa_mano.clients import G1Unavailable with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_factory: g1 = MagicMock() g1.wrist.side_effect = G1Unavailable("connection refused") g1_factory.return_value = g1 r = client.get("/api/right_arm/state") assert r.status_code == 502 def test_activate_sets_robot_active_g1_and_side_right(): # /activate è idempotente: chiama PUT /api/config e ritorna /state with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_factory: g1 = MagicMock() g1.wrist.return_value = _fake_wrist_response() g1_factory.return_value = g1 r = client.post("/api/right_arm/activate") assert r.status_code == 200 body = r.json() assert body["robot_active"] == "g1" assert body["side"] == "right" ``` - [ ] **Step 2: Run test to verify it fails** Run: `uv run pytest tests/test_right_arm_routes.py -v` Expected: FAIL (`404 Not Found` perché il router non esiste ancora) - [ ] **Step 3: Create `src/presa_mano/api/right_arm_routes.py`** ```python """Endpoint per la tab Braccio DX (G1, polso destro, Nuwa eye-in-hand).""" from __future__ import annotations from typing import Any from fastapi import APIRouter, HTTPException from pydantic import BaseModel from ..clients import G1Unavailable from ..config import update_config from ..geometry import quaternion_xyzw_to_rpy_deg from ..timing import Stopwatch from . import _deps router = APIRouter() def _state_payload(sw: Stopwatch) -> dict[str, Any]: cfg = _deps.tab_right_arm_cfg() with sw.step("g1_wrist"): wrist = _deps.g1().wrist()["right_wrist"] pos = wrist["position"] quat = wrist["quaternion"] rpy = quaternion_xyzw_to_rpy_deg(quat) return { "wrist": {"position": pos, "quaternion": quat, "rpy_deg": [rpy[0], rpy[1], rpy[2]]}, "robot_active": _deps.active_robot_key(), "side": _deps.default_side(), "home_pose": cfg.get("home_pose"), "custom_pose": cfg.get("custom_pose"), "offset_camera_mm": cfg.get("move_offset_camera_mm"), "axis_signs": cfg.get("axis_sign_cam_to_wrist"), "max_step_m": cfg.get("max_step_m"), "z_floor_m": cfg.get("z_floor_m"), "timings_ms": sw.as_dict(), } @router.get("/state") def get_state() -> dict[str, Any]: sw = Stopwatch() try: return _state_payload(sw) except G1Unavailable as e: raise HTTPException(status_code=502, detail=str(e)) @router.post("/activate") def post_activate() -> dict[str, Any]: sw = Stopwatch() with sw.step("config_update"): update_config({"robot": {"active": "g1", "wrist_side": "right"}}) try: return _state_payload(sw) except G1Unavailable as e: raise HTTPException(status_code=502, detail=str(e)) ``` - [ ] **Step 4: Wire router in `src/presa_mano/main.py`** In the imports block, add `right_arm_routes`: ```python from .api import ( agent_routes, calibration_routes, config_routes, grasp_routes, home_routes, models_routes, perception_routes, posture_routes, right_arm_routes, robot_routes, wrist_routes, work_plane_routes, ) ``` After the existing `app.include_router(...)` lines, add: ```python app.include_router(right_arm_routes.router, prefix="/api/right_arm", tags=["right_arm"]) ``` - [ ] **Step 5: Run test to verify it passes** Run: `uv run pytest tests/test_right_arm_routes.py -v` Expected: PASS (3 test) - [ ] **Step 6: Commit** ```bash git add src/presa_mano/api/right_arm_routes.py src/presa_mano/main.py tests/test_right_arm_routes.py git commit -m "feat(right_arm): router skeleton con /state e /activate" ``` --- ## Task 6: `/snapshot/front` + `/snapshot/wrist` **Files:** - Modify: `src/presa_mano/api/right_arm_routes.py` - Modify: `tests/test_right_arm_routes.py` - [ ] **Step 1: Add failing tests** Append to `tests/test_right_arm_routes.py`: ```python def test_snapshot_front_returns_jpeg(): fake_jpeg = b"\xff\xd8\xff" + b"\x00" * 100 # magic JPEG bytes with patch("presa_mano.api.right_arm_routes._deps.front_camera") as factory: cam = MagicMock() cam.rgb_jpeg.return_value = fake_jpeg factory.return_value = cam r = client.get("/api/right_arm/snapshot/front") assert r.status_code == 200 assert r.headers["content-type"].startswith("image/jpeg") assert r.content == fake_jpeg def test_snapshot_wrist_returns_jpeg(): fake_jpeg = b"\xff\xd8\xff" + b"\x00" * 50 with patch("presa_mano.api.right_arm_routes._deps.nuwa") as factory: cam = MagicMock() cam.rgb_jpeg.return_value = fake_jpeg factory.return_value = cam r = client.get("/api/right_arm/snapshot/wrist") assert r.status_code == 200 assert r.headers["content-type"].startswith("image/jpeg") assert r.content == fake_jpeg def test_snapshot_front_502_on_failure(): with patch("presa_mano.api.right_arm_routes._deps.front_camera") as factory: cam = MagicMock() cam.rgb_jpeg.side_effect = RuntimeError("connection refused") factory.return_value = cam r = client.get("/api/right_arm/snapshot/front") assert r.status_code == 502 ``` - [ ] **Step 2: Run tests to verify they fail** Run: `uv run pytest tests/test_right_arm_routes.py -v -k snapshot` Expected: FAIL (404) - [ ] **Step 3: Add endpoints to `src/presa_mano/api/right_arm_routes.py`** Add `from fastapi.responses import Response` to imports. Append: ```python @router.get("/snapshot/front") def snapshot_front() -> Response: try: jpeg = _deps.front_camera().rgb_jpeg() except Exception as e: raise HTTPException(status_code=502, detail=f"front_camera unreachable: {e}") return Response(content=jpeg, media_type="image/jpeg") @router.get("/snapshot/wrist") def snapshot_wrist() -> Response: try: jpeg = _deps.nuwa().rgb_jpeg() except Exception as e: raise HTTPException(status_code=502, detail=f"wrist nuwa unreachable: {e}") return Response(content=jpeg, media_type="image/jpeg") ``` - [ ] **Step 4: Run tests to verify they pass** Run: `uv run pytest tests/test_right_arm_routes.py -v -k snapshot` Expected: PASS (3 test) - [ ] **Step 5: Commit** ```bash git add src/presa_mano/api/right_arm_routes.py tests/test_right_arm_routes.py git commit -m "feat(right_arm): endpoint /snapshot/{front,wrist}" ``` --- ## Task 7: `/detect/front` con YOLO + proxy fallback **Files:** - Modify: `src/presa_mano/api/right_arm_routes.py` - Modify: `tests/test_right_arm_routes.py` - [ ] **Step 1: Add failing tests** Append: ```python def _fake_yolo_top(class_name="pestello", conf=0.87): return {"detections": [ {"class": class_name, "confidence": conf, "bbox": {"x1": 100.0, "y1": 80.0, "x2": 220.0, "y2": 180.0}}, {"class": "altra", "confidence": 0.5, "bbox": {"x1": 10.0, "y1": 10.0, "x2": 50.0, "y2": 50.0}}, ]} def test_detect_front_filters_class_and_returns_top(): with patch("presa_mano.api.right_arm_routes._deps.yolo") as ying, \ patch("presa_mano.api.right_arm_routes._deps.front_camera") as cam_f: yolo = MagicMock() yolo.detect_url.return_value = _fake_yolo_top() ying.return_value = yolo cam = MagicMock() cam.rgb_url.return_value = "http://192.168.123.99:8878/api/rgb" cam.intrinsics.return_value = {"width": 640, "height": 480} cam_f.return_value = cam r = client.post("/api/right_arm/detect/front", json={"class_name": "pestello", "conf": 0.4}) assert r.status_code == 200 body = r.json() assert body["detections"][0]["class"] == "pestello" assert body["image_size"] == [640, 480] def test_detect_front_404_when_no_match(): with patch("presa_mano.api.right_arm_routes._deps.yolo") as ying, \ patch("presa_mano.api.right_arm_routes._deps.front_camera") as cam_f: yolo = MagicMock() yolo.detect_url.return_value = {"detections": []} ying.return_value = yolo cam = MagicMock() cam.rgb_url.return_value = "http://x/api/rgb" cam.intrinsics.return_value = {"width": 640, "height": 480} cam_f.return_value = cam r = client.post("/api/right_arm/detect/front", json={"class_name": "pestello"}) assert r.status_code == 404 def test_detect_front_falls_back_to_proxy_url_on_yolo_failure(): """Se YOLO non riesce sull'URL diretto, ripeti con `proxy_base_url`.""" attempts = [] def _detect_url(image_url, conf=None): attempts.append(image_url) if "192.168" in image_url: raise RuntimeError("dns timeout") return _fake_yolo_top() with patch("presa_mano.api.right_arm_routes._deps.yolo") as ying, \ patch("presa_mano.api.right_arm_routes._deps.front_camera") as cam_f: yolo = MagicMock() yolo.detect_url.side_effect = _detect_url ying.return_value = yolo cam = MagicMock() cam.rgb_url.return_value = "http://192.168.123.99:8878/api/rgb" cam.intrinsics.return_value = {"width": 640, "height": 480} cam_f.return_value = cam r = client.post("/api/right_arm/detect/front", json={"class_name": "pestello", "proxy_base_url": "http://localhost:8890"}) assert r.status_code == 200 assert len(attempts) == 2 assert "192.168" in attempts[0] assert attempts[1].endswith("/api/right_arm/snapshot/front") ``` - [ ] **Step 2: Run tests to verify they fail** Run: `uv run pytest tests/test_right_arm_routes.py -v -k detect_front` Expected: FAIL (404) - [ ] **Step 3: Add endpoint to `src/presa_mano/api/right_arm_routes.py`** Add to imports: ```python from typing import Any, Optional import logging log = logging.getLogger(__name__) ``` Add Pydantic model and endpoint: ```python class DetectFrontReq(BaseModel): class_name: Optional[str] = None conf: Optional[float] = None proxy_base_url: Optional[str] = None # se YOLO non raggiunge la front camera def _yolo_detect_with_fallback(image_url: str, conf: float | None, proxy_image_url: str | None, sw: Stopwatch ) -> dict[str, Any]: """Prova YOLO sull'URL diretto; se fallisce e proxy_image_url è settato, ritenta.""" yolo = _deps.yolo() try: with sw.step("yolo"): return yolo.detect_url(image_url=image_url, conf=conf) except Exception as e: if not proxy_image_url: raise HTTPException(status_code=502, detail=f"YOLO unreachable: {e}") log.warning("YOLO failed on direct URL (%s), retrying via proxy %s", e, proxy_image_url) try: with sw.step("yolo_proxy"): return yolo.detect_url(image_url=proxy_image_url, conf=conf) except Exception as e2: raise HTTPException(status_code=502, detail=f"YOLO unreachable (proxy): {e2}") def _top_detection(payload: dict[str, Any], class_name: str | None) -> dict[str, Any] | None: dets = payload.get("detections") or payload.get("data", {}).get("detections") or [] if class_name: dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()] if not dets: return None dets.sort(key=lambda d: float(d.get("confidence", 0.0)), reverse=True) return dets[0] @router.post("/detect/front") def detect_front(req: DetectFrontReq) -> dict[str, Any]: sw = Stopwatch() cam = _deps.front_camera() image_url = cam.rgb_url() proxy_url = None if req.proxy_base_url: proxy_url = f"{req.proxy_base_url.rstrip('/')}/api/right_arm/snapshot/front" res = _yolo_detect_with_fallback(image_url, req.conf, proxy_url, sw) dets_raw = res.get("detections") or res.get("data", {}).get("detections") or [] if req.class_name: dets_filtered = [d for d in dets_raw if (d.get("class") or "").lower() == req.class_name.lower()] else: dets_filtered = list(dets_raw) if not dets_filtered: raise HTTPException(status_code=404, detail=f"no '{req.class_name or 'object'}' detected") dets_filtered.sort(key=lambda d: float(d.get("confidence", 0.0)), reverse=True) intr = cam.intrinsics() return { "detections": dets_filtered, "image_size": [int(intr.get("width", 0)), int(intr.get("height", 0))], "timings_ms": sw.as_dict(), } ``` - [ ] **Step 4: Run tests to verify they pass** Run: `uv run pytest tests/test_right_arm_routes.py -v -k detect_front` Expected: PASS (3 test) - [ ] **Step 5: Commit** ```bash git add src/presa_mano/api/right_arm_routes.py tests/test_right_arm_routes.py git commit -m "feat(right_arm): /detect/front con proxy fallback per YOLO" ``` --- ## Task 8: `/detect/wrist` con YOLO + SAM21 + centroide **Files:** - Modify: `src/presa_mano/api/right_arm_routes.py` - Modify: `tests/test_right_arm_routes.py` - [ ] **Step 1: Add failing tests** Append: ```python def test_detect_wrist_returns_centroid_and_distance(): # YOLO → bbox → SAM21 → centroide import numpy as np fake_points = np.array([ [0.01, -0.02, 0.20], [0.02, -0.01, 0.21], [0.00, -0.03, 0.19], ] * 5, dtype=np.float64) # 15 punti, > 10 minimo with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \ patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \ patch("presa_mano.api.right_arm_routes._deps.sam21") as sam_f: nuwa = MagicMock() nuwa.rgb_url.return_value = "http://localhost:8877/api/rgb" nuwa_f.return_value = nuwa yolo = MagicMock() yolo.detect_url.return_value = _fake_yolo_top() yolo_f.return_value = yolo sam_f.return_value = MagicMock() seg = MagicMock() seg.points_cv_m = fake_points seg_f.return_value = seg r = client.post("/api/right_arm/detect/wrist", json={"class_name": "pestello"}) assert r.status_code == 200 body = r.json() assert body["detection"]["class"] == "pestello" assert "bbox" in body c = body["centroid_camera_m"] assert abs(c[2] - 0.20) < 0.05 assert body["distance_m"] > 0 assert body["n_sam21_points"] == 15 def test_detect_wrist_422_too_few_points(): import numpy as np with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \ patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \ patch("presa_mano.api.right_arm_routes._deps.sam21") as sam_f: nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa yolo = MagicMock(); yolo.detect_url.return_value = _fake_yolo_top(); yolo_f.return_value = yolo sam_f.return_value = MagicMock() seg = MagicMock(); seg.points_cv_m = np.zeros((3, 3)) seg_f.return_value = seg r = client.post("/api/right_arm/detect/wrist", json={"class_name": "pestello"}) assert r.status_code == 422 def test_detect_wrist_404_when_yolo_no_match(): with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f: nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa yolo = MagicMock(); yolo.detect_url.return_value = {"detections": []}; yolo_f.return_value = yolo r = client.post("/api/right_arm/detect/wrist", json={"class_name": "pestello"}) assert r.status_code == 404 ``` - [ ] **Step 2: Run tests to verify they fail** Run: `uv run pytest tests/test_right_arm_routes.py -v -k detect_wrist` Expected: FAIL (404) - [ ] **Step 3: Add endpoint** Add imports at top of `right_arm_routes.py`: ```python import numpy as np from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21 from .perception_routes import _bbox_to_roi ``` Add model and endpoint: ```python class DetectWristReq(BaseModel): class_name: Optional[str] = None conf: Optional[float] = None def _detect_wrist_core(class_name: str | None, conf: float | None, sw: Stopwatch ) -> dict[str, Any]: """Funzione core riusata da /detect/wrist e da /move/toward.""" nuwa = _deps.nuwa() image_url = nuwa.rgb_url() try: with sw.step("yolo"): yres = _deps.yolo().detect_url(image_url=image_url, conf=conf) except Exception as e: raise HTTPException(status_code=502, detail=f"YOLO unreachable: {e}") top = _top_detection(yres, class_name) if top is None: raise HTTPException(status_code=404, detail=f"no '{class_name or 'object'}' detected") bbox = _bbox_to_roi(top["bbox"]) try: seg = segment_with_sam21(nuwa, _deps.sam21(), bbox, sw) except Sam21Fallback as e: raise HTTPException(status_code=422, detail=f"SAM21 fallback: {e}") pts = np.asarray(seg.points_cv_m, dtype=np.float64) if pts.shape[0] < 10: raise HTTPException(status_code=422, detail=f"too few sam21 points: {pts.shape[0]}") with sw.step("centroid"): centroid = pts.mean(axis=0) distance = float(np.linalg.norm(centroid)) return { "detection": top, "bbox": list(bbox), "centroid_camera_m": [float(centroid[0]), float(centroid[1]), float(centroid[2])], "distance_m": distance, "n_sam21_points": int(pts.shape[0]), } @router.post("/detect/wrist") def detect_wrist(req: DetectWristReq) -> dict[str, Any]: sw = Stopwatch() body = _detect_wrist_core(req.class_name, req.conf, sw) body["timings_ms"] = sw.as_dict() return body ``` - [ ] **Step 4: Run tests to verify they pass** Run: `uv run pytest tests/test_right_arm_routes.py -v -k detect_wrist` Expected: PASS (3 test) - [ ] **Step 5: Commit** ```bash git add src/presa_mano/api/right_arm_routes.py tests/test_right_arm_routes.py git commit -m "feat(right_arm): /detect/wrist con YOLO + SAM21 + centroide camera" ``` --- ## Task 9: `/move/toward` — endpoint critico **Files:** - Modify: `src/presa_mano/api/right_arm_routes.py` - Modify: `tests/test_right_arm_routes.py` - [ ] **Step 1: Add failing tests** Append: ```python def _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20): """Helper: patcha yolo + sam21 bridge per il /move/toward.""" import numpy as np seg_pts = np.tile(np.array(centroid_m, dtype=np.float64), (n_points, 1)) return seg_pts def test_move_toward_dry_run_returns_delta_no_call_to_g1(): import numpy as np seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20) g1_called = [] with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \ patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \ patch("presa_mano.api.right_arm_routes._deps.sam21"), \ patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f: nuwa_f.return_value.rgb_url.return_value = "http://x" yolo_f.return_value.detect_url.return_value = _fake_yolo_top() seg_f.return_value = MagicMock(points_cv_m=seg_pts) g1 = MagicMock() g1.wrist.return_value = _fake_wrist_response() def _track_relative(**kw): g1_called.append(kw) return {"ok": True} g1.wrist_relative.side_effect = _track_relative g1_f.return_value = g1 r = client.post("/api/right_arm/move/toward", json={"class_name": "pestello", "dry_run": True}) assert r.status_code == 200 body = r.json() # offset default (0,0,100mm) → centroid - offset = (0, 0, 0.10) → # con sign (1,-1,-1) → (0, 0, -0.10) m assert abs(body["delta_wrist_m"][2] - (-0.10)) < 1e-6 assert body["applied"] is False # Anche se dry_run=true, il G1Client viene comunque chiamato per la # lettura della posa wrist (g1.wrist()) ma con dry_run=True il # _post viene short-circuited; verifichiamo che wrist_relative sia # comunque chiamato (G1Client gestisce dry-run internamente) # NOTA: g1.wrist_relative viene comunque invocato perché lo dry-run # è gestito nel client G1, non saltato dall'endpoint. # Quindi g1_called avrà 1 entry, ma il client non avrebbe fatto HTTP. assert len(g1_called) == 1 assert g1_called[0]["side"] == "right" def test_move_toward_real_call_returns_wrist_after(): import numpy as np seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20) with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \ patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \ patch("presa_mano.api.right_arm_routes._deps.sam21"), \ patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f: nuwa_f.return_value.rgb_url.return_value = "http://x" yolo_f.return_value.detect_url.return_value = _fake_yolo_top() seg_f.return_value = MagicMock(points_cv_m=seg_pts) g1 = MagicMock() g1.wrist.return_value = _fake_wrist_response() g1.wrist_relative.return_value = {"ok": True} g1_f.return_value = g1 r = client.post("/api/right_arm/move/toward", json={"class_name": "pestello", "dry_run": False}) assert r.status_code == 200 body = r.json() assert body["applied"] is True assert body["wrist_after"] is not None def test_move_toward_422_oversize_delta_without_force(): import numpy as np # centroid=(0,0,0.50), default offset=(0,0,0.10) → delta_z=-0.40 → > max_step 0.15 seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.50), n_points=20) with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \ patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \ patch("presa_mano.api.right_arm_routes._deps.sam21"), \ patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f: nuwa_f.return_value.rgb_url.return_value = "http://x" yolo_f.return_value.detect_url.return_value = _fake_yolo_top() seg_f.return_value = MagicMock(points_cv_m=seg_pts) g1 = MagicMock(); g1.wrist.return_value = _fake_wrist_response(); g1_f.return_value = g1 r = client.post("/api/right_arm/move/toward", json={"class_name": "pestello", "dry_run": True, "force": False}) assert r.status_code == 422 assert "delta" in r.json()["detail"].lower() def test_move_toward_412_when_not_g1_right(): """Se config attiva != g1 o side != right, rifiuta con 412.""" import numpy as np seg_pts = _patch_full_detect_chain(n_points=20) with patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="h1"), \ patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="left"): r = client.post("/api/right_arm/move/toward", json={"class_name": "pestello", "dry_run": True}) assert r.status_code == 412 ``` - [ ] **Step 2: Run tests to verify they fail** Run: `uv run pytest tests/test_right_arm_routes.py -v -k move_toward` Expected: FAIL (404 sui primi 3, eventualmente 404/500 sul 412) - [ ] **Step 3: Add endpoint to `right_arm_routes.py`** Add to imports: ```python from ..right_arm.move import DeltaOversize, apply_z_floor, compute_delta_wrist ``` Add Pydantic model and endpoint: ```python class MoveTowardReq(BaseModel): class_name: Optional[str] = None conf: Optional[float] = None dry_run: bool = True force: bool = False def _guard_g1_right() -> None: if _deps.active_robot_key() != "g1": raise HTTPException(status_code=412, detail=f"activate G1 right first (active={_deps.active_robot_key()})") if _deps.default_side() != "right": raise HTTPException(status_code=412, detail=f"activate G1 right first (side={_deps.default_side()})") @router.post("/move/toward") def move_toward(req: MoveTowardReq) -> dict[str, Any]: sw = Stopwatch() _guard_g1_right() cfg = _deps.tab_right_arm_cfg() off_mm = cfg.get("move_offset_camera_mm", {"x": 0, "y": 0, "z": 0}) sign = cfg.get("axis_sign_cam_to_wrist", {"x": 1, "y": 1, "z": 1}) max_step_m = float(cfg.get("max_step_m", 0.15)) z_floor = cfg.get("z_floor_m") z_floor_m = float(z_floor) if z_floor is not None else None det = _detect_wrist_core(req.class_name, req.conf, sw) centroid = np.asarray(det["centroid_camera_m"], dtype=np.float64) offset_m = np.array([off_mm["x"], off_mm["y"], off_mm["z"]], dtype=np.float64) / 1000.0 signs = (int(sign["x"]), int(sign["y"]), int(sign["z"])) try: delta = compute_delta_wrist(centroid, offset_m, signs, max_step_m=max_step_m, force=req.force) except DeltaOversize as e: raise HTTPException(status_code=422, detail=str(e)) # Leggi posa polso PRIMA del move per applicare z-floor try: with sw.step("g1_wrist_before"): wrist_before = _deps.g1().wrist()["right_wrist"] except G1Unavailable as e: raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}") delta, z_clipped = apply_z_floor(float(wrist_before["position"][2]), delta, z_floor_m) try: with sw.step("wrist_relative"): _deps.g1(dry_run=req.dry_run).wrist_relative( side="right", delta_position=delta.tolist(), ) except G1Unavailable as e: raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}") wrist_after: dict[str, Any] | None = None if not req.dry_run: try: with sw.step("g1_wrist_after"): wrist_after = _deps.g1().wrist()["right_wrist"] except G1Unavailable: wrist_after = None # best-effort; il move è stato già inviato log.info( "move_toward class=%s conf=%s centroid=%s offset=%s sign=%s delta=%s dry_run=%s applied=%s", req.class_name, req.conf, centroid.tolist(), offset_m.tolist(), signs, delta.tolist(), req.dry_run, not req.dry_run, ) return { "detection": det["detection"], "centroid_camera_m": det["centroid_camera_m"], "distance_m": det["distance_m"], "offset_camera_m": offset_m.tolist(), "axis_signs": list(signs), "delta_wrist_m": delta.tolist(), "delta_wrist_mm": (delta * 1000.0).tolist(), "z_clipped": bool(z_clipped), "wrist_before": wrist_before, "wrist_after": wrist_after, "applied": not req.dry_run, "n_sam21_points": det["n_sam21_points"], "timings_ms": sw.as_dict(), } ``` - [ ] **Step 4: Run tests to verify they pass** Run: `uv run pytest tests/test_right_arm_routes.py -v -k move_toward` Expected: PASS (4 test) - [ ] **Step 5: Commit** ```bash git add src/presa_mano/api/right_arm_routes.py tests/test_right_arm_routes.py git commit -m "feat(right_arm): /move/toward con sign flips, max-step e z-floor" ``` --- ## Task 10: `/home`, `/custom_pose`, `PUT /custom_pose`, `PUT /offset` **Files:** - Modify: `src/presa_mano/api/right_arm_routes.py` - Modify: `tests/test_right_arm_routes.py` - [ ] **Step 1: Add failing tests** Append: ```python def test_home_calls_wrist_absolute_with_home_pose(): calls = [] def _abs(**kw): calls.append(kw) return {"ok": True} with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \ patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \ patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"): g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs; g1_f.return_value = g1 r = client.post("/api/right_arm/home", json={"dry_run": True}) assert r.status_code == 200 assert len(calls) == 1 assert calls[0]["side"] == "right" assert len(calls[0]["position"]) == 3 assert len(calls[0]["quaternion"]) == 4 def test_custom_pose_executes_pose_with_dry_run_default_false(): calls = [] def _abs(**kw): calls.append(kw); return {"ok": True} with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \ patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \ patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"): g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs; g1_f.return_value = g1 r = client.post("/api/right_arm/custom_pose", json={}) assert r.status_code == 200 assert len(calls) == 1 def test_put_custom_pose_updates_config(): new_pos = [0.40, -0.15, 0.25] new_rpy = [10.0, -5.0, 90.0] r = client.put("/api/right_arm/custom_pose", json={"position": new_pos, "rpy_deg": new_rpy}) assert r.status_code == 200 r2 = client.get("/api/config") cp = r2.json()["tab_right_arm"]["custom_pose"] assert cp["position"] == new_pos assert cp["rpy_deg"] == new_rpy def test_put_offset_updates_config(): r = client.put("/api/right_arm/offset", json={"move_offset_camera_mm": {"x": 5, "y": -3, "z": 75}, "axis_sign_cam_to_wrist": {"x": -1, "y": 1, "z": -1}}) assert r.status_code == 200 r2 = client.get("/api/config") body = r2.json()["tab_right_arm"] assert body["move_offset_camera_mm"] == {"x": 5, "y": -3, "z": 75} assert body["axis_sign_cam_to_wrist"] == {"x": -1, "y": 1, "z": -1} ``` - [ ] **Step 2: Run tests to verify they fail** Run: `uv run pytest tests/test_right_arm_routes.py -v -k "home or custom_pose or offset"` Expected: FAIL (404) - [ ] **Step 3: Add endpoints** Add to imports: ```python from ..geometry import rpy_deg_to_quaternion_xyzw ``` Append: ```python class PoseExecReq(BaseModel): dry_run: bool = False class PoseUpdateReq(BaseModel): position: Optional[list[float]] = None rpy_deg: Optional[list[float]] = None class OffsetUpdateReq(BaseModel): move_offset_camera_mm: Optional[dict[str, float]] = None axis_sign_cam_to_wrist: Optional[dict[str, int]] = None def _exec_named_pose(pose_key: str, dry_run: bool, sw: Stopwatch) -> dict[str, Any]: _guard_g1_right() cfg = _deps.tab_right_arm_cfg() pose = cfg.get(pose_key) or {} pos = pose.get("position") rpy = pose.get("rpy_deg") if not (pos and rpy and len(pos) == 3 and len(rpy) == 3): raise HTTPException(status_code=422, detail=f"{pose_key} malformed in config") quat = rpy_deg_to_quaternion_xyzw(rpy[0], rpy[1], rpy[2]) try: with sw.step("wrist_absolute"): res = _deps.g1(dry_run=dry_run).wrist_absolute( side="right", position=list(pos), quaternion=quat.tolist(), ) except G1Unavailable as e: raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}") return {"pose_key": pose_key, "position": pos, "rpy_deg": rpy, "quaternion": quat.tolist(), "result": res, "dry_run": dry_run, "applied": not dry_run, "timings_ms": sw.as_dict()} @router.post("/home") def post_home(req: PoseExecReq) -> dict[str, Any]: return _exec_named_pose("home_pose", req.dry_run, Stopwatch()) @router.post("/custom_pose") def post_custom_pose(req: PoseExecReq) -> dict[str, Any]: return _exec_named_pose("custom_pose", req.dry_run, Stopwatch()) @router.put("/custom_pose") def put_custom_pose(req: PoseUpdateReq) -> dict[str, Any]: patch: dict[str, Any] = {} if req.position is not None: if len(req.position) != 3: raise HTTPException(status_code=422, detail="position must be 3 floats") patch["position"] = list(req.position) if req.rpy_deg is not None: if len(req.rpy_deg) != 3: raise HTTPException(status_code=422, detail="rpy_deg must be 3 floats") patch["rpy_deg"] = list(req.rpy_deg) if not patch: raise HTTPException(status_code=422, detail="nothing to update") update_config({"tab_right_arm": {"custom_pose": patch}}) return {"custom_pose": _deps.tab_right_arm_cfg().get("custom_pose")} @router.put("/offset") def put_offset(req: OffsetUpdateReq) -> dict[str, Any]: patch: dict[str, Any] = {} if req.move_offset_camera_mm is not None: for k in ("x", "y", "z"): if k not in req.move_offset_camera_mm: raise HTTPException(status_code=422, detail=f"move_offset_camera_mm missing '{k}'") patch["move_offset_camera_mm"] = {k: float(req.move_offset_camera_mm[k]) for k in ("x", "y", "z")} if req.axis_sign_cam_to_wrist is not None: for k in ("x", "y", "z"): v = req.axis_sign_cam_to_wrist.get(k) if v not in (-1, 1): raise HTTPException(status_code=422, detail=f"axis_sign_cam_to_wrist['{k}'] must be +/-1") patch["axis_sign_cam_to_wrist"] = {k: int(req.axis_sign_cam_to_wrist[k]) for k in ("x", "y", "z")} if not patch: raise HTTPException(status_code=422, detail="nothing to update") update_config({"tab_right_arm": patch}) cfg = _deps.tab_right_arm_cfg() return {"move_offset_camera_mm": cfg.get("move_offset_camera_mm"), "axis_sign_cam_to_wrist": cfg.get("axis_sign_cam_to_wrist")} ``` - [ ] **Step 4: Run tests to verify they pass** Run: `uv run pytest tests/test_right_arm_routes.py -v -k "home or custom_pose or offset"` Expected: PASS (4 test) - [ ] **Step 5: Run full test file to check nothing regressed** Run: `uv run pytest tests/test_right_arm_routes.py -v` Expected: PASS (tutti i test del file) - [ ] **Step 6: Commit** ```bash git add src/presa_mano/api/right_arm_routes.py tests/test_right_arm_routes.py git commit -m "feat(right_arm): endpoint /home, /custom_pose (POST+PUT), PUT /offset" ``` --- ## Task 11: Markup HTML della tab `right-arm` **Files:** - Modify: `web/index.html` - [ ] **Step 1: Aggiungi entry alla nav** Locate the line: ```html ``` Insert BEFORE it: ```html ``` - [ ] **Step 2: Aggiungi il pannello della tab** Locate the line `
` and insert BEFORE it: ```html

Fronte

front camera
— nessuna detection

Polso (Nuwa)

wrist camera
— nessuna detection

Posa polso DX

X—m
Y—m
Z—m
Roll—°
Pitch—°
Yaw—°

Offset presa rispetto a camera (mm)

|
— nessun move

Posa Home (preset)

Posa Custom (editabile)

``` - [ ] **Step 3: Aggiungi lo script include** Locate: ```html ``` Add immediately after: ```html ``` - [ ] **Step 4: Aggiungi gli stili CSS minimi** Append to `web/style.css`: ```css /* ----- Tab Braccio DX ----- */ .ra-banner { padding: 8px 12px; border-radius: 4px; margin-bottom: 12px; display: flex; align-items: center; gap: 12px; } .ra-banner-warn { background: #fff3cd; color: #856404; border: 1px solid #ffeeba; } .ra-banner-ok { background: #d4edda; color: #155724; border: 1px solid #c3e6cb; } .ra-grid { display: grid; grid-template-columns: 1fr 1fr 1fr; gap: 12px; margin-bottom: 12px; } .ra-card { border: 1px solid #ddd; border-radius: 4px; padding: 12px; background: #fafafa; } .ra-card h3 { margin-top: 0; } .ra-card.ra-wide { grid-column: 1 / -1; } .ra-imgwrap { position: relative; width: 100%; } .ra-imgwrap img { width: 100%; display: block; } .ra-imgwrap canvas { position: absolute; top: 0; left: 0; pointer-events: none; width: 100%; height: 100%; } .ra-row { display: flex; flex-wrap: wrap; gap: 8px; align-items: center; margin: 6px 0; } .ra-sep { color: #888; } .ra-result { font-family: monospace; font-size: 0.9em; padding: 6px; background: #f0f0f0; border-radius: 4px; } .ra-pose-table { border-collapse: collapse; } .ra-pose-table th, .ra-pose-table td { padding: 2px 8px; text-align: left; } .ra-pose-form { display: grid; grid-template-columns: 1fr 1fr 1fr; gap: 6px; margin-bottom: 8px; } .ra-pose-form input { width: 100%; } .ra-wide-grid { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; } .ra-danger { background: #dc3545; color: white; border: none; padding: 6px 12px; border-radius: 4px; } .ra-danger:hover { background: #c82333; } .dim { color: #888; } ``` - [ ] **Step 5: Commit** ```bash git add web/index.html web/style.css git commit -m "feat(gui): markup + CSS tab Braccio DX" ``` --- ## Task 12: JS module `web/right_arm.js` **Files:** - Create: `web/right_arm.js` - [ ] **Step 1: Create the file** ```javascript // Tab "Braccio DX" — auto-refresh immagini, YOLO on-demand, move-toward, // posa Home/Custom. Vanilla JS, niente dipendenze. const $ = (id) => document.getElementById(id); const FRONT_REFRESH_MS = 1000; const WRIST_REFRESH_MS = 1000; const POSE_REFRESH_MS = 2000; let frontTimer = null; let wristTimer = null; let poseTimer = null; // ---------------- Helpers ---------------- async function jsonFetch(url, opts) { const r = await fetch(url, opts); const body = await r.json().catch(() => ({})); if (!r.ok) { const err = new Error(body.detail || `HTTP ${r.status}`); err.status = r.status; throw err; } return body; } function setText(id, v) { const el = $(id); if (el) el.textContent = v; } function drawBbox(canvas, img, bbox, label) { if (!canvas || !img || !bbox) return; canvas.width = img.naturalWidth || img.width; canvas.height = img.naturalHeight || img.height; const ctx = canvas.getContext('2d'); ctx.clearRect(0, 0, canvas.width, canvas.height); const x = bbox.x1, y = bbox.y1, w = bbox.x2 - bbox.x1, h = bbox.y2 - bbox.y1; ctx.strokeStyle = '#00ff00'; ctx.lineWidth = 3; ctx.strokeRect(x, y, w, h); if (label) { ctx.fillStyle = '#00ff00'; ctx.font = '14px monospace'; ctx.fillText(label, x, y - 4); } } function clearCanvas(canvas) { if (!canvas) return; const ctx = canvas.getContext('2d'); ctx.clearRect(0, 0, canvas.width, canvas.height); } // ---------------- Banner stato ---------------- async function refreshBanner() { let state; try { state = await jsonFetch('/api/right_arm/state'); } catch (e) { $('raStatusLabel').textContent = `Errore stato: ${e.message}`; $('raStatusBanner').hidden = false; $('raStatusBanner').className = 'ra-banner ra-banner-warn'; return; } const ok = state.robot_active === 'g1' && state.side === 'right'; const banner = $('raStatusBanner'); if (ok) { banner.hidden = true; } else { banner.hidden = false; banner.className = 'ra-banner ra-banner-warn'; $('raStatusLabel').textContent = `Robot attivo: ${state.robot_active} / ${state.side}. Per usare questa tab serve G1 / right.`; } return state; } async function onActivateClick() { try { await jsonFetch('/api/right_arm/activate', { method: 'POST' }); await refreshBanner(); await refreshPose(); } catch (e) { alert(`Attivazione fallita: ${e.message}`); } } // ---------------- Immagini ---------------- function urlWithBust(path) { return `${path}?t=${Date.now()}`; } function startFrontLoop() { if (frontTimer) clearInterval(frontTimer); const tick = () => { $('raFrontImg').src = urlWithBust('/api/right_arm/snapshot/front'); }; tick(); if ($('raFrontAutoRefresh').checked) { frontTimer = setInterval(tick, FRONT_REFRESH_MS); } } function startWristLoop() { if (wristTimer) clearInterval(wristTimer); const tick = () => { $('raWristImg').src = urlWithBust('/api/right_arm/snapshot/wrist'); }; tick(); if ($('raWristAutoRefresh').checked) { wristTimer = setInterval(tick, WRIST_REFRESH_MS); } } // ---------------- YOLO classes ---------------- async function populateClassDropdowns() { let classes = []; try { classes = await jsonFetch('/api/perception/yolo/classes'); if (Array.isArray(classes.classes)) classes = classes.classes; } catch (_) { classes = []; } const cfg = await jsonFetch('/api/config').catch(() => ({})); const dflt = (cfg.tab_right_arm && cfg.tab_right_arm.yolo_class) || 'pestello'; for (const id of ['raFrontClass', 'raWristClass']) { const sel = $(id); sel.innerHTML = ''; const names = classes.length ? classes.map(c => c.name || c) : [dflt]; for (const n of names) { const opt = document.createElement('option'); opt.value = n; opt.textContent = n; if (n === dflt) opt.selected = true; sel.appendChild(opt); } } if (cfg.tab_right_arm) { $('raFrontConf').value = cfg.tab_right_arm.yolo_conf_front ?? 0.4; $('raWristConf').value = cfg.tab_right_arm.yolo_conf_wrist ?? 0.4; } } // ---------------- Detect on-demand ---------------- async function onDetectFront() { const className = $('raFrontClass').value; const conf = parseFloat($('raFrontConf').value) || null; const proxy = window.location.origin; try { const res = await jsonFetch('/api/right_arm/detect/front', { method: 'POST', headers: { 'Content-Type': 'application/json' }, body: JSON.stringify({ class_name: className, conf, proxy_base_url: proxy }), }); const top = res.detections[0]; drawBbox($('raFrontCanvas'), $('raFrontImg'), top.bbox, `${top.class} ${(top.confidence * 100).toFixed(0)}%`); $('raFrontResult').classList.remove('dim'); $('raFrontResult').textContent = `classe: ${top.class} conf: ${top.confidence.toFixed(2)} ` + `bbox: [${Math.round(top.bbox.x1)},${Math.round(top.bbox.y1)},` + `${Math.round(top.bbox.x2)},${Math.round(top.bbox.y2)}]`; } catch (e) { clearCanvas($('raFrontCanvas')); $('raFrontResult').textContent = `Errore: ${e.message}`; } } async function onDetectWrist() { const className = $('raWristClass').value; const conf = parseFloat($('raWristConf').value) || null; try { const res = await jsonFetch('/api/right_arm/detect/wrist', { method: 'POST', headers: { 'Content-Type': 'application/json' }, body: JSON.stringify({ class_name: className, conf }), }); drawBbox($('raWristCanvas'), $('raWristImg'), res.detection.bbox, `${res.detection.class} ${(res.detection.confidence * 100).toFixed(0)}%`); const c = res.centroid_camera_m; $('raWristResult').classList.remove('dim'); $('raWristResult').textContent = `classe: ${res.detection.class} conf: ${res.detection.confidence.toFixed(2)}\n` + `centroide cam (m): x=${c[0].toFixed(3)} y=${c[1].toFixed(3)} z=${c[2].toFixed(3)}\n` + `distanza: ${(res.distance_m * 1000).toFixed(0)} mm ` + `punti SAM21: ${res.n_sam21_points}`; } catch (e) { clearCanvas($('raWristCanvas')); $('raWristResult').textContent = `Errore: ${e.message}`; } } // ---------------- Posa polso ---------------- async function refreshPose() { try { const s = await jsonFetch('/api/right_arm/state'); const p = s.wrist.position; const r = s.wrist.rpy_deg; setText('raPoseX', p[0].toFixed(3)); setText('raPoseY', p[1].toFixed(3)); setText('raPoseZ', p[2].toFixed(3)); setText('raPoseR', r[0].toFixed(1)); setText('raPoseP', r[1].toFixed(1)); setText('raPoseW', r[2].toFixed(1)); // Aggiorna anche i campi delle pose (solo se non sono stati toccati) if (s.home_pose) fillPoseFields('raHome', s.home_pose); if (s.custom_pose) fillPoseFields('raCust', s.custom_pose); if (s.offset_camera_mm) { $('raOffX').value = s.offset_camera_mm.x; $('raOffY').value = s.offset_camera_mm.y; $('raOffZ').value = s.offset_camera_mm.z; } if (s.axis_signs) { $('raSgnX').value = s.axis_signs.x; $('raSgnY').value = s.axis_signs.y; $('raSgnZ').value = s.axis_signs.z; } } catch (e) { // silenzioso: il polso può non essere disponibile } } function fillPoseFields(prefix, pose) { const xId = prefix + 'X'; if ($(xId).dataset.userTouched) return; $(xId).value = pose.position[0]; $(prefix + 'Y').value = pose.position[1]; $(prefix + 'Z').value = pose.position[2]; $(prefix + 'R').value = pose.rpy_deg[0]; $(prefix + 'P').value = pose.rpy_deg[1]; $(prefix + 'W').value = pose.rpy_deg[2]; } function readPoseFields(prefix) { return { position: [ parseFloat($(prefix + 'X').value), parseFloat($(prefix + 'Y').value), parseFloat($(prefix + 'Z').value), ], rpy_deg: [ parseFloat($(prefix + 'R').value), parseFloat($(prefix + 'P').value), parseFloat($(prefix + 'W').value), ], }; } // ---------------- Offset save ---------------- async function onSaveOffset() { try { await jsonFetch('/api/right_arm/offset', { method: 'PUT', headers: { 'Content-Type': 'application/json' }, body: JSON.stringify({ move_offset_camera_mm: { x: parseFloat($('raOffX').value), y: parseFloat($('raOffY').value), z: parseFloat($('raOffZ').value), }, axis_sign_cam_to_wrist: { x: parseInt($('raSgnX').value, 10), y: parseInt($('raSgnY').value, 10), z: parseInt($('raSgnZ').value, 10), }, }), }); alert('Offset salvato.'); } catch (e) { alert(`Errore salvataggio: ${e.message}`); } } // ---------------- Move toward ---------------- async function onMove() { const className = $('raWristClass').value; const conf = parseFloat($('raWristConf').value) || null; const dryRun = $('raMoveDryRun').checked; try { const res = await jsonFetch('/api/right_arm/move/toward', { method: 'POST', headers: { 'Content-Type': 'application/json' }, body: JSON.stringify({ class_name: className, conf, dry_run: dryRun }), }); const d = res.delta_wrist_mm; $('raMoveResult').classList.remove('dim'); $('raMoveResult').textContent = `delta polso (mm): X=${d[0].toFixed(1)} Y=${d[1].toFixed(1)} Z=${d[2].toFixed(1)} ` + `distanza: ${(res.distance_m * 1000).toFixed(0)} mm ` + `applicato: ${res.applied} z_clipped: ${res.z_clipped}`; if (!dryRun) await refreshPose(); } catch (e) { $('raMoveResult').textContent = `Errore: ${e.message}`; } } // ---------------- Home / Custom ---------------- async function onHome() { const dryRun = false; // home è "safe": default false if (!confirm('Eseguire move verso posa Home? (no dry-run)')) return; try { const res = await jsonFetch('/api/right_arm/home', { method: 'POST', headers: { 'Content-Type': 'application/json' }, body: JSON.stringify({ dry_run: dryRun }), }); await refreshPose(); alert(`Home eseguita: applied=${res.applied}`); } catch (e) { alert(`Errore Home: ${e.message}`); } } async function onCustomGo() { if (!confirm('Eseguire move verso posa Custom?')) return; try { await jsonFetch('/api/right_arm/custom_pose', { method: 'POST', headers: { 'Content-Type': 'application/json' }, body: JSON.stringify({ dry_run: false }), }); await refreshPose(); } catch (e) { alert(`Errore Custom: ${e.message}`); } } async function onCustomSave() { const pose = readPoseFields('raCust'); try { await jsonFetch('/api/right_arm/custom_pose', { method: 'PUT', headers: { 'Content-Type': 'application/json' }, body: JSON.stringify(pose), }); alert('Posa custom salvata in config.'); } catch (e) { alert(`Errore salvataggio: ${e.message}`); } } async function onCustomUseCurrent() { try { const s = await jsonFetch('/api/right_arm/state'); $('raCustX').value = s.wrist.position[0].toFixed(3); $('raCustY').value = s.wrist.position[1].toFixed(3); $('raCustZ').value = s.wrist.position[2].toFixed(3); $('raCustR').value = s.wrist.rpy_deg[0].toFixed(1); $('raCustP').value = s.wrist.rpy_deg[1].toFixed(1); $('raCustW').value = s.wrist.rpy_deg[2].toFixed(1); // Marca i campi come "user touched" così refreshPose non li sovrascrive for (const k of ['X','Y','Z','R','P','W']) $('raCust' + k).dataset.userTouched = '1'; } catch (e) { alert(`Errore lettura posa: ${e.message}`); } } // ---------------- Init ---------------- function markUserTouchedOnInput(prefix) { for (const k of ['X','Y','Z','R','P','W']) { const el = $(prefix + k); if (el) el.addEventListener('input', () => { el.dataset.userTouched = '1'; }); } } function init() { if (!$('tab-right-arm')) return; $('raActivateBtn').addEventListener('click', onActivateClick); $('raFrontDetectBtn').addEventListener('click', onDetectFront); $('raWristDetectBtn').addEventListener('click', onDetectWrist); $('raMoveBtn').addEventListener('click', onMove); $('raSaveOffsetBtn').addEventListener('click', onSaveOffset); $('raHomeBtn').addEventListener('click', onHome); $('raCustomGoBtn').addEventListener('click', onCustomGo); $('raCustomSaveBtn').addEventListener('click', onCustomSave); $('raCustomUseCurrentBtn').addEventListener('click', onCustomUseCurrent); $('raPoseRefreshBtn').addEventListener('click', refreshPose); $('raFrontAutoRefresh').addEventListener('change', startFrontLoop); $('raWristAutoRefresh').addEventListener('change', startWristLoop); markUserTouchedOnInput('raHome'); markUserTouchedOnInput('raCust'); populateClassDropdowns(); startFrontLoop(); startWristLoop(); refreshBanner(); refreshPose(); if (poseTimer) clearInterval(poseTimer); poseTimer = setInterval(refreshPose, POSE_REFRESH_MS); } if (document.readyState === 'loading') { document.addEventListener('DOMContentLoaded', init); } else { init(); } ``` - [ ] **Step 2: Update cache-busting in `src/presa_mano/main.py`** Locate: ```python for name in ("app.js", "work_plane.js", "style.css"): ``` Change to: ```python for name in ("app.js", "work_plane.js", "right_arm.js", "style.css"): ``` - [ ] **Step 3: Smoke test (manual)** ```bash # In un terminale separato: uv run uvicorn presa_mano.main:app --host 0.0.0.0 --port 8890 --reload ``` In browser: `http://localhost:8890`. Click sulla tab "Braccio DX". Verifica: - Banner stato visibile (verde se già g1/right, giallo altrimenti). - Immagine fronte si aggiorna (placeholder OK se la front camera non è raggiungibile dal browser/orchestrator — controllare console). - Immagine polso si aggiorna. - Click "Refresh" sulla posa polso → valori numerici (se G1 disponibile). - Pulsanti Home / Custom non testati senza robot fisico. - [ ] **Step 4: Commit** ```bash git add web/right_arm.js src/presa_mano/main.py git commit -m "feat(gui): right_arm.js — auto-refresh, detect, move, home, custom" ``` --- ## Task 13: Run full test suite **Files:** — - [ ] **Step 1: Esegui tutti i test del progetto** Run: `uv run pytest -v` Expected: tutti i test esistenti PASS + i 25+ nuovi test della tab Braccio DX PASS. - [ ] **Step 2: Se qualcosa rompe, fix e commit** I test esistenti non dovrebbero essere toccati da queste modifiche (router nuovo, nessuna firma esistente cambiata). Se rompono, è probabile un side-effect — investigare e fixare con un commit dedicato. - [ ] **Step 3: No commit se tutto passa al primo colpo** --- ## Self-Review **1. Spec coverage**: - ✅ Front camera reuso `NuwaClient` → Task 3 - ✅ Config `tab_right_arm` + `services.front_camera` → Task 1 - ✅ Conversioni RPY ↔ quaternion → Task 2 - ✅ Modulo math puro `right_arm/move.py` → Task 4 - ✅ Router `/state`, `/activate` → Task 5 - ✅ `/snapshot/{front,wrist}` → Task 6 - ✅ `/detect/front` con proxy fallback → Task 7 - ✅ `/detect/wrist` con YOLO+SAM21 → Task 8 - ✅ `/move/toward` con sign flips, max-step, z-floor → Task 9 - ✅ `/home`, `/custom_pose` (POST), PUT `/custom_pose`, PUT `/offset` → Task 10 - ✅ Wire router in main.py → Task 5 - ✅ Markup HTML → Task 11 - ✅ JS module → Task 12 - ✅ Cache-busting JS → Task 12 - ✅ Test geometry + move (unit) + routes (integration) → Task 2, 4, 5-10 - ✅ Smoke test manuale → Task 12 **2. Placeholder scan**: - Nessun TBD, TODO, "implement later". Step 3 di Task 2 ha una nota "Note: the line with `cx if False else ...`" che spiega ed elimina inline il typo — l'engineer scrive la riga corretta `qz = cr * cp * sy - sr * sp * cy`. Coverage del test parametrico cattura comunque l'errore. **3. Type consistency**: - `compute_delta_wrist(centroid, offset, axis_signs, max_step_m, force)` — firma costante tra Task 4 (definizione) e Task 9 (uso). - `apply_z_floor(wrist_z_m, delta, z_floor_m) -> (delta, clipped)` — coerente. - `rpy_deg_to_quaternion_xyzw(roll, pitch, yaw)` — coerente. - `_detect_wrist_core(class_name, conf, sw)` definita in Task 8, usata in Task 9. - `_top_detection(payload, class_name)` definita in Task 7, usata in Task 8 (`_detect_wrist_core`). - `_guard_g1_right()` definita in Task 9, usata anche in Task 10 (`_exec_named_pose`). - `_state_payload(sw)` definita in Task 5, usata da `/state` e `/activate`. - IDs HTML coerenti tra index.html (Task 11) e right_arm.js (Task 12): `raStatusBanner`, `raFrontImg`, ecc. Nessuna inconsistenza trovata.