Download astra/BuildTent-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 11.2 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/BuildTent-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/BuildTent-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/BuildTent-v1/controller.py
11.2 kB
| import cv2,numpy as np | |
| def posts_hough(im): | |
| im=cv2.resize(np.asarray(im).squeeze(),(480,640),interpolation=cv2.INTER_LINEAR) | |
| hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV);g=cv2.cvtColor(im,cv2.COLOR_RGB2GRAY) | |
| cs=cv2.HoughCircles(g,cv2.HOUGH_GRADIENT,1,9,param1=70,param2=8,minRadius=3,maxRadius=7) | |
| out=[] | |
| if cs is None:return out | |
| for x,y,r in cs[0]: | |
| if not 170<x<430 or not 170<y<470:continue | |
| xx,yy=int(x),int(y) | |
| if np.median(hsv[yy-1:yy+2,xx-1:xx+2,1])>80 or np.median(hsv[yy-1:yy+2,xx-1:xx+2,2])<165:continue | |
| ang=np.arange(12)*2*np.pi/12;px=np.clip((x+np.cos(ang)*(r+3)).astype(int),0,479);py=np.clip((y+np.sin(ang)*(r+3)).astype(int),0,639) | |
| if np.sum((hsv[py,px,1]>75)&(hsv[py,px,2]<220))<3:continue | |
| out.append([float(x/2),float(y/2),float(np.pi*r*r/4)]) | |
| return out | |
| def update_posts(state, grid, im): | |
| groups=[[] for _ in range(4)] | |
| wrist=wrist_hough(im) | |
| for direct, points in [(True,posts(im)),(False,posts_hough(im))]: | |
| for px,py,area in points: | |
| if not 4<area<(25 if direct else 40):continue | |
| xy=np.array([px,py]) | |
| if np.isfinite(wrist).all() and np.linalg.norm(xy-wrist)<18:continue | |
| dist=np.linalg.norm(grid-xy,axis=1);j=int(np.argmin(dist)) | |
| if dist[j]>32 or np.sort(dist)[1]-dist[j]<5:continue | |
| groups[j].append((xy,direct)) | |
| for j,group in enumerate(groups): | |
| if not group:continue | |
| anchor=state.post_positions[j] if state.post_counts[j] else grid[j] | |
| preferred=[v for v in group if v[1]] or group | |
| xy,direct=min(preferred,key=lambda v:np.linalg.norm(v[0]-anchor)) | |
| if state.post_counts[j]==0 or (state.post_counts[j]<5 and np.linalg.norm(xy-anchor)>2): | |
| state.post_positions[j]=xy;state.post_counts[j]=1;state.direct_counts[j]=int(direct) | |
| elif np.linalg.norm(xy-anchor)<=2: | |
| state.post_positions[j]=.9*anchor+.1*xy;state.post_counts[j]+=1;state.direct_counts[j]+=int(direct) | |
| known=np.flatnonzero(state.post_counts>=5) | |
| best=None;best_score=-1 | |
| z=grid[:,0]+1j*grid[:,1] | |
| for ii,j in enumerate(known): | |
| for k in known[ii+1:]: | |
| p=state.post_positions | |
| c=complex(*(p[k]-p[j]))/(z[k]-z[j]) | |
| if not .92<abs(c)<1.08 or abs(np.angle(c))>.75:continue | |
| zz=c*(z-z[j])+complex(*p[j]);pred=np.stack([zz.real,zz.imag],axis=1) | |
| residual=np.linalg.norm(pred[known]-p[known],axis=1) | |
| score=float(np.sum(state.post_counts[known][residual<3])) | |
| if score>best_score:best=pred;best_score=score | |
| if best is not None: | |
| old_score=-1 if state.live_grid is None else np.sum(state.post_counts[known][np.linalg.norm(state.live_grid[known]-state.post_positions[known],axis=1)<3]) | |
| if best_score>=old_score:state.live_grid=best | |
| def target_post_delta(state, grid, target): | |
| if state.live_grid is not None: | |
| delta=state.live_grid[target]-grid[target] | |
| if np.linalg.norm(delta)<35:return delta,True | |
| return np.zeros(2),False | |
| import cv2,numpy as np | |
| def posts(im): | |
| im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA) | |
| hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV) | |
| mask=((hsv[:,:,1]<70)&(hsv[:,:,2]>170)).astype(np.uint8)*255 | |
| cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) | |
| out=[] | |
| for c in cs: | |
| a=cv2.contourArea(c);p=cv2.arcLength(c,True) | |
| if 2<a<55 and p>0 and 4*np.pi*a/p**2>.45: | |
| m=cv2.moments(c);x,y=m['m10']/m['m00'],m['m01']/m['m00'] | |
| if 85<x<215 and 85<y<235:out.append([x,y,a]) | |
| return out | |
| import numpy as np,cv2 | |
| def holes(im): | |
| im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA) | |
| hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV) | |
| white=((hsv[:,:,1]<75)&(hsv[:,:,2]>150)).astype(np.uint8)*255 | |
| cs,hh=cv2.findContours(white,cv2.RETR_CCOMP,cv2.CHAIN_APPROX_SIMPLE) | |
| out=[] | |
| if hh is None:return out | |
| for c,h in zip(cs,hh[0]): | |
| area=cv2.contourArea(c);per=cv2.arcLength(c,True);x,y,w,height=cv2.boundingRect(c) | |
| if h[3]>=0 and 20<area<180 and per>0 and 4*np.pi*area/per**2>.65 and x>70: | |
| m=cv2.moments(c);out.append([m['m10']/m['m00'],m['m01']/m['m00']]) | |
| return out | |
| def wrist(im): | |
| im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA) | |
| hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV) | |
| mask=((hsv[:,:,1]<28)&(hsv[:,:,2]>45)&(hsv[:,:,2]<180)).astype(np.uint8)*255 | |
| cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) | |
| out=[] | |
| for c in cs: | |
| area=cv2.contourArea(c);per=cv2.arcLength(c,True) | |
| if 80<area<900 and per>0 and 4*np.pi*area/per**2>.6: | |
| (x,y),r=cv2.minEnclosingCircle(c);out.append((x,y,r,4*np.pi*area/per**2)) | |
| return out | |
| def wrist_hough(im): | |
| im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA) | |
| g=cv2.cvtColor(im,cv2.COLOR_RGB2GRAY);hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV) | |
| cs=cv2.HoughCircles(g,cv2.HOUGH_GRADIENT,1,12,param1=80,param2=14,minRadius=6,maxRadius=13) | |
| if cs is None:return np.array([np.nan,np.nan]) | |
| valid=[] | |
| for x,y,r in cs[0]: | |
| xx,yy=int(x),int(y) | |
| if x>75 and 50<y<270 and np.median(hsv[max(0,yy-3):yy+4,max(0,xx-3):xx+4,1])<60 and 35<np.median(g[max(0,yy-3):yy+4,max(0,xx-3):xx+4])<200:valid.append((x,y)) | |
| return np.array(max(valid,key=lambda p:p[0]) if valid else [np.nan,np.nan]) | |
| """Authorized-demo trajectory alignment with current-pose correction; no learned weights.""" | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| def descriptor(im): | |
| im=np.asarray(im).squeeze() | |
| im=cv2.resize(im,(120,160),interpolation=cv2.INTER_AREA) | |
| hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV) | |
| mask=((hsv[:,:,1]<55)&(hsv[:,:,2]>135)).astype(np.float32) | |
| # Exclude the initial robot footprint from cloth localization. | |
| mask[:,:50]=0;mask[:45]=0;mask[120:]=0 | |
| return cv2.resize(mask,(60,80),interpolation=cv2.INTER_AREA) | |
| class Controller: | |
| def __init__(self): | |
| self.bank=None;self.reset_episode() | |
| def reset_episode(self): | |
| self.selected=None;self.t=1;self.calls=0;self.direct_counts=np.zeros(4,dtype=int);self.live_grid=None;self.last_target_delta=np.zeros(2);self.target_known=False;self.release_wait={};self.last_release_t=None;self.post_bank=None;self.post_positions=np.full((4,2),np.nan);self.post_counts=np.zeros(4,dtype=int);self.geometry=None;self.offset=np.zeros(2);self.approach_key=None;self.last_diagnostic={};self.previous=None | |
| def act(self,obs): | |
| self.calls+=1 | |
| step=int(obs.get('step_id',self.calls)) | |
| if step>=1000:return None | |
| q=np.asarray(obs['proprioception']['agent/qpos']).reshape(-1) | |
| if self.bank is None: | |
| with np.load(Path(__file__).with_name('sequences.npz')) as z:self.bank={k:z[k] for k in z.files} | |
| if step<25: | |
| self.t=step+1 | |
| return np.array([0,0,0,0,0,0,1.],dtype=np.float32) | |
| if self.post_bank is None: | |
| with np.load(Path(__file__).with_name('posts_inferred.npz')) as z:self.post_bank={k:z[k] for k in z.files} | |
| if self.geometry is None: | |
| with np.load(Path(__file__).with_name('geometry_clean.npz')) as z:self.geometry={k:z[k] for k in z.files} | |
| if self.selected is None: | |
| f=descriptor(obs['rgb']['top_camera']) | |
| scores=[np.mean((descriptor(self.bank[f'demo_{i:02d}_im'])-f)**2) for i in range(20)] | |
| self.selected=f'demo_{np.argmin(scores):02d}';self.t=25 | |
| self.initial_score=float(min(scores)) | |
| a=self.bank[self.selected+'_a'];qs=self.bank[self.selected+'_q'] | |
| if self.t>=len(a):return None | |
| # Re-align only locally by observed joint pose, with a one-frame timing prior. | |
| lo=max(25,self.t-1);hi=min(len(a),self.t+4) | |
| ix=np.arange(lo,hi) | |
| costs=np.mean((qs[ix,:7]-q[:7])**2,axis=1)+.0004*(ix-self.t)**2 | |
| t=min(self.t,int(ix[np.argmin(costs)])) | |
| transitions=np.flatnonzero(np.diff(a[:,6])!=0)+1 | |
| if np.any(np.abs(transitions-self.t)<=4):t=self.t | |
| command=a[t].copy() | |
| # A local finite-difference inverse maps joint tracking error to EE deltas. | |
| lo=max(0,t-25);hi=min(len(a),t+25) | |
| aa=a[lo:hi,:6].astype(float);dq=np.diff(qs[lo:hi+1,:7],axis=0) | |
| J=np.linalg.solve(aa.T@aa+.002*np.eye(6),aa.T@dq) | |
| err=qs[t,:7]-q[:7] | |
| correction=np.linalg.solve(J@J.T+.02*np.eye(6),J@err) | |
| command[:3]+=np.clip(correction[:3],-.005,.005) | |
| # Keep demonstrated orientation deltas; position errors must not induce wrist rotation. | |
| # Match the next grasp's reference eyelet to the current legal image. | |
| ws=self.geometry[self.selected+'_w'];hs=self.geometry[self.selected+'_h'] | |
| visual_used=False | |
| grid=self.post_bank[self.selected] | |
| update_posts(self,grid,obs['rgb']['top_camera']) | |
| target_delta=np.zeros(2) | |
| future=np.flatnonzero((a[:,6]<-.5)&(np.r_[1,a[:-1,6]]>=-.5)) | |
| closes=future[future>=t] | |
| if len(closes) and command[6]==0: | |
| close=int(closes[0]);ww=ws[max(25,close-5):close+1];ww=ww[np.isfinite(ww).all(axis=1)] | |
| ref=hs[t];ref=ref[np.isfinite(ref).all(axis=1)] | |
| live=np.array(holes(obs['rgb']['top_camera'])) | |
| if len(ww) and len(ref) and len(live): | |
| goal=ww[-1];target=ref[np.argmin(np.linalg.norm(ref-goal,axis=1))] | |
| match=live[np.argmin(np.linalg.norm(live-target,axis=1))] | |
| delta=match-target | |
| if np.linalg.norm(delta)<25 and np.linalg.norm(target-goal)<40: | |
| self.offset=.8*self.offset+.2*delta if self.approach_key==close else delta | |
| self.approach_key=close | |
| else: | |
| past=future[future<=t] | |
| dwell=(command[6]<-.5 and len(past)>0 and t-int(past[-1])<12) | |
| if command[6]<-.5 and len(past): | |
| self.target_known=False | |
| close=int(past[-1]);release=np.flatnonzero(a[close:,6]>=0) | |
| end=close+int(release[0]) if len(release) else len(a)-1 | |
| ww=ws[max(0,end-3):min(len(ws),end+2)];ww=ww[np.isfinite(ww).all(axis=1)] | |
| if len(ww): | |
| target=int(np.argmin(np.linalg.norm(grid-np.median(ww,axis=0),axis=1))) | |
| target_delta,self.target_known=target_post_delta(self,grid,target) | |
| self.last_target_delta=target_delta.copy() | |
| if not dwell:self.offset=.92*self.offset+.08*target_delta | |
| else:self.offset*=.85 | |
| is_release=t>0 and a[t,6]>.5 and a[t-1,6]<-.5 | |
| if is_release:self.last_release_t=t | |
| if self.target_known and self.last_release_t is not None and 0<=t-self.last_release_t<8: | |
| self.offset=self.last_target_delta.copy() | |
| refws=ws[max(0,t-2):min(len(ws),t+3)] | |
| validws=refws[np.isfinite(refws).all(axis=1)] | |
| refw=np.median(validws,axis=0) if len(validws) else np.array([np.nan,np.nan]) | |
| w=wrist_hough(obs['rgb']['top_camera']) | |
| if np.isfinite(w).all() and np.isfinite(refw).all(): | |
| error=refw+self.offset-w | |
| if np.linalg.norm(refw-w)<35: | |
| visual=np.clip(np.array([error[0],-error[1]])/75.,-0.06,0.06) | |
| command[:2]=a[t,:2]+visual | |
| visual_used=True | |
| pause=False | |
| if is_release and self.target_known and np.isfinite(w).all() and np.isfinite(refw).all(): | |
| error=refw+self.last_target_delta-w | |
| count=self.release_wait.get(t,0) | |
| if 3<np.linalg.norm(error)<35 and count<30 and (1000-step)>(len(a)-t): | |
| command[:6]=0.;command[6]=-1. | |
| command[:2]=np.clip(np.array([error[0],-error[1]])/75.,-0.06,0.06) | |
| self.release_wait[t]=count+1;pause=True | |
| if not pause:self.t+=1 | |
| self.last_diagnostic={'reason':'pose-aligned demonstration with bounded correction','demo':self.selected,'frame':t,'initial_mask_error':self.initial_score,'joint_error':float(np.linalg.norm(err)), 'visual_used':visual_used,'offset':self.offset.tolist(),'post_target_offset':target_delta.tolist(),'post_counts':self.post_counts.tolist(),'live_grid':None if self.live_grid is None else self.live_grid.tolist(),'release_pause':pause} | |
| return np.clip(command,-1,1).astype(np.float32) | |
| _controller=Controller() | |
| def reset_episode():_controller.reset_episode() | |
| def act(obs):return _controller.act(obs) | |