Files
a7l20-hug-sim/scripts/test_cup_dynamic_grasp.py

87 lines
5.4 KiB
Python

"""Simulation-only grasp trial: ideal arm tracking, force-limited hand, free cup."""
from pathlib import Path
import json,os
import numpy as np
import mujoco
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation as Rot
ROOT=Path(__file__).resolve().parents[1];out=ROOT/os.environ.get('CUP_TRIAL_DIR','data/cup_approach_search')
s=mujoco.MjSpec.from_file(str(ROOT/'scene/cup_workcell.xml'))
s.body('measured_cup').add_freejoint(name='cup_free')
s.option.timestep=.002
# Explicit assumed simulation parameters, not identified physical properties.
for geom in s.geoms:
if geom.name.startswith('cup_'):
geom.contype=2;geom.conaffinity=1;geom.friction=[1.,.01,.001];geom.mass=.25/8
elif geom.name=='desk_top':geom.contype=1;geom.conaffinity=2
elif geom.parent.name.startswith('left_hand_'):
geom.contype=1;geom.conaffinity=2;geom.friction=[1.,.01,.001]
# Keyframes have old qpos dimension after adding a free body.
for key in list(s.keys):s.delete(key)
# Simulation servo tuning; retain original torque limits.
for actuator in s.actuators:
actuator.gainprm[0]=2.
actuator.biasprm[1]=-2.
actuator.biasprm[2]=-.08
m=s.compile();(out/'dynamic_scene.xml').write_text(s.to_xml());d=mujoco.MjData(m)
old=mujoco.MjModel.from_xml_path(str(ROOT/'scene/cup_workcell.xml'));trajectory=np.load(out/'approach_qpos.npy');start=trajectory[-1]
for j in range(old.njnt):d.qpos[m.joint(old.joint(j).name).qposadr[0]]=start[old.jnt_qposadr[j]]
cup=m.body('measured_cup').id;hid=m.body('left_hand_hand_base_link').id
idx=np.array([m.joint(f'L{i}_JOINT').qposadr[0] for i in range(1,8)]);bounds=m.jnt_range[[m.joint(f'L{i}_JOINT').id for i in range(1,8)]].T
mujoco.mj_forward(m,d);R=d.xmat[hid].reshape(3,3).copy();p=d.xpos[hid].copy();initial_z=float(d.xpos[cup,2]);arm0=d.qpos[idx].copy()
# Solve a 10cm vertical lift, retaining grasp orientation.
lifts=[];seed=arm0.copy()
for dz in np.linspace(0,.10,101):
target=p+[0,0,dz]
def fun(q):
d.qpos[idx]=q;mujoco.mj_forward(m,d)
return np.r_[d.xpos[hid]-target,.15*Rot.from_matrix(R.T@d.xmat[hid].reshape(3,3)).as_rotvec()]
fit=least_squares(fun,seed,bounds=bounds,max_nfev=100);err=fun(fit.x)
if np.linalg.norm(err[:3])>.002 or np.linalg.norm(err[3:])/.15>np.deg2rad(2):raise RuntimeError('lift IK failed')
seed=fit.x;lifts.append(seed.copy())
d.qpos[idx]=arm0;d.qvel[:]=0
handpre=d.qpos[m.jnt_qposadr[m.actuator_trnid[:,0]]].copy();closed=handpre.copy()
for a in range(m.nu):
name=m.actuator(a).name
if any(x in name for x in ['mcp_pitch','_pip']):closed[a]=min(m.actuator_ctrlrange[a,1],1.2 if 'pitch' in name else 1.5)
if 'thumb_cmc_yaw' in name:closed[a]=1.2
if 'thumb_cmc_roll' in name:closed[a]=.6
if 'thumb_cmc_pitch' in name:closed[a]=.6
if 'thumb_mcp_' in name:closed[a]=1.
if (out/'closed_qpos.npy').exists():
cq=np.load(out/'closed_qpos.npy')
for a in range(m.nu):
jname=m.joint(m.actuator_trnid[a,0]).name
closed[a]=cq[old.joint(jname).qposadr[0]]
if any(n in jname for n in ['mcp_pitch','_pip','thumb_mcp']):closed[a]=min(closed[a]+.08,m.actuator_ctrlrange[a,1])
arm_j=[j for j in range(m.njnt) if m.joint(j).name.startswith(('L','R')) and m.joint(j).name.endswith('_JOINT')]
arm_q=np.array([m.jnt_qposadr[j] for j in arm_j]);arm_v=np.array([m.jnt_dofadr[j] for j in arm_j]);fixed=d.qpos[arm_q].copy()
frames=[];stats=[];frozen={};lift_authorized=False;contact_fingers=set()
for step in range(4500):
t=step*.002
d.ctrl[:]=handpre+min(1,t/3.)*(closed-handpre)
contact_fingers=set()
for c in d.contact:
b1,b2=m.geom_bodyid[c.geom1],m.geom_bodyid[c.geom2]
other=b2 if b1==cup else b1 if b2==cup else -1
if other>=0:
name=m.body(other).name
for f in ['thumb','index','middle','ring','pinky']:
if name.startswith('left_hand_'+f+'_'):contact_fingers.add(f)
for a in range(m.nu):
name=m.actuator(a).name;finger=name.removeprefix('left_hand_').split('_')[0]
if finger in contact_fingers and a not in frozen and any(k in name for k in ['mcp_pitch','_pip','thumb_mcp']):frozen[a]=min(float(d.qpos[m.jnt_qposadr[m.actuator_trnid[a,0]]])+.08,float(m.actuator_ctrlrange[a,1]))
if a in frozen:d.ctrl[a]=frozen[a]
if 4.9<t<5 and 'thumb' in contact_fingers and len(contact_fingers)>=3:lift_authorized=True
d.qpos[arm_q]=fixed;d.qvel[arm_v]=0
d.qpos[idx]=lifts[int(np.clip((t-5)/2,0,1)*100)] if lift_authorized else arm0
mujoco.mj_step(m,d)
if step%10==0:
frames.append(d.qpos.copy());stats.append([t,float(d.xpos[cup,2]),int(d.ncon)])
np.save(out/'dynamic_qpos.npy',frames)
mujoco.mj_forward(m,d)
table_gap=min(mujoco.mj_geomDistance(m,d,g,m.geom('desk_top').id,1.,None) for g in range(m.ngeom) if m.geom_bodyid[g]==cup)
hand_contacts=sum(1 for c in d.contact if (m.geom_bodyid[c.geom1]==cup and m.body(m.geom_bodyid[c.geom2]).name.startswith('left_hand_')) or (m.geom_bodyid[c.geom2]==cup and m.body(m.geom_bodyid[c.geom1]).name.startswith('left_hand_')))
r={'lift_authorized_by_contacts':lift_authorized,'cup_table_gap_m':float(table_gap),'final_hand_contacts':hand_contacts,'grasp_success':bool(table_gap>.02 and hand_contacts>=2),'assumptions':{'cup_mass_kg':.25,'sliding_friction':1.,'arm_tracking':'ideal prescribed joints','hand':'force limited position actuators, kp=2, kd=0.08; original torque limits'},'initial_cup_bottom_z_m':initial_z,'final_cup_bottom_z_m':float(d.xpos[cup,2]),'lift_m':float(d.xpos[cup,2]-initial_z),'lifted_5cm':bool(d.xpos[cup,2]-initial_z>.05),'statistics':stats}
(out/'dynamic_report.json').write_text(json.dumps(r,indent=2));print(json.dumps({k:v for k,v in r.items() if k!='statistics'},indent=2))