File size: 3,318 Bytes
7399b6f | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 | """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)
|