File size: 2,439 Bytes
20033c3 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 | """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)
|