"""Non-neural demonstration path tracking with legal RGB layout selection.""" from pathlib import Path import numpy as np import numpy as np OFFSETS=[(0,0,.333),(0,0,0),(0,-.316,0),(.0825,0,0),(-.0825,.384,0),(0,0,0),(.088,0,0)] RX=[0,-np.pi/2,np.pi/2,np.pi/2,-np.pi/2,np.pi/2,np.pi/2] def fk(q): q=np.asarray(q);q=q.reshape(-1,q.shape[-1]);T=np.tile(np.eye(4),(len(q),1,1)) for j,(off,rx) in enumerate(zip(OFFSETS,RX)): c,s=np.cos(rx),np.sin(rx);F=np.eye(4);F[:3,3]=off;F[:3,:3]=[[1,0,0],[0,c,-s],[0,s,c]] R=np.tile(np.eye(4),(len(q),1,1));c,s=np.cos(q[:,j]),np.sin(q[:,j]);R[:,0,0]=c;R[:,0,1]=-s;R[:,1,0]=s;R[:,1,1]=c T=T@F@R return T[:,:3,3]+.107*T[:,:3,2] import cv2,numpy as np import numpy as np,cv2 def locate(rgb): im=cv2.resize(np.asarray(rgb),(240,320)).astype(np.float32);r,g,b=im.transpose(2,0,1) masks={'green':(g>r*1.16)&(g>b*1.12)&(g>65),'yellow':(np.minimum(r,g)>160)&(b160)&(r160)&(g20);good=good[good!=0] if len(good): biggest=good[np.argmax(stats[good,4])];dist=cents[good]-cents[biggest] keep=(np.linalg.norm(dist,axis=1)<100) if name=='green' else ((np.abs(dist[:,0])<50)&(np.abs(dist[:,1])<22)) good=good[keep] mask=np.isin(labels,good) pts=np.column_stack(np.nonzero(mask))[:,::-1].astype(np.float32) if len(pts): rect=cv2.minAreaRect(pts);out[name]={'center':list(rect[0]),'size':list(rect[1]),'angle':rect[2],'box':cv2.boxPoints(rect).tolist(),'pixels':len(pts)} return out def orientation(loc): vals=[] for c in ['green','yellow','cyan','pink']: d=loc[c];angle=np.deg2rad(d['angle']+(90 if d['size'][0]=350 and min(live['size'])>=20: error=(self.target_pixels[state['cloth']]-np.asarray(live['center']))@self.data['pixel_map'] action[:2]+=5*error action[:2]=np.clip(action[:2],-.09,.09) offset=pos[:2]-state['origin'][:2] for axis,(lower,upper) in enumerate([(-.06,.06),(-.04,.16)]): if (offset[axis]<=lower and action[axis]<0) or (offset[axis]>=upper and action[axis]>0):action[axis]=0 if state['steps']>=80: extra=np.clip(pos[:2]-state['origin'][:2],[-.08,-.06],[.08,.18]);resume=state['resume'];j=state['cloth'] shift=np.zeros((len(self.p),2)) if j<2: pickup=int(self.groups[j+1][0])-60 shift[resume:pickup]=np.linspace(extra,0,pickup-resume) else:shift[resume:]=extra self.p[:,:2]+=shift;self.a[:,:2]+=np.diff(shift,axis=0)/.04 state['stage']=1;state['steps']=0;state['goal']=pos.copy();state['goal'][2]=self.p[resume,2] else: action[:3]=np.clip(5*(state['goal']-pos),-.15,.15) if np.linalg.norm(state['goal']-pos)<.008 or state['steps']>=40: self.phase=float(state['resume']);self.release_state=None return action def act(self,obs): self.tick+=1 if int(obs.get('step_id',self.tick))>=1500:return None q=np.asarray(obs['proprioception']['agent/qpos']).reshape(-1) if self.demo is None: loc=locate(np.asarray(obs['rgb']['top_camera']).squeeze()) layout=np.array([v for c in ['green','yellow','cyan','pink'] for v in loc[c]['center']]) self.demo=int(np.argmin(np.mean((self.data['layouts']-layout)**2,axis=1)+12.5*np.sum((self.data['orientations']-orientation(loc))**2,axis=1))) self.a=self.data[f'a{self.demo}'];self.q=self.data[f'q{self.demo}'];self.p=self.data[f'p{self.demo}'].copy() self.a=self.a.copy() delta=(layout-self.data['layouts'][self.demo]).reshape(4,2)@self.data['pixel_map'] pattern_delta=np.clip((strips(loc)-self.data['strips'][self.demo])@self.data['pixel_map'],-.06,.06) delta=np.clip(delta,-.045,.045) delta[1:,1]+=.03 hit=np.flatnonzero(self.a[:,1]>.02) groups=[g for g in np.split(hit,np.flatnonzero(np.diff(hit)>10)+1) if len(g)>30][:3] self.groups=groups;self.target_pixels=strips(loc) knots=[0]; offsets=[np.zeros(2)] for j,g in enumerate(groups): start,end=int(g[0]),int(g[-1]+1) knots.extend([start-60,start,end,end+70]);offsets.extend([delta[j+1],delta[j+1],pattern_delta[j],pattern_delta[j]]) knots.append(len(self.p)-1);offsets.append(pattern_delta[-1]) shift=np.stack([np.interp(np.arange(len(self.p)),knots,np.array(offsets)[:,axis]) for axis in range(2)],axis=1) self.p[:,:2]+=shift self.a[:,:2]+=np.diff(shift,axis=0)/.04 i=min(int(self.phase),len(self.a)-1) if self.phase>=len(self.a)+40:return None if self.phase>=len(self.a): self.phase+=1;return np.zeros(22,dtype=np.float32) pos=fk(q)[0] if self.release_state is not None:return self.release_action(q,pos,obs) order=[4,0,8,12,5,1,9,13,6,2,10,14,3,7,11,15] for j,g in enumerate(self.groups): end=int(g[-1]+1) if not self.corrected[j] and end-1<=i=650 and min(live['size'])>=20 and abs(dx)<30 ax=float(np.clip(-.024*dx,-.06,.06)) if lateral and abs(dx)>3 else 0. if abs(pos[0]-self.p[i,0])>=.06 and ax*(pos[0]-self.p[i,0])>0:ax=0. if live is not None and live['pixels']>=100 and -203 or ax!=0) and self.correction_steps[j]<64: action=np.zeros(22,dtype=np.float32) action[0]=ax action[1]=np.clip(.025*dy,0,.09) if dy>3 else 0. action[2]=np.clip(5*(self.p[i,2]-pos[2]),-.09,.09) target=self.grasp_pose[j] if self.grasp_pose[j] is not None else self.data[f'hand_target{self.demo}'][i] action[6:]=np.clip(.5*(target-q[7:][order]),-1,1) self.correction_steps[j]+=1 return action self.corrected[j]=True extra=np.clip(pos[:2]-self.p[i,:2],[-.07,0],[.07,.24]) shift=np.zeros((len(self.p),2)) if j<2: release=end+70;pickup=int(self.groups[j+1][0])-60 shift[i:release]=extra shift[release:pickup]=np.linspace(extra,0,pickup-release) else:shift[i:]=extra self.p[:,:2]+=shift;self.a[:,:2]+=np.diff(shift,axis=0)/.04 if self.corrected[j] and not self.released[j] and end-1<=i.015:speed=1.8 if i<75:speed=1.5 if any(int(g[0])-65<=i<=int(g[-1])+35 for g in self.groups):speed=1.0 a=self.a[i].copy()*speed a[:3]=np.clip(a[:3]+5.*err,-.22,.22) gain=float(self.data[f'hand_gain{self.demo}'][i]) if gain>.4:gain=1.0 a[6:]=np.clip(gain*(self.data[f'hand_target{self.demo}'][i]-q[7:][order]),-1,1) for j,g in enumerate(self.groups): if int(g[0])<=i<=int(g[-1]): if self.grasp_pose[j] is None:self.grasp_pose[j]=q[7:][order].copy() a[6:]=np.clip(.5*(self.grasp_pose[j]-q[7:][order]),-1,1) # Bound both pose holds and image-guided extensions within the native horizon. if np.linalg.norm(err)>.02 and self.waits<120:self.waits+=1 else:self.phase+=speed return a.astype(np.float32) _controller=None def reset_episode(): global _controller if _controller is None:_controller=Controller() else:_controller.reset_episode() def act(obs): global _controller if _controller is None:reset_episode() return _controller.act(obs)