scripts/yam_fingertip_probe.py

"""Probe the YAM gripper geometry: where the fingertips actually are relative to link_6.

The pick scripts assume the grasp reference point sits ``OFF=0.13`` m down the link_6 approach
axis. This prints the measured fingertip position (body poses + world bboxes of the finger links)
so that offset can be set from the real asset instead of by feel.

    python scripts/yam_fingertip_probe.py --headless --kit_args="--/rtx/verifyDriverVersion/enabled=false"
"""
import argparse, sys, os
from isaaclab.app import AppLauncher
p = argparse.ArgumentParser()
AppLauncher.add_app_launcher_args(p)
args = p.parse_args(); args.headless = True
app = AppLauncher(args).app
import numpy as np, torch, gymnasium as gym
REPO = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.insert(0, os.path.join(REPO, "source")); sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import bimanual.tasks.manager_based.yam  # noqa
from isaaclab_tasks.utils import parse_env_cfg

TASK = "Template-YAM-Play-v0"; dev = "cuda:0"
cfg = parse_env_cfg(TASK, device=dev, num_envs=1)
env = gym.make(TASK, cfg=cfg, render_mode=None); u = env.unwrapped; env.reset()
R = u.scene["right_robot"]; bn = list(R.data.body_names)
print("[probe] body_names:", bn, flush=True)
print("[probe] joint_names:", list(R.data.joint_names), flush=True)


def Rq(q):
    w, x, y, z = q
    return np.array([[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
                     [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
                     [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)]])


i6 = bn.index("link_6")
p6 = R.data.body_pos_w[0, i6].cpu().numpy(); q6 = R.data.body_quat_w[0, i6].cpu().numpy()
M6 = Rq(q6)
for name in bn:
    j = bn.index(name)
    pj = R.data.body_pos_w[0, j].cpu().numpy()
    local = M6.T @ (pj - p6)                      # body origin in link_6 frame
    print(f"[probe] body {name:16s} world={np.round(pj,4)} in_link6={np.round(local,4)}", flush=True)

# world bbox of each finger link -> lowest point along the link_6 approach axis (= fingertip)
try:
    import omni.usd
    from pxr import UsdGeom, Usd
    stage = omni.usd.get_context().get_stage()
    root = R.root_physx_view.prim_paths[0]
    bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
    approach = M6 @ np.array([0.0, 0.0, 1.0])     # link_6 +z; the pick scripts drive -z downward
    for name in [b for b in bn if "finger" in b.lower()] or bn[-2:]:
        prim = stage.GetPrimAtPath(f"{root}/{name}")
        if not prim.IsValid():
            print(f"[probe] no prim for {name} at {root}/{name}", flush=True); continue
        rng = bb.ComputeWorldBound(prim).ComputeAlignedRange()
        lo = np.array(rng.GetMin()); hi = np.array(rng.GetMax())
        corners = np.array([[x, y, z] for x in (lo[0], hi[0]) for y in (lo[1], hi[1]) for z in (lo[2], hi[2])])
        d = (corners - p6) @ approach             # signed distance along +approach
        print(f"[probe] {name}: bbox world lo={np.round(lo,4)} hi={np.round(hi,4)} "
              f"along_link6_z: min={d.min():+.4f} max={d.max():+.4f} (tip offset = {-d.min():.4f} m below link_6)",
              flush=True)
except Exception as e:
    print("[probe] bbox probe failed:", e, flush=True)

env.close(); app.close(); print("PROBE_OK", flush=True)