File size: 3,106 Bytes
e25638c
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""Geometric feedback control compiled from this task's D20 demonstrations."""
from pathlib import Path
import importlib.util
import numpy as np
import cv2
_spec=importlib.util.spec_from_file_location('_kin',Path(__file__).with_name('kinematics.py'));_k=importlib.util.module_from_spec(_spec);_spec.loader.exec_module(_k)
_phase=0;_ticks=0;_calls=0;_progress=0;_grasp=None;_place=None
SCALE=396.5
def reset_episode():
 global _phase,_ticks,_calls,_progress,_grasp,_place
 _phase=0;_ticks=0;_calls=0;_progress=0;_grasp=None;_place=None
def locate(rgb):
 a=cv2.resize(np.asarray(rgb).squeeze(),(480,640)).astype(float)
 cyan=((a[:,:,1]-a[:,:,0]>25)&(a[:,:,2]-a[:,:,0]>25)).astype('uint8');cyan[:210]=0;cyan[440:]=0
 n,l,stats,cent=cv2.connectedComponentsWithStats(cyan)
 i=1+np.argmax(stats[1:,4]);ys,xs=np.where(l==i);boundary=np.array([(xs.min()+xs.max())/2,(ys.min()+ys.max())/2])
 white=((a.min(2)>180)&(a.max(2)-a.min(2)<50)).astype('uint8');white[:210]=0;white[440:]=0
 n,l,stats,cent=cv2.connectedComponentsWithStats(white);i=1+np.argmax(stats[1:,4]);ys,xs=np.where(l==i)
 rect=cv2.minAreaRect(np.column_stack([xs,ys]).astype('float32'));center=np.array(rect[0]);angle=rect[2]
 if angle>45:angle-=90
 return center,boundary,np.deg2rad(angle)
def offset(pos,delta):
 p=pos.copy();p[0,0]+=delta[1]/SCALE;p[1,0]-=delta[1]/SCALE;p[0,1]+=delta[0]/SCALE;p[1,1]-=delta[0]/SCALE
 return p
def act(obs):
 global _phase,_ticks,_calls,_progress,_grasp,_place
 if int(obs.get('step_id',_calls+1))>=200:return None
 _calls+=1
 try:
  cur=np.array([_k.fk(np.asarray(obs['proprioception'][f'agent/panda-{i}/qpos']).reshape(-1)) for i in range(2)])
  if _grasp is None:
   center,boundary,theta=locate(obs['rgb']['top_camera'])
   with np.load(Path(__file__).with_name('pose_bank.npz')) as b:
    gs=np.array([offset(p,center-c) for p,c in zip(b['place'],b['boundary'])]);ps=np.array([offset(p,boundary-c) for p,c in zip(b['place'],b['boundary'])])
   _grasp=np.median(gs,axis=0);_place=np.median(ps,axis=0)
   for i,dy in enumerate([-.08,.08]):
    delta=np.array([-np.sin(theta)*dy,(np.cos(theta)-1)*dy])*SCALE
    tmp=offset(_grasp,delta);_grasp[i]=tmp[i]
   _grasp[:,2]=.124;_place[:,2]=.124
 except (KeyError,ValueError,IndexError):return None
 _ticks+=1;_progress=float(_phase*20+_ticks)
 target=_grasp.copy();grip=1.;limit=.15
 if _phase==0:target[:,2]=.24
 elif _phase==1:pass
 elif _phase==2:grip=-1
 elif _phase==3:target[:,2]=.20;grip=-1;limit=.18
 elif _phase==4:target=_place.copy();target[:,2]=.20;grip=-1;limit=.18
 elif _phase==5:target=_place.copy();grip=-1;limit=.095
 elif _phase==6:target=_place.copy();limit=.095
 elif _phase==7:target=_place.copy();target[:,2]=.30;limit=.12
 else:return None
 err=target-cur
 a=np.zeros(14,dtype=np.float32)
 for i in range(2):
  e=err[i];d=np.array([e[1],-e[0],e[2]]) if i==0 else np.array([-e[1],e[0],e[2]])
  a[i*7:i*7+3]=np.clip(d*5,-limit,limit);a[i*7+6]=grip
 # Pose convergence, with bounded dwell and phase timeout.
 done=np.max(np.abs(err))<.006
 if _phase in (2,6):done=_ticks>=10
 if (done and _ticks>=3) or _ticks>65:
  _phase+=1;_ticks=0
 return a