File size: 4,535 Bytes
044c27e
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""Task-local offline reference controller with image feedback and grip gates."""
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('_'+name,ROOT/(name+'.py'));m=importlib.util.module_from_spec(s);s.loader.exec_module(m);return m
features=module('features');geometry=module('geometry')
_memory=None
_targets=None
_wrists=None
_initial=None

def reset_episode():
 global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count
 _frame=1;_selected=None;_ids=None;_last_grip=np.ones(2,np.float32);_last_kind=np.zeros(2,np.int8);_wrist_previous=None;_gate_count=0

def load():
 global _memory,_targets,_wrists,_initial
 if _memory is None:
  with np.load(ROOT/'memory.npz',allow_pickle=False) as f:_memory={k:f[k] for k in f.files}
  with np.load(ROOT/'targets.npz',allow_pickle=False) as f:_targets={k:f[k] for k in f.files}
  _wrists=np.load(ROOT/'wrists.npz',allow_pickle=False)['wrist']
  _initial=np.load(ROOT/'initial_geometry.npz',allow_pickle=False)['pot']

def hold(q=None):
 a=np.zeros(14,np.float32);a[[6,13]]=_last_grip if q is None else np.clip(2*(q[[7,16]]+.01)/.05-1,-1,1);return a

def act(obs):
 global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count
 if 'observation' in obs:
  outer=obs;obs=dict(outer['observation']);obs['step_id']=outer.get('step_id',obs.get('step_id',1))
 if int(obs.get('step_id',1))>=1000:return None
 try:q,v=features.proprio(obs['proprioception'])
 except (KeyError,TypeError,ValueError):return hold()
 try:
  rgb=obs['rgb']
  if any(np.std(features.rgb_array(rgb[c]))<2 for c in features.CAMERAS):return hold(q)
  im=features.visual(rgb)
 except (KeyError,TypeError,ValueError):return hold(q)
 load();m=_memory;pot=geometry.pot_geometry(rgb['top_camera']);cubes=geometry.cube_positions(rgb['top_camera'])
 if _selected is None:
  ids=np.flatnonzero(m['frame']==0);delta=_initial-pot
  prior=200*delta[:,0]**2+350*delta[:,1]**2+.75*np.sum(delta[:,2:4]**2,axis=1)
  if not pot[4]:prior=np.zeros(len(_initial))
  score=35*np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1)+prior[m['demo'][ids]]
  _selected=int(m['demo'][ids[np.argmin(score)]]);_ids=np.flatnonzero(m['demo']==_selected)
 if _frame>int(m['frame'][_ids[-1]])+25:return None
 # Keep demonstrated order; observation matching refines only a narrow local window.
 ids=_ids[np.abs(m['frame'][_ids]-min(_frame,m['frame'][_ids[-1]]))<=2]
 scales=np.tile([.35]*7+[.025,.025],2)
 qd=np.mean(((m['q'][ids]-q)/scales)**2,axis=1)
 vd=np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1)
 score=qd+20*vd+.08*(m['frame'][ids]-_frame)**2
 best=int(ids[np.argmin(score)]);a=m['action'][best].copy();kind=_targets['kind'][best].copy()
 live=geometry.track_wrists(rgb['top_camera'],_wrist_previous)
 if _wrist_previous is None:_wrist_previous=np.array([[240,55],[240,582]],np.float32)
 correction=np.zeros((2,2),np.float32);gates=np.zeros(2,bool)
 for r in range(2):
  closing=_last_grip[r]>0 and a[6+7*r]<0
  opening=_last_grip[r]<0 and a[6+7*r]>0
  active_kind=int(kind[r])
  if (closing or opening) and active_kind==0:active_kind=int(_last_kind[r])
  if live[r,2]:
   _wrist_previous[r]=live[r,:2]
   if _wrists[best,r,2]:
    correction[r]=.0045*(_wrists[best,r,:2]-live[r,:2])*np.array([1,-1])
    correction[r]+=geometry.target_offset(active_kind,r,pot,cubes,_targets['pot'][best],_targets['cubes'][best])
    # Small horizontal correction before a contact-changing command.
    if closing and active_kind in (1,2,3) and np.linalg.norm(correction[r])>.036:gates[r]=True
  if opening and int(_last_kind[r])==4 and pot[4] and cubes[r,2]:
   delta=(pot[:2]-cubes[r,:2])*np.array([480,640])
   held=True
   if live[r,2] and _wrists[best,r,2] and _targets['cubes'][best,r,2]:
    rel=cubes[r,:2]*[480,640]-live[r,:2]
    refrel=_targets['cubes'][best,r,:2]*[480,640]-_wrists[best,r,:2]
    held=np.linalg.norm(rel-refrel)<35
   if held and np.linalg.norm(delta)>35:
    gates[r]=True;correction[r]=.0045*delta*np.array([1,-1])
  a[7*r:7*r+2]+=np.clip(correction[r],-.06,.06)
 if gates.any() and _gate_count<30:
  # Pause the reference clock and other motion; no repeated descent while aligning.
  a=hold();a[[6,13]]=_last_grip
  for r in range(2):
   if gates[r]:a[7*r:7*r+2]=np.clip(correction[r],-.06,.06)
  _gate_count+=1
 else:
  _gate_count=0;_frame+=1;_last_kind=kind
 a=np.clip(np.nan_to_num(a),-1,1).astype(np.float32);_last_grip=a[[6,13]].copy()
 return a

reset_episode()