"""D20 linear trajectory interpolation with measured-joint pose feedback. Only fixed geometry, color localization and linear algebra; no neural model. """ from pathlib import Path import importlib.util import numpy as np _ROOT=Path(__file__).resolve().parent def module(name): s=importlib.util.spec_from_file_location('_task_'+name,_ROOT/(name+'.py'));m=importlib.util.module_from_spec(s);s.loader.exec_module(m);return m _loc=module('localization');_kin=module('kinematics') _db=None;_layout=None;_t=0;_last=None;_diagnostics={} GAIN=3.0 PUSH_EXTRA=12 PRESS_DEPTH=0.0 HOLD_TUBE=False def reset_episode(): global _layout,_t,_last,_diagnostics _layout=None;_t=0;_last=None;_diagnostics={} def act(obs): global _db,_layout,_t,_last,_diagnostics step=int(obs['step_id']) if step>=400:return None if _last is not None and step<=_last:reset_episode() _last=step if _db is None: with np.load(_ROOT/'trajectory.npz') as z:_db={k:z[k] for k in z.files} if _layout is None: f=_loc.locate(obs['rgb'])[4:10] _layout=np.r_[1,np.clip((f-_db['mean'])/.02,-3,3)] t=_t if t>=351+PUSH_EXTRA:return None ref=min(t,332) if t<333+PUSH_EXTRA else t-PUSH_EXTRA a=(_layout@_db['action'][ref]).copy() target=(_layout@_db['position'][ref]).reshape(2,3) yawtarget=_layout@_db['yaw'][ref] if 333<=t<333+PUSH_EXTRA: a[:6]=0;a[7:13]=0;a[8]=.04 target[1,0]+=(t-332)*.0022 elif t>=333+PUSH_EXTRA: target[1,0]+=PUSH_EXTRA*.0022 if HOLD_TUBE and ref>=207: a[:6]=0;a[6]=-1 target[0]=(_layout@_db["position"][206]).reshape(2,3)[0] yawtarget[0]=(_layout@_db["yaw"][206])[0] for r in range(2): depth=PRESS_DEPTH*max(0,min(1,(ref-20)/5,(65-ref)/7)) if r==1:depth=max(depth,PRESS_DEPTH*max(0,min(1,(ref-95)/5,(139-ref)/7))) target[r,2]-=depth errors=[] for r in range(2): q=np.asarray(obs['proprioception'][f'agent/panda-{r}/qpos']).reshape(-1) T=_kin.pose(q);err=target[r]-T[:3,3];errors.append(float(np.linalg.norm(err))) correction=np.array([err[1],-err[0],err[2]]) if r==0 else np.array([-err[1],err[0],err[2]]) a[r*7:r*7+3]+=np.clip(GAIN*correction,-.06,.06) R=T[:3,:3]@_db['initial_rotation'][r].T;yaw=np.arctan2(R[1,0],R[0,0]);err_yaw=(yawtarget[r]-yaw+np.pi)%(2*np.pi)-np.pi a[r*7+5]+=np.clip(-.6*err_yaw,-.04,.04) a[[6,13]]=np.where(a[[6,13]]>0,1.,-1.) _t+=1;_diagnostics={'reference_frame':ref,'phase_estimate':t,'pose_errors_m':errors,'reason':'pose_feedback'} return np.clip(a,-1,1).astype(np.float32)