67 lines
5.0 KiB
Python
67 lines
5.0 KiB
Python
"""Offline HUG pregrasp IK + sampled tabletop/cup checks; no robot connection."""
|
|
from pathlib import Path
|
|
import json
|
|
import numpy as np
|
|
import mujoco
|
|
from scipy.optimize import least_squares
|
|
from scipy.spatial.transform import Rotation
|
|
ROOT=Path(__file__).resolve().parents[1]
|
|
|
|
def main():
|
|
m=mujoco.MjModel.from_xml_path(str(ROOT/'scene/cup_workcell.xml'));d=mujoco.MjData(m)
|
|
mujoco.mj_resetDataKeyframe(m,d,m.key('snapshot').id);mujoco.mj_forward(m,d)
|
|
qinitial=d.qpos.copy();idx=np.array([m.joint(f'L{i}_JOINT').qposadr[0] for i in range(1,8)]);jid=[m.joint(f'L{i}_JOINT').id for i in range(1,8)];bounds=m.jnt_range[jid].T
|
|
escape=json.loads((ROOT/'data/cup_workcell/escape_report.json').read_text());start=np.array(escape['waypoints_rad'][-1]);base=qinitial.copy();base[idx]=start
|
|
handid=m.body('left_hand_hand_base_link').id;table=m.geom('desk_top').id;cup=m.body('measured_cup').id
|
|
armgeom=[g for g in range(m.ngeom) if m.geom_group[g]==0 and (m.body(m.geom_bodyid[g]).name.startswith(('L','left_hand_')) or m.body(m.geom_bodyid[g]).name=='left_adapter')]
|
|
cupgeom=[g for g in range(m.ngeom) if m.geom_bodyid[g]==cup]
|
|
rng=np.random.default_rng(12)
|
|
report={'hardware_connected':False,'executable_on_hardware':False,'table_margin_m':.005,'stages':{'sim_rgbd':'completed','cup_perception':'completed','hug':'8 candidates','retarget':str(len(list((ROOT/'data/cup_sim_capture_20260928').glob('grasp_l20_cup_*.json'))))+' left-hand candidates','start_escape':escape['status']},'candidates':[],
|
|
'scope':'sampled full left-arm/adapter/hand vs tabletop and cup; no continuous full-path proof/self-collision/dynamics or force closure'}
|
|
def gaps(q):
|
|
d.qpos[:]=q;mujoco.mj_forward(m,d)
|
|
tg=min(mujoco.mj_geomDistance(m,d,g,table,1.,None) for g in armgeom)
|
|
cg=min(mujoco.mj_geomDistance(m,d,g,c,1.,None) for g in armgeom for c in cupgeom)
|
|
return float(tg),float(cg)
|
|
for path in sorted((ROOT/'data/cup_sim_capture_20260928').glob('grasp_l20_cup_*.json')):
|
|
g=json.loads(path.read_text());row={'candidate':path.name};report['candidates'].append(row)
|
|
target=np.array(g['wrist_pos'])-.07*np.array(g['approach']);R=Rotation.from_quat(np.roll(g['wrist_quat_wxyz'],-1)).as_matrix()
|
|
def fun(q):
|
|
d.qpos[:]=base;d.qpos[idx]=q;mujoco.mj_forward(m,d)
|
|
return np.r_[d.xpos[handid]-target,.15*Rotation.from_matrix(R.T@d.xmat[handid].reshape(3,3)).as_rotvec()]
|
|
solutions=[]
|
|
for seed in [start,*[rng.uniform(bounds[0],bounds[1]) for _ in range(7)]]:
|
|
fit=least_squares(fun,np.clip(seed,bounds[0]+1e-8,bounds[1]-1e-8),bounds=bounds,max_nfev=180,ftol=1e-8,xtol=1e-8,gtol=1e-8)
|
|
err=fun(fit.x)
|
|
if np.linalg.norm(err[:3])<.002 and np.linalg.norm(err[3:])/.15<np.deg2rad(2):solutions.append(fit.x)
|
|
row['ik_solutions']=len(solutions)
|
|
if not solutions:row['status']='pregrasp_ik_not_found';continue
|
|
goal=min(solutions,key=lambda q:np.linalg.norm(q-start));qgoal=base.copy();qgoal[idx]=goal
|
|
pre=base.copy()
|
|
for name,value in g['hand_pre'].items():
|
|
name='left_hand_'+name
|
|
if mujoco.mj_name2id(m,mujoco.mjtObj.mjOBJ_JOINT,name)>=0:pre[m.joint(name).qposadr[0]]=value;qgoal[m.joint(name).qposadr[0]]=value
|
|
# Apply all original mimic equalities for hand joints in kinematic mode.
|
|
for q in [pre,qgoal]:
|
|
for e in range(m.neq):
|
|
if m.eq_type[e]==mujoco.mjtEq.mjEQ_JOINT:
|
|
j1,j2=m.eq_obj1id[e],m.eq_obj2id[e]
|
|
if j2>=0:q[m.jnt_qposadr[j1]]=np.polynomial.polynomial.polyval(q[m.jnt_qposadr[j2]],m.eq_data[e,:5])
|
|
phases=[('preshape',base,pre),('transfer',pre,qgoal)];samples=[];valid=True
|
|
for phase,a,b in phases:
|
|
count=max(2,int(np.ceil(np.max(abs(b-a))/.01))+1)
|
|
for frac in np.linspace(0,1,count):
|
|
q=(1-frac)*a+frac*b;tgap,cgap=gaps(q);samples.append({'phase':phase,'fraction':float(frac),'table_gap_m':tgap,'cup_gap_m':cgap})
|
|
if tgap<.005-1e-6 or cgap<.005-1e-6:valid=False
|
|
row.update({'status':'sampled_pregrasp_path_clear' if valid else 'sampled_pregrasp_path_rejected','min_table_gap_m':min(x['table_gap_m'] for x in samples),'min_cup_gap_m':min(x['cup_gap_m'] for x in samples),'goal_joints_rad':goal.tolist(),'target_hand_world_m':target.tolist(),'sample_count':len(samples)})
|
|
if valid:
|
|
report['selected_candidate']=path.name
|
|
way=[]
|
|
for a,b in zip([qinitial,base,pre],[base,pre,qgoal]):
|
|
for f in np.linspace(0,1,max(2,int(np.ceil(np.max(abs(b-a))/.01))+1)):way.append((1-f)*a+f*b)
|
|
np.save(ROOT/'data/cup_workcell/pregrasp_qpos.npy',way)
|
|
break
|
|
report['status']='pregrasp_sampled_path_found' if 'selected_candidate' in report else 'pregrasp_not_validated'
|
|
(ROOT/'data/cup_workcell/pipeline_report.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))
|
|
if __name__=='__main__':main()
|