"""Isolated gripper test: feed -1 then +1, read finger 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, torch, 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"; dev="cuda:0"
env=gym.make(TASK, cfg=parse_env_cfg(TASK, device=dev, num_envs=1)); u=env.unwrapped
obs,_=env.reset()
R=u.scene["right_robot"]; jn=list(R.data.joint_names)
lf=jn.index("left_finger"); rf=jn.index("right_finger")
def fpos(): return float(R.data.joint_pos[0,lf].item()), float(R.data.joint_pos[0,rf].item())
# hold arm at zero-ish; action = [L_arm7,L_grip1,R_arm7,R_grip1]; keep arm poses = current eef won't matter for gripper.
# Use identity-ish arm command = current joint-derived pose is complex; instead feed a stay by reading eef.
# Simpler: just feed zero pose (arm may move but we only watch fingers). Use small action.
def act(grip):
    z=np.zeros(16,np.float32); z[7]=1.0; z[15]=grip  # left grip +1 (open), right grip = grip
    # arm pose slots left as zeros -> IK target origin; fine for gripper test
    return torch.tensor(z,dtype=torch.float32,device=dev).view(1,-1)
print("[t] reset fingers:", np.round(fpos(),4), flush=True)
for _ in range(40): env.step(act(-1.0))
print("[t] after grip=-1.0 (x40):", np.round(fpos(),4), flush=True)
for _ in range(40): env.step(act(+1.0))
print("[t] after grip=+1.0 (x40):", np.round(fpos(),4), flush=True)
# direct velocity target test
R.set_joint_velocity_target(torch.tensor([[0.5,0.5]],device=dev), joint_ids=[lf,rf])
for _ in range(40): u.sim.step()
print("[t] after direct vel +0.5 (x40):", np.round(fpos(),4), flush=True)
env.close(); app.close(); print("GRIP_TEST_OK", flush=True)