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)