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

28 lines
2.4 KiB
Python

"""Validate final Cartesian approach offline; no dynamics or hardware."""
import json,os
from pathlib import Path
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')
m=mujoco.MjModel.from_xml_path(str(ROOT/'scene/cup_workcell.xml'));d=mujoco.MjData(m)
p=json.loads((out/'selected.json').read_text());way=list(np.load(out/'pregrasp_qpos.npy'));q=way[-1].copy()
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
hid=m.body('left_hand_hand_base_link').id;R=np.array(p['rotation_matrix']);table=m.geom('desk_top').id;cup=m.body('measured_cup').id
ag=[i for i in range(m.ngeom) if m.geom_group[i]==0 and (m.body(m.geom_bodyid[i]).name.startswith(('L','left_hand_')) or m.body(m.geom_bodyid[i]).name=='left_adapter')];cg=[i for i in range(m.ngeom) if m.geom_bodyid[i]==cup]
rows=[]
for f in np.linspace(0,1,71)[1:]:
target=(1-f)*np.array(p['hand_target_m'])+f*np.array(p['grasp_hand_target_m'])
def fun(x):
d.qpos[:]=q;d.qpos[idx]=x;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,q[idx],bounds=bounds,max_nfev=100);err=fun(fit.x);candidate=q.copy();candidate[idx]=fit.x
tg=min(mujoco.mj_geomDistance(m,d,a,table,1.,None) for a in ag);dist=[(mujoco.mj_geomDistance(m,d,a,b,1.,None),m.geom(a).name) for a in ag for b in cg];gap,name=min(dist)
row={'fraction':float(f),'table_gap_m':float(tg),'cup_gap_m':float(gap),'nearest_robot_geom':name,'position_error_m':float(np.linalg.norm(err[:3])),'angle_error_deg':float(np.rad2deg(np.linalg.norm(err[3:])/.15))};rows.append(row)
if tg<.005 or gap<0 or row['position_error_m']>.002 or row['angle_error_deg']>2:break
q=candidate;way.append(q.copy())
np.save(out/'approach_qpos.npy',way)
report={'status':'approach_complete' if len(rows)==70 and rows[-1]['cup_gap_m']>=0 and rows[-1]['table_gap_m']>=.005 and rows[-1]['position_error_m']<=.002 and rows[-1]['angle_error_deg']<=2 else 'approach_stopped','samples':rows,'scope':'kinematic approach; contact with cup allowed at end, no penetration; 5mm tabletop margin'}
(out/'approach_report.json').write_text(json.dumps(report,indent=2));print(report['status']);print(json.dumps(rows[-1],indent=2))