Download astra/DualArmClothAlign-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 3.11 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmClothAlign-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmClothAlign-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmClothAlign-v1/controller.py
3.11 kB
| """Geometric feedback control compiled from this task's D20 demonstrations.""" | |
| from pathlib import Path | |
| import importlib.util | |
| import numpy as np | |
| import cv2 | |
| _spec=importlib.util.spec_from_file_location('_kin',Path(__file__).with_name('kinematics.py'));_k=importlib.util.module_from_spec(_spec);_spec.loader.exec_module(_k) | |
| _phase=0;_ticks=0;_calls=0;_progress=0;_grasp=None;_place=None | |
| SCALE=396.5 | |
| def reset_episode(): | |
| global _phase,_ticks,_calls,_progress,_grasp,_place | |
| _phase=0;_ticks=0;_calls=0;_progress=0;_grasp=None;_place=None | |
| def locate(rgb): | |
| a=cv2.resize(np.asarray(rgb).squeeze(),(480,640)).astype(float) | |
| cyan=((a[:,:,1]-a[:,:,0]>25)&(a[:,:,2]-a[:,:,0]>25)).astype('uint8');cyan[:210]=0;cyan[440:]=0 | |
| n,l,stats,cent=cv2.connectedComponentsWithStats(cyan) | |
| i=1+np.argmax(stats[1:,4]);ys,xs=np.where(l==i);boundary=np.array([(xs.min()+xs.max())/2,(ys.min()+ys.max())/2]) | |
| white=((a.min(2)>180)&(a.max(2)-a.min(2)<50)).astype('uint8');white[:210]=0;white[440:]=0 | |
| n,l,stats,cent=cv2.connectedComponentsWithStats(white);i=1+np.argmax(stats[1:,4]);ys,xs=np.where(l==i) | |
| rect=cv2.minAreaRect(np.column_stack([xs,ys]).astype('float32'));center=np.array(rect[0]);angle=rect[2] | |
| if angle>45:angle-=90 | |
| return center,boundary,np.deg2rad(angle) | |
| def offset(pos,delta): | |
| p=pos.copy();p[0,0]+=delta[1]/SCALE;p[1,0]-=delta[1]/SCALE;p[0,1]+=delta[0]/SCALE;p[1,1]-=delta[0]/SCALE | |
| return p | |
| def act(obs): | |
| global _phase,_ticks,_calls,_progress,_grasp,_place | |
| if int(obs.get('step_id',_calls+1))>=200:return None | |
| _calls+=1 | |
| try: | |
| cur=np.array([_k.fk(np.asarray(obs['proprioception'][f'agent/panda-{i}/qpos']).reshape(-1)) for i in range(2)]) | |
| if _grasp is None: | |
| center,boundary,theta=locate(obs['rgb']['top_camera']) | |
| with np.load(Path(__file__).with_name('pose_bank.npz')) as b: | |
| gs=np.array([offset(p,center-c) for p,c in zip(b['place'],b['boundary'])]);ps=np.array([offset(p,boundary-c) for p,c in zip(b['place'],b['boundary'])]) | |
| _grasp=np.median(gs,axis=0);_place=np.median(ps,axis=0) | |
| for i,dy in enumerate([-.08,.08]): | |
| delta=np.array([-np.sin(theta)*dy,(np.cos(theta)-1)*dy])*SCALE | |
| tmp=offset(_grasp,delta);_grasp[i]=tmp[i] | |
| _grasp[:,2]=.124;_place[:,2]=.124 | |
| except (KeyError,ValueError,IndexError):return None | |
| _ticks+=1;_progress=float(_phase*20+_ticks) | |
| target=_grasp.copy();grip=1.;limit=.15 | |
| if _phase==0:target[:,2]=.24 | |
| elif _phase==1:pass | |
| elif _phase==2:grip=-1 | |
| elif _phase==3:target[:,2]=.20;grip=-1;limit=.18 | |
| elif _phase==4:target=_place.copy();target[:,2]=.20;grip=-1;limit=.18 | |
| elif _phase==5:target=_place.copy();grip=-1;limit=.095 | |
| elif _phase==6:target=_place.copy();limit=.095 | |
| elif _phase==7:target=_place.copy();target[:,2]=.30;limit=.12 | |
| else:return None | |
| err=target-cur | |
| a=np.zeros(14,dtype=np.float32) | |
| for i in range(2): | |
| e=err[i];d=np.array([e[1],-e[0],e[2]]) if i==0 else np.array([-e[1],e[0],e[2]]) | |
| a[i*7:i*7+3]=np.clip(d*5,-limit,limit);a[i*7+6]=grip | |
| # Pose convergence, with bounded dwell and phase timeout. | |
| done=np.max(np.abs(err))<.006 | |
| if _phase in (2,6):done=_ticks>=10 | |
| if (done and _ticks>=3) or _ticks>65: | |
| _phase+=1;_ticks=0 | |
| return a | |