Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
7.22 kB
"""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