"""Read YAM arm joint names, limits, and the home (init) joint positions."""
import argparse, sys, os
from isaaclab.app import AppLauncher
p=argparse.ArgumentParser(); AppLauncher.add_app_launcher_args(p); a=p.parse_args(); a.headless=True; a.enable_cameras=False
app=AppLauncher(a).app
import numpy as np, gymnasium as gym
REPO=os.path.dirname(os.path.dirname(os.path.abspath(__file__))); sys.path.insert(0,os.path.join(REPO,"source"))
import bimanual.tasks.manager_based.yam  # noqa
from isaaclab_tasks.utils import parse_env_cfg
TASK="Template-YAM-Play-v0"
env=gym.make(TASK, cfg=parse_env_cfg(TASK, device="cuda:0", num_envs=1)); u=env.unwrapped
env.reset()
R=u.scene["right_robot"]
jn=list(R.data.joint_names)
lim=R.data.joint_pos_limits[0].cpu().numpy()  # (J,2)
pos=R.data.joint_pos[0].cpu().numpy()
print("[j] joint | home(rad) | home(deg) | lower | upper", flush=True)
for i,n in enumerate(jn):
    deg=np.degrees(pos[i]) if "finger" not in n else pos[i]
    print(f"[j] {n:14s} {pos[i]:+.4f}  {deg:+8.2f}  [{lim[i,0]:+.4f}, {lim[i,1]:+.4f}]", flush=True)
# body chain
print("[j] bodies:", list(R.data.body_names), flush=True)
env.close(); app.close(); print("JINFO_OK", flush=True)