Adriano Dal PastroandClaude Opus 4.7
57954b5c8d
Lavoro accumulato nella sessione 2026-05-21 prima del brainstorming
della tab Pose 3D:
- compute_delta_wrist generalizzato con cam_to_wrist_perm: la mappa
cam->polso non e' solo sign-flip ma una permutazione (default
identita' per backward compat). DeltaOversize ora espone X/Y/Z
nella stringa d'errore. Test aggiornati.
- G1Client.wrist_absolute / wrist_relative: precise=True di default
(era False). Stessa cosa per wrist_routes.AbsoluteReq Pydantic
e per agent.tools move_wrist_*. Movimenti reali ora chiedono
controllo accurato salvo override esplicito.
- Tab Braccio DX:
- /verify_tag endpoint: snapshot wrist FK + tag id=1 dalla front
camera. Per verifica relativa moveto vs marker (no calibrazione).
- GUI: pannello "Verifica tag id=1" + campi camera_offset_wrist_mm.
- markUserTouchedOnInput esteso a raOff/raSgn/raCamOff.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
439 lines
17 KiB
Python
439 lines
17 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.detector import _normalize as _normalize_yolo
|
|
from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21
|
|
from ..right_arm.cluster import filter_nearest_cluster
|
|
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"),
|
|
"camera_offset_wrist_mm": cfg.get("camera_offset_wrist_mm"),
|
|
"cam_to_wrist_perm": cfg.get("cam_to_wrist_perm"),
|
|
"cluster_depth_window_m": cfg.get("cluster_depth_window_m"),
|
|
"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.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 = _normalize_yolo(payload)
|
|
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 = _normalize_yolo(res)
|
|
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)
|
|
with sw.step("front_size"):
|
|
w, h = cam.image_size()
|
|
return {
|
|
"detections": dets_filtered,
|
|
"image_size": [w, h],
|
|
"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_all = np.asarray(seg.points_cv_m, dtype=np.float64)
|
|
if pts_all.shape[0] < 10:
|
|
raise HTTPException(status_code=422, detail=f"too few sam21 points: {pts_all.shape[0]}")
|
|
cfg = _deps.tab_right_arm_cfg()
|
|
window_m = float(cfg.get("cluster_depth_window_m", 0.20))
|
|
with sw.step("cluster_filter"):
|
|
pts, cluster_diag = filter_nearest_cluster(pts_all, window_m=window_m)
|
|
if pts.shape[0] < 10:
|
|
raise HTTPException(
|
|
status_code=422,
|
|
detail=f"cluster troppo piccolo: {pts.shape[0]} punti dopo filtro "
|
|
f"(z_near={cluster_diag['z_near_m']:.3f}m window={window_m:.3f}m)")
|
|
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_all.shape[0]),
|
|
"n_centroid_points": int(pts.shape[0]),
|
|
"cluster_window_m": window_m,
|
|
"cluster_z_near_m": cluster_diag["z_near_m"],
|
|
"cluster_z_cutoff_m": cluster_diag["z_cutoff_m"],
|
|
}
|
|
|
|
|
|
@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
|
|
|
|
|
|
@router.post("/move/toward")
|
|
def move_toward(req: MoveTowardReq) -> dict[str, Any]:
|
|
sw = Stopwatch()
|
|
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})
|
|
cam_off_mm = cfg.get("camera_offset_wrist_mm", {"x": 0, "y": 0, "z": 0})
|
|
perm_cfg = cfg.get("cam_to_wrist_perm", ["x", "y", "z"])
|
|
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
|
|
cam_offset_wrist_m = np.array(
|
|
[cam_off_mm["x"], cam_off_mm["y"], cam_off_mm["z"]], dtype=np.float64) / 1000.0
|
|
signs = (int(sign["x"]), int(sign["y"]), int(sign["z"]))
|
|
_AXIS_IDX = {"x": 0, "y": 1, "z": 2}
|
|
try:
|
|
perm = tuple(_AXIS_IDX[str(p).lower()] for p in perm_cfg)
|
|
except KeyError as e:
|
|
raise HTTPException(status_code=422,
|
|
detail=f"cam_to_wrist_perm: asse sconosciuto {e}")
|
|
|
|
try:
|
|
delta = compute_delta_wrist(centroid, offset_m, signs,
|
|
cam_to_wrist_perm=perm,
|
|
camera_offset_wrist_m=cam_offset_wrist_m,
|
|
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),
|
|
"camera_offset_wrist_m": cam_offset_wrist_m.tolist(),
|
|
"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
|
|
camera_offset_wrist_mm: Optional[dict[str, float]] = None
|
|
|
|
|
|
def _exec_named_pose(pose_key: str, dry_run: bool, sw: Stopwatch) -> dict[str, Any]:
|
|
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")}
|
|
|
|
|
|
def _front_cam_marker_intrinsics() -> dict[str, Any]:
|
|
"""Estrae le intrinsics RGB della front camera nel formato marker-finder
|
|
(camera_matrix + dist_coeffs). marker-finder ha intrinsics 'baked-in'
|
|
della D435i che non combaciano con la Nuwa: passarle per-call evita
|
|
errori di scala sistematici sulla posa del tag."""
|
|
intr = _deps.front_camera().intrinsics()
|
|
rgb = (intr or {}).get("rgb") or {}
|
|
K = [[float(rgb.get("fx", 0)), 0.0, float(rgb.get("cx", 0))],
|
|
[0.0, float(rgb.get("fy", 0)), float(rgb.get("cy", 0))],
|
|
[0.0, 0.0, 1.0]]
|
|
dist = rgb.get("distortion") or {}
|
|
d = [float(dist.get("k1", 0.0)), float(dist.get("k2", 0.0)),
|
|
float(dist.get("p1", 0.0)), float(dist.get("p2", 0.0)),
|
|
float(dist.get("k3", 0.0))]
|
|
return {"camera_matrix": K, "dist_coeffs": d}
|
|
|
|
|
|
@router.post("/verify_tag")
|
|
def post_verify_tag() -> dict[str, Any]:
|
|
"""Snapshot per verifica relativa: posa polso (G1 FK) + posa tag (marker
|
|
finder sulla front camera). Da chiamare ripetutamente con il polso in
|
|
posizioni diverse; il client confronta i moduli dei delta consecutivi.
|
|
|
|
Niente calibrazione assoluta richiesta: il tag e' rigidamente attaccato
|
|
al polso, quindi ||Δtag|| dovrebbe approssimare ||Δwrist|| (a meno di
|
|
qualche % di errore intrinsics)."""
|
|
sw = Stopwatch()
|
|
cfg = _deps.tab_right_arm_cfg()
|
|
vm = cfg.get("verify_marker") or {"id": 1, "size_mm": 40.0}
|
|
try:
|
|
with sw.step("g1_wrist"):
|
|
wrist = _deps.g1().wrist()["right_wrist"]
|
|
except G1Unavailable as e:
|
|
raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
|
|
try:
|
|
with sw.step("front_intr"):
|
|
intr = _front_cam_marker_intrinsics()
|
|
with sw.step("marker_detect"):
|
|
image_url = _deps.front_camera().rgb_url()
|
|
res = _deps.marker().detect_url(
|
|
markers=[{"id": int(vm["id"]), "size_mm": float(vm["size_mm"])}],
|
|
image_url=image_url,
|
|
intrinsics=intr,
|
|
)
|
|
except Exception as e:
|
|
raise HTTPException(status_code=502, detail=f"marker-finder: {e}")
|
|
found = (res or {}).get("found") or []
|
|
if not found:
|
|
raise HTTPException(status_code=404,
|
|
detail=f"tag id={vm['id']} non rilevato dalla front camera")
|
|
tag = found[0]
|
|
return {
|
|
"marker_id": int(vm["id"]),
|
|
"wrist_position_m": list(wrist["position"]),
|
|
"wrist_quaternion": list(wrist["quaternion"]),
|
|
"tag_position_m_in_front_cam": list(tag["position_m"]),
|
|
"tag_quaternion": list(tag.get("quaternion_xyzw") or []),
|
|
"timings_ms": sw.as_dict(),
|
|
}
|
|
|
|
|
|
@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 req.camera_offset_wrist_mm is not None:
|
|
for k in ("x", "y", "z"):
|
|
if k not in req.camera_offset_wrist_mm:
|
|
raise HTTPException(status_code=422, detail=f"camera_offset_wrist_mm missing '{k}'")
|
|
patch["camera_offset_wrist_mm"] = {k: float(req.camera_offset_wrist_mm[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"),
|
|
"camera_offset_wrist_mm": cfg.get("camera_offset_wrist_mm")}
|