Download astra/CoverPattern-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 9.38 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/CoverPattern-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/CoverPattern-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/CoverPattern-v1/controller.py
9.38 kB
| """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)&(b<np.minimum(r,g)-5)&(np.abs(r-g)<12),'cyan':(np.minimum(g,b)>160)&(r<np.minimum(g,b)-3)&(np.abs(g-b)<12),'pink':(np.minimum(r,b)>160)&(g<np.minimum(r,b)-3)&(np.abs(r-b)<12)} | |
| out={} | |
| for name,mask in masks.items(): | |
| mask[:70]=False;mask[:,:100]=False | |
| n,labels,stats,cents=cv2.connectedComponentsWithStats(mask.astype('uint8')) | |
| good=np.flatnonzero(stats[:,4]>20);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]<d['size'][1] else 0)) | |
| vals.extend([np.cos(2*angle),np.sin(2*angle)]) | |
| return np.array(vals) | |
| def strips(loc): | |
| d=loc['green'];angle=np.deg2rad(d['angle']+(90 if d['size'][0]<d['size'][1] else 0));v=np.array([np.cos(angle),np.sin(angle)]) | |
| if v[1]<0:v=-v | |
| return np.array(d['center'])+np.arange(-1,2)[:,None]*max(d['size'])/3*v | |
| def finger_shape(hand): | |
| angles=np.asarray(hand)[[1,2,3,5,6,7,9,10,11]].reshape(3,3).mean(axis=0) | |
| cumulative=np.cumsum(angles);weights=np.array([.45,.30,.25]) | |
| return float(weights@np.cos(cumulative)),float(weights@np.sin(cumulative)) | |
| class Controller: | |
| def __init__(self): | |
| self.data=np.load(Path(__file__).with_name('paths.npz'),allow_pickle=False) | |
| self.reset_episode() | |
| def reset_episode(self): | |
| self.phase=0.;self.demo=None;self.waits=0;self.tick=0 | |
| self.corrected=[False]*3;self.correction_steps=[0]*3 | |
| self.released=[False]*3;self.release_state=None | |
| self.grasp_pose=[None]*3 | |
| def release_action(self,q,pos,obs): | |
| state=self.release_state;state['steps']+=1 | |
| order=[4,0,8,12,5,1,9,13,6,2,10,14,3,7,11,15] | |
| action=np.zeros(22,dtype=np.float32) | |
| hand=q[7:][order];action[6:]=np.clip(-.35*hand,-1,1) | |
| if state['stage']==0: | |
| reach0,height0=state['shape0'] | |
| def movement(h): | |
| reach,height=finger_shape(h) | |
| return np.array([0,.05*np.clip((reach-reach0)/max(1-reach0,.05),0,1),.06*np.clip((height0-height)/max(height0,.05),0,1)]) | |
| now=movement(hand);future=movement(hand+.095*action[6:]) | |
| goal=state['origin']+now | |
| action[:3]=np.clip((future-now)/.04+5*(goal-pos),-.15,.15) | |
| action[:2]=(future-now)[:2]/.04 | |
| live=locate(np.asarray(obs['rgb']['top_camera']).squeeze()).get(['yellow','cyan','pink'][state['cloth']]) | |
| if live is not None and live['pixels']>=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<end+5: | |
| live=locate(np.asarray(obs['rgb']['top_camera']).squeeze()).get(['yellow','cyan','pink'][j]) | |
| dy=(live['center'][1]-(self.target_pixels[j,1]+12)) if live is not None else 0 | |
| dx=(live['center'][0]-(self.target_pixels[j,0]+4)) if live is not None else 0 | |
| lateral=live is not None and live['pixels']>=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 -20<dy<65 and (dy>3 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<end+5: | |
| self.released[j]=True | |
| resume=min(end+80,len(self.a)-1) | |
| self.release_state={'stage':0,'steps':0,'goal':pos.copy(),'origin':pos.copy(),'shape0':finger_shape(q[7:][order]),'resume':resume,'cloth':j} | |
| return self.release_action(q,pos,obs) | |
| err=self.p[i]-pos | |
| speed=1.0 | |
| if self.a[i,1]<-.015 or self.a[i,2]>.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) | |