diff --git a/config.yaml b/config.yaml index a918341..475be3f 100644 --- a/config.yaml +++ b/config.yaml @@ -48,11 +48,15 @@ tab_right_arm: x: 1 y: -1 z: -1 + cam_to_wrist_perm: [z, x, y] # wrist_X<-cam_Z, wrist_Y<-cam_X, wrist_Z<-cam_Y camera_offset_wrist_mm: x: 0.0 y: 0.0 z: 100.0 - cluster_depth_window_m: 0.20 + cluster_depth_window_m: 0.2 + verify_marker: + id: 1 + size_mm: 40.0 max_step_m: 0.15 z_floor_m: null home_pose: @@ -73,6 +77,9 @@ tab_right_arm: - 0 - 0 - 0 +pose_3d: + live_max_step_m: 0.15 + named_poses: {} grasp: approach_offset_m: 0.1 pre_grasp_clearance_m: 0.05 diff --git a/src/presa_mano/api/pose_3d_routes.py b/src/presa_mano/api/pose_3d_routes.py new file mode 100644 index 0000000..63317bb --- /dev/null +++ b/src/presa_mano/api/pose_3d_routes.py @@ -0,0 +1,55 @@ +"""Endpoint per la tab "Pose 3D": editor visuale + jog dei polsi.""" +from __future__ import annotations + +import logging +from typing import Any, Literal, Optional + +from fastapi import APIRouter, HTTPException +from pydantic import BaseModel, Field + +from ..clients import G1Unavailable +from ..config import get_config +from ..timing import Stopwatch +from . import _deps + +router = APIRouter() +log = logging.getLogger(__name__) + + +class WristPose(BaseModel): + position: list[float] + quaternion: list[float] + + +class NamedPose(BaseModel): + side: Literal["left", "right"] + position: list[float] = Field(min_length=3, max_length=3) + quaternion: list[float] = Field(min_length=4, max_length=4) + note: Optional[str] = None + + +@router.get("/state") +def get_state() -> dict[str, Any]: + sw = Stopwatch() + try: + with sw.step("g1_wrist"): + w = _deps.g1().wrist() + except G1Unavailable as e: + raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}") + cfg = get_config().get("pose_3d", {}) or {} + return { + "robot_active": _deps.active_robot_key(), + "wrist": { + "left": { + "position": list(w["left_wrist"]["position"]), + "quaternion": list(w["left_wrist"]["quaternion"]), + }, + "right": { + "position": list(w["right_wrist"]["position"]), + "quaternion": list(w["right_wrist"]["quaternion"]), + }, + }, + "poses": cfg.get("named_poses") or {}, + "live_max_step_m": float(cfg.get("live_max_step_m", 0.15)), + "timings_ms": sw.as_dict(), + } diff --git a/src/presa_mano/main.py b/src/presa_mano/main.py index da7a172..7927383 100644 --- a/src/presa_mano/main.py +++ b/src/presa_mano/main.py @@ -17,6 +17,7 @@ from .api import ( home_routes, models_routes, perception_routes, + pose_3d_routes, posture_routes, right_arm_routes, robot_routes, @@ -48,6 +49,7 @@ app.include_router(posture_routes.router, prefix="/api/posture", tags=["posture" app.include_router(robot_routes.router, prefix="/api/robot", tags=["robot"]) app.include_router(right_arm_routes.router, prefix="/api/right_arm", tags=["right_arm"]) app.include_router(work_plane_routes.router, prefix="/api/work_plane", tags=["work_plane"]) +app.include_router(pose_3d_routes.router, prefix="/api/pose_3d", tags=["pose_3d"]) WEB_DIR = Path(__file__).resolve().parents[2] / "web" if WEB_DIR.exists(): diff --git a/tests/test_pose_3d_routes.py b/tests/test_pose_3d_routes.py new file mode 100644 index 0000000..09054ce --- /dev/null +++ b/tests/test_pose_3d_routes.py @@ -0,0 +1,43 @@ +"""Integration test del router /api/pose_3d/*.""" +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_pair(): + 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_both_wrists(): + 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 + body = r.json() + assert "wrist" in body + assert body["wrist"]["left"]["position"] == [0.10, 0.20, 0.30] + assert body["wrist"]["right"]["position"] == [0.30, -0.20, 0.30] + assert "poses" in body + assert "live_max_step_m" in body + assert body["robot_active"] in ("g1", "h1") + + +def test_state_502_when_g1_unavailable(): + from presa_mano.clients import G1Unavailable + with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory: + g1 = MagicMock() + g1.wrist.side_effect = G1Unavailable("connection refused") + g1_factory.return_value = g1 + r = client.get("/api/pose_3d/state") + assert r.status_code == 502