File size: 2,907 Bytes
2b7cca5
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""D20 geometric visual localization and proprioceptive pose servo, offline only."""
from pathlib import Path
import importlib.util
import numpy as np
_ROOT=Path(__file__).resolve().parent
_s=importlib.util.spec_from_file_location('_geom',_ROOT/'geometry.py');g=importlib.util.module_from_spec(_s);_s.loader.exec_module(g)
with np.load(_ROOT/'geometry.npz') as z:B=z['pixel_to_fk'];P=z['poses'];A=z['actions']
_last_phase=None

def reset_episode():
 global state,age,objects,pick,idx,last_step,last_action,_last_phase,retries,end_age
 state=0;age=0;objects=None;pick=0;idx=275;retries=0;end_age=0;last_step=None;last_action=None;_last_phase=0
reset_episode()

def act(obs):
 global state,age,objects,pick,idx,last_step,last_action,_last_phase,retries,end_age
 step=int(obs['step_id'])
 if step>=700:return None
 if step==last_step:return last_action.copy()
 q=[np.asarray(obs['proprioception']['agent/panda-%d/qpos'%r]).reshape(-1) for r in range(2)]
 cur=[g.fk(x) for x in q]
 if objects is None:
  objects=sorted(g.detect(obs['rgb']['top_camera'],B),key=lambda o:o[0][0])
  if len(objects)<2:
   objects=[(np.array([.5,.1]),0.),(np.array([.7,-.1]),0.)]
  objects=objects[:2]
 a=np.zeros(14,dtype=np.float32);a[[6,13]]=1
 if state<9:
  xy,yaw=objects[pick];dest=.55 if pick==0 else .48
  place_yaw=yaw+((.785-yaw+np.pi/2)%np.pi-np.pi/2)
  if state==0 and age==0:
   seen=g.detect(obs['rgb']['top_camera'],B)
   if seen:
    nearest=min(seen,key=lambda o:np.linalg.norm(o[0]-xy))
    if np.linalg.norm(nearest[0]-xy)<.055:
     objects[pick]=nearest;xy,yaw=nearest
  grasp_z=.123-.006*retries
  if state==0:target=g.pose([*xy,.27],yaw)
  elif state in [1,2]:target=g.pose([*xy,grasp_z],yaw)
  elif state==3:target=g.pose([*xy,.27],yaw)
  elif state==4:target=g.pose([dest,0,.27],yaw)
  elif state==5:target=g.pose([dest,0,.27],place_yaw)
  elif state in [6,7]:target=g.pose([dest,0,.201],place_yaw)
  else:target=g.pose([dest,0,.36],place_yaw if pick==0 else -.785)
  u,dist,ang=g.servo(cur[1],target);a[7:13]=u
  a[13]=-1 if 2<=state<=6 else 1
  age+=1
  dwell=14 if state in [2,7] else 2
  reached=dist<.008 and ang<.065
  if (reached and age>=dwell) or age>=75:
   if state==2 and min(q[1][7:])<.012 and retries<1:
    retries+=1;state=0;age=0
   else:
    state+=1;age=0
   if state==9 and pick==0:pick=1;state=0;retries=0
  _last_phase=pick*9+state
 else:
  # Advance the final demonstrated bilateral maneuver using current pose feedback.
  if idx>=len(P)-1:
   end_age+=1
   if end_age>=35:return None
  target=P[min(idx+2,len(P)-1)]
  errors=[]
  for r in range(2):
   u,d,ang=g.servo(cur[r],target[r]);
   if r==0:u[[0,1,3,4]]*=-1
   a[r*7:r*7+6]=u;a[r*7+6]=A[min(idx,len(A)-1),r*7+6];errors.append((d,ang))
  age+=1
  if max(e[0] for e in errors)<.018 and max(e[1] for e in errors)<.18 or age>=8:
   idx=min(idx+2,len(P)-1);age=0
  _last_phase=100+idx
 last_step=step;last_action=a.copy();return a