Download astra/DualArmMahjongSetup-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 2.91 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmMahjongSetup-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmMahjongSetup-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmMahjongSetup-v1/controller.py
2.91 kB
| """D20 geometric visual localization and proprioceptive pose servo, offline only.""" | |
| from pathlib import Path | |
| import importlib.util | |
| import numpy as np | |
| _ROOT=Path(__file__).resolve().parent | |
| _s=importlib.util.spec_from_file_location('_geom',_ROOT/'geometry.py');g=importlib.util.module_from_spec(_s);_s.loader.exec_module(g) | |
| with np.load(_ROOT/'geometry.npz') as z:B=z['pixel_to_fk'];P=z['poses'];A=z['actions'] | |
| _last_phase=None | |
| def reset_episode(): | |
| global state,age,objects,pick,idx,last_step,last_action,_last_phase,retries,end_age | |
| state=0;age=0;objects=None;pick=0;idx=275;retries=0;end_age=0;last_step=None;last_action=None;_last_phase=0 | |
| reset_episode() | |
| def act(obs): | |
| global state,age,objects,pick,idx,last_step,last_action,_last_phase,retries,end_age | |
| step=int(obs['step_id']) | |
| if step>=700:return None | |
| if step==last_step:return last_action.copy() | |
| q=[np.asarray(obs['proprioception']['agent/panda-%d/qpos'%r]).reshape(-1) for r in range(2)] | |
| cur=[g.fk(x) for x in q] | |
| if objects is None: | |
| objects=sorted(g.detect(obs['rgb']['top_camera'],B),key=lambda o:o[0][0]) | |
| if len(objects)<2: | |
| objects=[(np.array([.5,.1]),0.),(np.array([.7,-.1]),0.)] | |
| objects=objects[:2] | |
| a=np.zeros(14,dtype=np.float32);a[[6,13]]=1 | |
| if state<9: | |
| xy,yaw=objects[pick];dest=.55 if pick==0 else .48 | |
| place_yaw=yaw+((.785-yaw+np.pi/2)%np.pi-np.pi/2) | |
| if state==0 and age==0: | |
| seen=g.detect(obs['rgb']['top_camera'],B) | |
| if seen: | |
| nearest=min(seen,key=lambda o:np.linalg.norm(o[0]-xy)) | |
| if np.linalg.norm(nearest[0]-xy)<.055: | |
| objects[pick]=nearest;xy,yaw=nearest | |
| grasp_z=.123-.006*retries | |
| if state==0:target=g.pose([*xy,.27],yaw) | |
| elif state in [1,2]:target=g.pose([*xy,grasp_z],yaw) | |
| elif state==3:target=g.pose([*xy,.27],yaw) | |
| elif state==4:target=g.pose([dest,0,.27],yaw) | |
| elif state==5:target=g.pose([dest,0,.27],place_yaw) | |
| elif state in [6,7]:target=g.pose([dest,0,.201],place_yaw) | |
| else:target=g.pose([dest,0,.36],place_yaw if pick==0 else -.785) | |
| u,dist,ang=g.servo(cur[1],target);a[7:13]=u | |
| a[13]=-1 if 2<=state<=6 else 1 | |
| age+=1 | |
| dwell=14 if state in [2,7] else 2 | |
| reached=dist<.008 and ang<.065 | |
| if (reached and age>=dwell) or age>=75: | |
| if state==2 and min(q[1][7:])<.012 and retries<1: | |
| retries+=1;state=0;age=0 | |
| else: | |
| state+=1;age=0 | |
| if state==9 and pick==0:pick=1;state=0;retries=0 | |
| _last_phase=pick*9+state | |
| else: | |
| # Advance the final demonstrated bilateral maneuver using current pose feedback. | |
| if idx>=len(P)-1: | |
| end_age+=1 | |
| if end_age>=35:return None | |
| target=P[min(idx+2,len(P)-1)] | |
| errors=[] | |
| for r in range(2): | |
| u,d,ang=g.servo(cur[r],target[r]); | |
| if r==0:u[[0,1,3,4]]*=-1 | |
| a[r*7:r*7+6]=u;a[r*7+6]=A[min(idx,len(A)-1),r*7+6];errors.append((d,ang)) | |
| age+=1 | |
| if max(e[0] for e in errors)<.018 and max(e[1] for e in errors)<.18 or age>=8: | |
| idx=min(idx+2,len(P)-1);age=0 | |
| _last_phase=100+idx | |
| last_step=step;last_action=a.copy();return a | |