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
|