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:
1 parent
8d40ebaf89
commit
57954b5c8d
8 files changed
+238
-23
No files matched your search
@@ -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] = {}
|
||||
|
||||
Reference in new issue
Block a user