361 lines
13 KiB
Python
361 lines
13 KiB
Python
"""Endpoint per la tab Braccio DX (G1, polso destro, Nuwa eye-in-hand)."""
|
|
from __future__ import annotations
|
|
|
|
import logging
|
|
from typing import Any, Optional
|
|
|
|
from fastapi import APIRouter, HTTPException
|
|
from fastapi.responses import Response
|
|
from pydantic import BaseModel
|
|
|
|
import numpy as np
|
|
|
|
from ..clients import G1Unavailable
|
|
from ..config import update_config
|
|
from ..geometry import quaternion_xyzw_to_rpy_deg, rpy_deg_to_quaternion_xyzw
|
|
from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21
|
|
from ..right_arm.move import DeltaOversize, apply_z_floor, compute_delta_wrist
|
|
from ..timing import Stopwatch
|
|
from . import _deps
|
|
from .perception_routes import _bbox_to_roi
|
|
|
|
router = APIRouter()
|
|
log = logging.getLogger(__name__)
|
|
|
|
|
|
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")
|
|
|
|
|
|
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(),
|
|
}
|
|
|
|
|
|
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
|
|
|
|
|
|
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(),
|
|
}
|
|
|
|
|
|
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")}
|