feat(right_arm): perm assi cam->polso, precise=true default, verify_tag UI

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>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed 2026-05-21 15:44:48 +02:00
1 parent 8d40ebaf89
commit 57954b5c8d
8 files changed
+238 -23

No files matched your search

+70
View File
@@ -42,6 +42,7 @@ def _state_payload(sw: Stopwatch) -> dict[str, Any]:
"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"),
@@ -211,6 +212,7 @@ def move_toward(req: MoveTowardReq) -> dict[str, Any]:
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
@@ -221,9 +223,16 @@ def move_toward(req: MoveTowardReq) -> dict[str, Any]:
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:
@@ -340,6 +349,67 @@ def put_custom_pose(req: PoseUpdateReq) -> dict[str, Any]:
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] = {}