"""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=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 15065)&(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.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