28 lines
2.4 KiB
Python
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))
|