File size: 7,215 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
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
129
130
131
132
133
134
135
"""Explicit image-targeted pick/place with analytic Panda pose feedback."""
import numpy as np,cv2
from kinematics import fk
_state=None

def reset_episode():
 global _state
 _state={'phase':'settle','since':1,'arm':1,'cycle':0,'target':None,'misses':0,'bowl':None,'dest':{},'bowls':{},'failures':[],'last_z':None,'stalled':0,'yaw':[-np.pi/4,-np.pi/4]}

def locate(obs,arm):
 im=cv2.resize(np.asarray(obs['rgb']['top_camera']).squeeze(),(480,640))
 r,g,b=im.astype(float).transpose(2,0,1)
 yy,xx=np.indices(r.shape);cx,cy,rad=_state['bowl']
 inside=(xx-cx)**2+(yy-cy)**2<(rad+3)**2
 cutoff=np.where(inside,12.,65. if arm==0 else 60.)
 mask=((r>cutoff)&(r>2*g)&(r>1.8*b)) if arm==0 else ((b>cutoff)&(b>1.7*r)&(b>1.4*g))
 tx,ty,tr=_state['bowls'].get(arm,([400,240,65] if arm==0 else [80,410,65]))
 mask &= (xx>15)&(xx<465)&(yy>175)&(yy<490)&(((xx-tx)**2+(yy-ty)**2)>(tr+10)**2)
 # Preserve bright-component targeting; use shadow pixels only when needed.
 channel=r if arm==0 else b
 bright=mask&(channel>(65. if arm==0 else 60.))
 bn,bl,bs,_=cv2.connectedComponentsWithStats(bright.astype('uint8'))
 usable_bright=any(bs[k,4]>=35 and cv2.distanceTransform((bl==k).astype('uint8'),cv2.DIST_L2,5).max()>=2 for k in range(1,bn))
 if usable_bright:mask=bright
 n,labels,stats,cents=cv2.connectedComponentsWithStats(mask.astype('uint8'))
 opts=[];sign=1 if arm==0 else -1;scale=(3.34215545-.07)/1363.46820
 for k in range(1,n):
  if stats[k,4]<35:continue
  comp=(labels==k).astype('uint8');dist=cv2.distanceTransform(comp,cv2.DIST_L2,5)
  _,val,_,pt=cv2.minMaxLoc(dist)
  if val<2:continue
  u,v=pt;xy=np.array([.548941725+sign*(v-319.030943)*scale,sign*(u-238.761654)*scale])
  failures=sum(1 for f in _state['failures'] if f[0]==arm and np.linalg.norm(xy-np.asarray(f[1:]))<.035)
  outside=np.hypot(u-cx,v-cy)>rad+4
  opts.append(((100 if outside and failures<2 else 0)+val/(1+failures),pt,k,failures,xy,outside))
 if not opts:return None
 _,(u,v),k,failures,xy,outside=max(opts,key=lambda t:t[0])
 contours,_=cv2.findContours((labels==k).astype('uint8'),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
 rect=cv2.minAreaRect(max(contours,key=cv2.contourArea));w,h=rect[1]
 theta=((rect[2]+45)%90-45)*np.pi/180 if w*h>0 and stats[k,4]/(w*h)>.55 else 0.
 # Either pair of opposite cube faces is usable; favor bowl-wall clearance.
 near=[(cx,cy,rad)]+list(_state['bowls'].values())
 circle=min(near,key=lambda c:abs(np.hypot(u-c[0],v-c[1])-c[2]))
 rx,ry,radius=circle;distance=np.hypot(u-rx,v-ry)
 if distance>40 and distance<radius+35:
  alternative=(theta+np.pi)%np.pi-np.pi/2;radial=np.array([u-rx,v-ry])
  if abs(radial@np.array([np.cos(alternative),np.sin(alternative)]))<abs(radial@np.array([np.cos(theta),np.sin(theta)])):theta=alternative
 if failures%2:theta=(theta+np.pi)%np.pi-np.pi/2
 _state['yaw'][arm]=-np.pi/4-theta
 height=.060
 if outside:
  height=.025
  for bx,by,br in _state['bowls'].values():
   if np.hypot(u-bx,v-by)<br-12:height=.055
 height=max(.018,height-.006*min(failures,2))
 return np.r_[xy,height]

def act(obs):
 global _state
 if _state is None:reset_episode()
 step=int(obs['step_id']);s=_state
 if step>=2200:return None
 q=[np.asarray(obs['proprioception'][f'agent/panda-{r}/qpos']).reshape(-1) for r in (0,1)]
 poses=[fk(v) for v in q];pos=[t[:3,3] for t in poses]
 a=np.zeros(14,dtype=np.float32);a[6]=a[13]=1
 arm=s['arm'];off=arm*7;phase=s['phase'];age=step-s['since']
 def transition(p):
  s['phase']=p;s['since']=step;s['last_z']=None;s['stalled']=0
  print('phase',step,'arm',s['arm'],p,'cycle',s['cycle'],'fingers',float(q[arm][7]),flush=True)
 if phase=='settle':
  if s['bowl'] is None:
   im=cv2.resize(np.asarray(obs['rgb']['top_camera']).squeeze(),(480,640)); gray=cv2.cvtColor(im,cv2.COLOR_RGB2GRAY)
   circles=cv2.HoughCircles(gray,cv2.HOUGH_GRADIENT,1,100,param1=100,param2=30,minRadius=65,maxRadius=100)
   if circles is not None:
    possible=[c for c in circles[0] if 150<c[0]<330 and 230<c[1]<410]
    if possible:s['bowl']=min(possible,key=lambda c:(c[0]-240)**2+(c[1]-320)**2).tolist()
   if s['bowl'] is None:s['bowl']=[247,337,88]
   rr,gg,bb=im.astype(float).transpose(2,0,1)
   for color in (0,1):
    cm=((rr>65)&(rr>1.6*gg)&(rr>1.5*bb)) if color==0 else ((bb>60)&(bb>1.5*rr)&(bb>1.3*gg))
    contours,_=cv2.findContours(cm.astype('uint8'),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    if contours:
     contour=max(contours,key=cv2.contourArea);(u,v),rad=cv2.minEnclosingCircle(contour)
     if cv2.contourArea(contour)>2500 and ((color==0 and u>310 and v<330) or (color==1 and u<180 and v>330)):
      sign=1 if color==0 else -1;scale=(3.34215545-.03)/1363.46820
      s['bowls'][color]=[u,v,rad]
      s['dest'][color]=np.array([np.clip(.548941725+sign*(v-319.030943)*scale,.22,.46),np.clip(sign*(u-238.761654)*scale,.24,.5),.30])
  if age>=40:transition('select')
 elif phase=='select':
  target=locate(obs,arm)
  if target is None:
   s['arm']=1-arm;s['misses']+=1
   if s['misses']>4:transition('done')
  else:s['target']=target;s['misses']=0;transition('hover')
 elif phase=='done':
  for r in (0,1):
   err=np.array([.307,0,.487])-pos[r];sign=1 if r==0 else -1
   a[r*7:r*7+3]=np.clip(np.array([sign*err[1],-sign*err[0],err[2]])*2,-.12,.12)
  if age>100:return None
 else:
  target=s['target'].copy()
  if phase=='hover':target[2]=.32
  elif phase=='descend':pass
  elif phase=='close':a[off+6]=-1
  elif phase=='lift':target[2]=.34;a[off+6]=-1
  elif phase=='transfer':target=np.asarray(s['dest'].get(arm,[.34,.37,.30]));a[off+6]=-1
  elif phase=='release':target=np.asarray(s['dest'].get(arm,[.34,.37,.30]))
  elif phase=='return':target=np.array([.307,0,.487])
  err=target-pos[arm];sign=1 if arm==0 else -1
  command=np.array([sign*err[1],-sign*err[0],err[2]])
  a[off:off+3]=np.clip(command*4,-.25,.25)
  phi=s['yaw'][arm];c,sn=np.cos(phi),np.sin(phi)
  desired=np.array([[c,sn,0],[sn,-c,0],[0,0,-1.]])
  omega=cv2.Rodrigues(desired@poses[arm][:3,:3].T)[0].ravel()
  rotation=np.array([sign*omega[1],-sign*omega[0],omega[2]])
  a[off+3:off+6]=np.clip(-rotation*np.array([2.,2.,.8]),-.25,.25)
  reached=np.linalg.norm(err)<.009
  if phase=='descend':
   s['stalled']=s['stalled']+1 if s['last_z'] is not None and abs(pos[arm][2]-s['last_z'])<.0015 else 0
   s['last_z']=float(pos[arm][2])
  contact=(phase=='descend' and age>=24 and s['stalled']>=3 and .012<q[arm][7]<.038 and np.linalg.norm(err[:2])<.035 and pos[arm][2]<.155 and -poses[arm][2,2]>.97)
  if phase=='hover' and (reached or age>120):transition('descend')
  elif phase=='descend' and (contact or (np.linalg.norm(err[:2])<.015 and abs(err[2])<.008)):transition('close')
  elif phase=='descend' and age>64:
   if np.linalg.norm(err)<.045:transition('close')
   else:s['failures'].append([arm,*s['target'][:2]]);transition('return')
  elif phase=='close' and age>=16:transition('lift')
  elif phase=='lift' and (reached or age>120):
   if q[arm][7]>.008:transition('transfer')
   else:s['failures'].append([arm,*s['target'][:2]]);transition('return')
  elif phase=='transfer' and (reached or age>120):transition('release')
  elif phase=='release' and age>=16:transition('return')
  elif phase=='return' and (reached or age>120):s['arm']=1-arm;s['cycle']+=1;transition('select')
 return a