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 17080 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 432 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.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 20 and 4*np.pi*a/p**2>.45: m=cv2.moments(c);x,y=m['m10']/m['m00'],m['m01']/m['m00'] if 85150)).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 200 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 800 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 50135)).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(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)