76 lines
2.3 KiB
Python
76 lines
2.3 KiB
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 fastapi.responses import Response
|
|
|
|
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))
|
|
|
|
|
|
@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")
|