diff --git a/src/presa_mano/agent/tools.py b/src/presa_mano/agent/tools.py
index c213fa9..c2414e7 100644
--- a/src/presa_mano/agent/tools.py
+++ b/src/presa_mano/agent/tools.py
@@ -121,7 +121,7 @@ def _h_move_wrist_absolute(args: dict[str, Any]) -> dict[str, Any]:
return _err("position deve avere 3 float (m), quaternion 4 float (xyzw)")
dry_run = bool(args.get("dry_run", True))
timeout_s = float(args.get("timeout_s", 5.0))
- precise = bool(args.get("precise", False))
+ precise = bool(args.get("precise", True))
result = _deps.g1(dry_run=dry_run).wrist_absolute(
side=side, position=position, quaternion=quaternion,
wait=True, timeout_s=timeout_s, precise=precise,
@@ -144,7 +144,7 @@ def _h_move_wrist_relative(args: dict[str, Any]) -> dict[str, Any]:
return _err("delta_quaternion deve avere 4 float (xyzw) o essere null")
dry_run = bool(args.get("dry_run", True))
timeout_s = float(args.get("timeout_s", 5.0))
- precise = bool(args.get("precise", False))
+ precise = bool(args.get("precise", True))
result = _deps.g1(dry_run=dry_run).wrist_relative(
side=side,
delta_position=list(dp) if dp is not None else None,
@@ -516,8 +516,8 @@ TOOL_DEFINITIONS: list[dict[str, Any]] = [
"quaternion": {"type": "array", "items": {"type": "number"}, "minItems": 4, "maxItems": 4,
"description": "x,y,z,w"},
"dry_run": {"type": "boolean", "default": True},
- "precise": {"type": "boolean", "default": False,
- "description": "true = controllo accurato (più lento)"},
+ "precise": {"type": "boolean", "default": True,
+ "description": "true = controllo accurato (più lento, default)"},
"timeout_s": {"type": "number", "default": 5.0},
},
},
@@ -540,7 +540,7 @@ TOOL_DEFINITIONS: list[dict[str, Any]] = [
"delta_position": {"type": "array", "items": {"type": "number"}, "minItems": 3, "maxItems": 3},
"delta_quaternion": {"type": "array", "items": {"type": "number"}, "minItems": 4, "maxItems": 4},
"dry_run": {"type": "boolean", "default": True},
- "precise": {"type": "boolean", "default": False},
+ "precise": {"type": "boolean", "default": True},
"timeout_s": {"type": "number", "default": 5.0},
},
},
diff --git a/src/presa_mano/api/right_arm_routes.py b/src/presa_mano/api/right_arm_routes.py
index 5ccfe0b..5be7e41 100644
--- a/src/presa_mano/api/right_arm_routes.py
+++ b/src/presa_mano/api/right_arm_routes.py
@@ -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] = {}
diff --git a/src/presa_mano/api/wrist_routes.py b/src/presa_mano/api/wrist_routes.py
index 78f6edc..130b7b4 100644
--- a/src/presa_mano/api/wrist_routes.py
+++ b/src/presa_mano/api/wrist_routes.py
@@ -19,7 +19,7 @@ class AbsoluteReq(BaseModel):
quaternion: list[float] = Field(..., min_length=4, max_length=4)
wait: bool = True
timeout_s: float = 5.0
- precise: bool = False
+ precise: bool = True
dry_run: bool = False
diff --git a/src/presa_mano/clients/g1.py b/src/presa_mano/clients/g1.py
index 5d4add3..8978fb2 100644
--- a/src/presa_mano/clients/g1.py
+++ b/src/presa_mano/clients/g1.py
@@ -61,7 +61,7 @@ class G1Client:
# ----- mutating -----
def wrist_absolute(self, side: str, position: list[float], quaternion: list[float],
- wait: bool = True, timeout_s: float = 5.0, precise: bool = False) -> dict[str, Any]:
+ wait: bool = True, timeout_s: float = 5.0, precise: bool = True) -> dict[str, Any]:
body = {
"side": side, "position": position, "quaternion": quaternion,
"wait": wait, "timeout_s": timeout_s, "precise": precise,
@@ -70,7 +70,7 @@ class G1Client:
def wrist_relative(self, side: str, delta_position: list[float] | None = None,
delta_quaternion: list[float] | None = None, wait: bool = True,
- timeout_s: float = 5.0, precise: bool = False) -> dict[str, Any]:
+ timeout_s: float = 5.0, precise: bool = True) -> dict[str, Any]:
# `precise` is forwarded but the bridge silently ignores it on
# /wrist/relative if not supported — safer to always send it.
body: dict[str, Any] = {"side": side, "wait": wait, "timeout_s": timeout_s,
diff --git a/src/presa_mano/right_arm/move.py b/src/presa_mano/right_arm/move.py
index eb7a062..5669167 100644
--- a/src/presa_mano/right_arm/move.py
+++ b/src/presa_mano/right_arm/move.py
@@ -5,11 +5,23 @@ import numpy as np
class DeltaOversize(ValueError):
- """Sollevata quando ||delta|| supera max_step_m e force=False."""
+ """Sollevata quando ||delta|| supera max_step_m e force=False.
- def __init__(self, delta_norm_m: float, max_step_m: float):
- super().__init__(f"delta {delta_norm_m*1000:.1f}mm > max {max_step_m*1000:.1f}mm")
- self.delta_norm_m = float(delta_norm_m)
+ Espone la scomposizione X/Y/Z del delta richiesto sia come attributo
+ `delta_m` sia nel messaggio, cosi' chi riceve l'errore (GUI o CLI) puo'
+ vedere subito su quali assi sta lavorando senza dover rifare il calcolo
+ con force=true."""
+
+ def __init__(self, delta_m: np.ndarray, max_step_m: float):
+ delta = np.asarray(delta_m, dtype=np.float64).reshape(3)
+ n = float(np.linalg.norm(delta))
+ mm = delta * 1000.0
+ super().__init__(
+ f"delta {n*1000:.1f}mm > max {max_step_m*1000:.1f}mm "
+ f"[X={mm[0]:.1f} Y={mm[1]:.1f} Z={mm[2]:.1f}]"
+ )
+ self.delta_m = delta
+ self.delta_norm_m = n
self.max_step_m = float(max_step_m)
@@ -17,6 +29,7 @@ def compute_delta_wrist(
centroid_camera_m: np.ndarray,
offset_camera_m: np.ndarray,
axis_signs: tuple[int, int, int],
+ cam_to_wrist_perm: tuple[int, int, int] = (0, 1, 2),
camera_offset_wrist_m: np.ndarray | None = None,
max_step_m: float | None = None,
force: bool = False,
@@ -24,29 +37,36 @@ def compute_delta_wrist(
"""Delta da applicare al polso (frame polso) per portare il TCP sull'offset
di presa dall'oggetto osservato dalla camera.
- Modello: assi camera ≈ assi polso a meno dei segni; la camera è montata sul
- polso con traslazione costante `camera_offset_wrist_m` (origine polso →
- origine camera, espressa nel frame polso).
+ Modello: la camera e' montata sul polso con orientamento che e' una
+ permutazione (+/-) degli assi del polso. `cam_to_wrist_perm = (i_x, i_y, i_z)`
+ indica da quale componente del frame camera proviene ciascun asse polso:
+ `wrist_X = sign_X * cam[i_x]`, etc. `axis_signs` applica il segno DOPO la
+ permutazione. La traslazione costante `camera_offset_wrist_m` e' l'origine
+ della camera nel frame polso.
- delta_wrist = sign * (centroid_camera - offset_camera) + t_wrist_cam
+ delta_wrist = sign * (centroid_cam[perm] - offset_cam[perm]) + t_wrist_cam
- Il termine `+ t_wrist_cam` compensa il fatto che camera e TCP non coincidono:
- spostare il polso di `sign * (centroid - offset)` porterebbe la CAMERA sul
- punto desiderato, lasciando il TCP indietro di t_wrist_cam.
+ Esempio (Nuwa eye-in-hand che guarda avanti come la mano, polso destro G1):
+ cam_to_wrist_perm = (2, 0, 1) # wrist_X<-cam_Z, _Y<-cam_X, _Z<-cam_Y
+ axis_signs = (+1, -1, -1)
- Se `max_step_m` è settato e ||delta|| > max_step_m e `force` è False, solleva
- DeltaOversize.
+ Default `perm = (0, 1, 2)`: nessuna permutazione, modello vecchio sign-only.
+ Se `max_step_m` e' settato e ||delta|| > max_step_m e `force` e' False,
+ solleva DeltaOversize.
"""
centroid = np.asarray(centroid_camera_m, dtype=np.float64).reshape(3)
offset = np.asarray(offset_camera_m, dtype=np.float64).reshape(3)
sign = np.asarray(axis_signs, dtype=np.float64).reshape(3)
+ perm = np.asarray(cam_to_wrist_perm, dtype=np.intp).reshape(3)
+ if not (np.all((perm >= 0) & (perm <= 2)) and len(set(perm.tolist())) == 3):
+ raise ValueError(f"cam_to_wrist_perm must be a permutation of (0,1,2), got {perm.tolist()}")
t_wc = (np.zeros(3, dtype=np.float64) if camera_offset_wrist_m is None
else np.asarray(camera_offset_wrist_m, dtype=np.float64).reshape(3))
- delta = sign * (centroid - offset) + t_wc
+ delta = sign * (centroid[perm] - offset[perm]) + t_wc
if max_step_m is not None and not force:
n = float(np.linalg.norm(delta))
if n > max_step_m:
- raise DeltaOversize(n, max_step_m)
+ raise DeltaOversize(delta, max_step_m)
return delta
diff --git a/tests/test_right_arm_move.py b/tests/test_right_arm_move.py
index 936363a..7e948d3 100644
--- a/tests/test_right_arm_move.py
+++ b/tests/test_right_arm_move.py
@@ -39,6 +39,40 @@ def test_signs_applied_componentwise():
assert np.allclose(delta, [0.05, -0.10, -0.20])
+def test_permutation_maps_cam_axes_to_wrist_axes():
+ # Mounting: cam guarda avanti = wrist_X. perm=(2,0,1): wrist_X<-cam_Z,
+ # wrist_Y<-cam_X, wrist_Z<-cam_Y. signs=(+1,-1,-1).
+ # centroid=(0.10, 0.05, 0.30) → wrist=(+0.30, -0.10, -0.05) prima offset/t_wc
+ delta = compute_delta_wrist(
+ centroid_camera_m=np.array([0.10, 0.05, 0.30]),
+ offset_camera_m=np.array([0.0, 0.0, 0.0]),
+ axis_signs=(1, -1, -1),
+ cam_to_wrist_perm=(2, 0, 1),
+ )
+ assert np.allclose(delta, [0.30, -0.10, -0.05])
+
+
+def test_permutation_default_is_identity_back_compat():
+ # Senza perm esplicito, comportamento = sign-only (modello vecchio).
+ delta_new = compute_delta_wrist(
+ centroid_camera_m=np.array([0.10, 0.20, 0.30]),
+ offset_camera_m=np.array([0.0, 0.0, 0.0]),
+ axis_signs=(1, -1, -1),
+ )
+ assert np.allclose(delta_new, [0.10, -0.20, -0.30])
+
+
+def test_invalid_permutation_raises():
+ import pytest as _pt
+ with _pt.raises(ValueError, match="permutation"):
+ compute_delta_wrist(
+ centroid_camera_m=np.zeros(3),
+ offset_camera_m=np.zeros(3),
+ axis_signs=(1, 1, 1),
+ cam_to_wrist_perm=(0, 0, 1),
+ )
+
+
def test_camera_offset_wrist_added_after_sign_flip():
# centroid=(0,0,1) offset=(0,0,0) sign=(1,1,1) t_wc=(0.05,-0.02,0.08)
# → sign*(centroid-offset) = (0,0,1)
@@ -73,6 +107,25 @@ def test_oversize_raises_when_not_forced():
)
+def test_oversize_message_includes_per_axis_breakdown():
+ # delta = (1,-1,-1) * ((0.10, -0.02, 0.50) - 0) = (0.10, 0.02, -0.50)
+ # ||delta|| = sqrt(0.01+0.0004+0.25) = ~0.510m, max=0.15
+ with pytest.raises(DeltaOversize) as exc_info:
+ compute_delta_wrist(
+ centroid_camera_m=np.array([0.10, -0.02, 0.50]),
+ offset_camera_m=np.array([0.0, 0.0, 0.0]),
+ axis_signs=(1, -1, -1),
+ max_step_m=0.15,
+ force=False,
+ )
+ msg = str(exc_info.value)
+ assert "X=100.0" in msg
+ assert "Y=20.0" in msg
+ assert "Z=-500.0" in msg
+ # E l'oggetto espone il vettore strutturato
+ assert np.allclose(exc_info.value.delta_m, [0.10, 0.02, -0.50])
+
+
def test_oversize_passes_when_forced():
delta = compute_delta_wrist(
centroid_camera_m=np.array([0.0, 0.0, 0.5]),
diff --git a/web/index.html b/web/index.html
index 3d8df1c..9d4e511 100644
--- a/web/index.html
+++ b/web/index.html
@@ -415,6 +415,22 @@
— nessun move
+
+
+
Verifica tag id=1 (relativa, no calibrazione)
+
+ Premi Snapshot con il polso fermo, muovi il polso, premi ancora.
+ Per ogni coppia consecutiva confronto ‖Δpolso‖ (FK) con ‖Δtag‖ (front camera).
+ Se combaciano (errore < ~10%) FK e detection sono coerenti.
+