File size: 6,862 Bytes
8e97cab
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
"""Task-local demo-reference tracking with visible selection and bounded feedback."""
from pathlib import Path
import json,numpy as np,cv2
from visual_features import marker_features,marker_distance
ROOT=Path(__file__).parent
CONFIG=json.loads((ROOT/'settings.json').read_text())
ROBOTS=('panda_leaphand-1','panda_leaphand_left-0')
_STATE={};_BANK=None

def _load():
 global _BANK
 if _BANK is None:
  with np.load(ROOT/'reference_bank.npz') as f:b={k:f[k] for k in f.files}
  with np.load(ROOT/'numeric_demos.npz') as f:b['numeric']={k:f[k] for k in f.files}
  b['hand_map']=np.load(ROOT/'hand_map.npy');b['hand_sign']=np.load(ROOT/'hand_sign.npy')
  b['table_features']=np.load(ROOT/'table_features.npy')
  b['markers']=np.load(ROOT/'marker_bank.npy');b['feedback']=np.load(ROOT/'pose_feedback.npy')
  with np.load(ROOT/'geometry_feedback.npz') as f:b['geometry']={k:f[k] for k in f.files}
  b['indices']=[np.flatnonzero(b['demo_index']==i) for i in range(20)]
  b['flip']=[int(np.flatnonzero((np.linalg.norm(b['numeric']['a'+str(i)][:,3:6],axis=1)>.25)&(np.arange(len(b['numeric']['a'+str(i)]))>len(b['numeric']['a'+str(i)])*.65))[0]) for i in range(20)]
  b['gate']=[int(np.flatnonzero(b['numeric']['a'+str(i)][:400,23]<-.04)[-1]) for i in range(20)]
  _BANK=b
 return _BANK

def reset_episode():
 _STATE.clear();_STATE.update(last_step=0,t=-1.,was_recovering=False,demo=None,gate_done=False,recovery=0,bad=0,reason='reset',phase=0.)

def image_features(rgb):
 return np.concatenate([cv2.resize(np.asarray(rgb[c]).squeeze(),(16,12),interpolation=cv2.INTER_AREA).reshape(-1).astype(np.float32)/255 for c in ['front_camera','left_camera']])

def scene_features(m):
 out=[]
 for st in [8,20]:
  p=m[st:st+4].reshape(2,2)
  if np.any(p<0):return None
  p=p[np.argsort(p[:,1 if st==20 else 0])];out.extend(p.mean(0));out.extend(p[1]-p[0])
 return np.array(out)

def observe_scene(m,reference,previous):
 result=np.asarray(previous if previous is not None else reference,np.float32).copy();visible=0
 for cam,st in enumerate([8,20]):
  p=m[st:st+4].reshape(2,2);p=p[p[:,0]>=0];visible+=len(p);base=result[cam*4:cam*4+4]
  if len(p)==2:
   p=p[np.argsort(p[:,cam])];result[cam*4:cam*4+4]=np.r_[p.mean(0),p[1]-p[0]]
  elif len(p)==1:
   endpoints=np.stack([base[:2]-.5*base[2:],base[:2]+.5*base[2:]])
   side=int(np.argmin(np.sum((endpoints-p[0])**2,axis=1)))
   result[cam*4:cam*4+2]=p[0]+(.5 if side==0 else -.5)*base[2:]
 return result,visible

def leg_error(m):
 red=m[:8].reshape(4,2);blue=m[8:12].reshape(2,2)
 if np.any(red[:,0]<0) or np.any(blue[:,0]<0):return None
 upper=red[np.argsort(red[:,1])[:2]]
 return float(np.min(upper[:,0])-np.min(blue[:,0]))

