Download astra/DualArmBallClassify-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 7.22 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmBallClassify-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmBallClassify-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmBallClassify-v1/controller.py
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 | |