def act(obs):
 if not _STATE:reset_episode()
 step=int(obs['step_id']);elapsed=step-_STATE['last_step']
 if step>=min(2400,CONFIG.get('stop_step',2400)):return None
 if elapsed<=0:raise ValueError('Non-increasing step')
 _STATE['last_step']=step;b=_load();p=obs['proprioception']
 q=np.concatenate([np.asarray(p['agent/'+r+'/qpos']).reshape(-1) for r in ROBOTS])
 if q.shape!=(46,) or not np.isfinite(q).all():return None
 im=image_features(obs['rgb']);marks=marker_features(obs['rgb'])
 if _STATE['demo'] is None or step<=9:
  ids=np.array([x[min(int((step-1)//8),len(x)-1)] for x in b['indices']])
  ds=marker_distance(b['markers'][ids],marks)*CONFIG.get('marker_weight',.1)+np.mean(((b['image'][ids]-im)/.15)**2,1)
  _STATE['demo']=int(np.argmin(ds));_STATE['selection_distance']=float(ds.min())
 did=_STATE['demo'];a=b['numeric']['a'+str(did)];qref=b['numeric']['q'+str(did)];n=len(a)
 if not _STATE['was_recovering']:_STATE['t']+=elapsed*CONFIG.get('speed',1.)
 t=int(round(_STATE['t']));gate=b['gate'][did];err=leg_error(marks);_STATE['leg_error']=err
 recovering=False
 if CONFIG.get('leg_gate',False) and not _STATE['gate_done'] and t>=gate:
  if err is not None and err<=CONFIG.get('leg_threshold',.025):_STATE['gate_done']=True
  elif _STATE['recovery']>=CONFIG.get('recovery_limit',80):
   _STATE['reason']='first-leg recovery exhausted; failed attempt';return None
  else:
   recovering=True;_STATE['recovery']+=elapsed;t=gate-20
 if t>=n:
  _STATE['reason']='reference endpoint reached; not a success assertion';return None
 j=b['indices'][did][min(t//8,len(b['indices'][did])-1)]
 action=a[t].copy();error=qref[t]-q
 flipping=t>=b['flip'][did]-4
 _STATE['flipping']=bool(flipping)
 _STATE['geometry_offset_norm']=0.
 geometry_joint_offset=np.zeros(14,np.float32)
 if CONFIG.get('geometry_gain',0.) and t>n*.02 and not flipping:
  reference=b['table_features'][j]
  current,visible=observe_scene(marks,reference,_STATE.get('table_geometry'))
  _STATE['table_geometry']=current.tolist();_STATE['geometry_visible_points']=int(visible)
  previous=np.asarray(_STATE.get('geometry_offset',[0.]*14),np.float32)
  offset=previous
  if current is not None and reference is not None:
   g=b['geometry'];k=int(np.argmin(abs(g['grid']-g['aligned_phase'][j])))
   if g['valid'][k]>=8:
    target=CONFIG['geometry_gain']*((current-reference)@g['jac'][k])
    target=np.clip(target,-CONFIG.get('geometry_limit',.1),CONFIG.get('geometry_limit',.1))
    alpha=1.-.9**elapsed;offset=previous+alpha*(target-previous)
  _STATE['geometry_offset']=offset.tolist()
  geometry_joint_offset=offset
  _STATE['geometry_offset_norm']=float(np.linalg.norm(offset))
 for r in range(2):
  qs=slice(r*23,r*23+7);aa=slice(r*22,r*22+6)
  gain=0. if (r==0 and flipping and CONFIG.get('disable_flip_feedback',True)) else CONFIG.get('pose_gain',.1)
  correction=gain*(error[qs]@b['feedback'][j,r])
  correction[3:]=0.  # Preserve demonstrated wrist orientation commands.
  correction[:3]+=gain*(geometry_joint_offset[r*7:r*7+7]@b['feedback'][j,r])[:3]
  action[aa]+=np.clip(correction,-CONFIG.get('pose_limit',.02),CONFIG.get('pose_limit',.02))
  hs=slice(r*23+7,r*23+23);ha=slice(r*22+6,r*22+22)
  action[ha]+=np.clip(CONFIG.get('hand_gain',.1)*error[hs][b['hand_map'][r]]*b['hand_sign'][r]/.1,-CONFIG.get('hand_limit',.05),CONFIG.get('hand_limit',.05))
 if recovering:
  action[:22]=0
  # Continue the demonstrated pull, without pulling back toward an earlier pose.
  action[22:25]=a[max(gate-30,0),22:25]
 if recovering:_STATE['t']=float(gate)
 _STATE['was_recovering']=recovering
 _STATE['phase']=float(_STATE['t']/n);_STATE['frame']=t
 _STATE['arm_distance']=float(np.mean((error[:7]/b['q_scale'][:7])**2))
 support_error=error.copy()
 if flipping and CONFIG.get('disable_flip_feedback',True):support_error[:7]=0
 _STATE['distance']=float(np.mean((support_error/b['q_scale'])**2))
 _STATE['bad']=_STATE['bad']+elapsed if _STATE['distance']>CONFIG.get('pose_abort',3.) else 0
 if _STATE['bad']>40:
  _STATE['reason']='sustained pose deviation; failed attempt';return None
 _STATE['reason']='visible leg recovery' if recovering else 'reference tracking with joint feedback'
 return np.clip(action,-1,1).astype(np.float32)