Download astra/DualArmCleanupPotEnv-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 4.54 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmCleanupPotEnv-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmCleanupPotEnv-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmCleanupPotEnv-v1/controller.py
4.54 kB
| """Task-local offline reference controller with image feedback and grip gates.""" | |
| from pathlib import Path | |
| import importlib.util | |
| import numpy as np | |
| ROOT=Path(__file__).resolve().parent | |
| def module(name): | |
| s=importlib.util.spec_from_file_location('_'+name,ROOT/(name+'.py'));m=importlib.util.module_from_spec(s);s.loader.exec_module(m);return m | |
| features=module('features');geometry=module('geometry') | |
| _memory=None | |
| _targets=None | |
| _wrists=None | |
| _initial=None | |
| def reset_episode(): | |
| global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count | |
| _frame=1;_selected=None;_ids=None;_last_grip=np.ones(2,np.float32);_last_kind=np.zeros(2,np.int8);_wrist_previous=None;_gate_count=0 | |
| def load(): | |
| global _memory,_targets,_wrists,_initial | |
| if _memory is None: | |
| with np.load(ROOT/'memory.npz',allow_pickle=False) as f:_memory={k:f[k] for k in f.files} | |
| with np.load(ROOT/'targets.npz',allow_pickle=False) as f:_targets={k:f[k] for k in f.files} | |
| _wrists=np.load(ROOT/'wrists.npz',allow_pickle=False)['wrist'] | |
| _initial=np.load(ROOT/'initial_geometry.npz',allow_pickle=False)['pot'] | |
| def hold(q=None): | |
| a=np.zeros(14,np.float32);a[[6,13]]=_last_grip if q is None else np.clip(2*(q[[7,16]]+.01)/.05-1,-1,1);return a | |
| def act(obs): | |
| global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count | |
| if 'observation' in obs: | |
| outer=obs;obs=dict(outer['observation']);obs['step_id']=outer.get('step_id',obs.get('step_id',1)) | |
| if int(obs.get('step_id',1))>=1000:return None | |
| try:q,v=features.proprio(obs['proprioception']) | |
| except (KeyError,TypeError,ValueError):return hold() | |
| try: | |
| rgb=obs['rgb'] | |
| if any(np.std(features.rgb_array(rgb[c]))<2 for c in features.CAMERAS):return hold(q) | |
| im=features.visual(rgb) | |
| except (KeyError,TypeError,ValueError):return hold(q) | |
| load();m=_memory;pot=geometry.pot_geometry(rgb['top_camera']);cubes=geometry.cube_positions(rgb['top_camera']) | |
| if _selected is None: | |
| ids=np.flatnonzero(m['frame']==0);delta=_initial-pot | |
| prior=200*delta[:,0]**2+350*delta[:,1]**2+.75*np.sum(delta[:,2:4]**2,axis=1) | |
| if not pot[4]:prior=np.zeros(len(_initial)) | |
| score=35*np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1)+prior[m['demo'][ids]] | |
| _selected=int(m['demo'][ids[np.argmin(score)]]);_ids=np.flatnonzero(m['demo']==_selected) | |
| if _frame>int(m['frame'][_ids[-1]])+25:return None | |
| # Keep demonstrated order; observation matching refines only a narrow local window. | |
| ids=_ids[np.abs(m['frame'][_ids]-min(_frame,m['frame'][_ids[-1]]))<=2] | |
| scales=np.tile([.35]*7+[.025,.025],2) | |
| qd=np.mean(((m['q'][ids]-q)/scales)**2,axis=1) | |
| vd=np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1) | |
| score=qd+20*vd+.08*(m['frame'][ids]-_frame)**2 | |
| best=int(ids[np.argmin(score)]);a=m['action'][best].copy();kind=_targets['kind'][best].copy() | |
| live=geometry.track_wrists(rgb['top_camera'],_wrist_previous) | |
| if _wrist_previous is None:_wrist_previous=np.array([[240,55],[240,582]],np.float32) | |
| correction=np.zeros((2,2),np.float32);gates=np.zeros(2,bool) | |
| for r in range(2): | |
| closing=_last_grip[r]>0 and a[6+7*r]<0 | |
| opening=_last_grip[r]<0 and a[6+7*r]>0 | |
| active_kind=int(kind[r]) | |
| if (closing or opening) and active_kind==0:active_kind=int(_last_kind[r]) | |
| if live[r,2]: | |
| _wrist_previous[r]=live[r,:2] | |
| if _wrists[best,r,2]: | |
| correction[r]=.0045*(_wrists[best,r,:2]-live[r,:2])*np.array([1,-1]) | |
| correction[r]+=geometry.target_offset(active_kind,r,pot,cubes,_targets['pot'][best],_targets['cubes'][best]) | |
| # Small horizontal correction before a contact-changing command. | |
| if closing and active_kind in (1,2,3) and np.linalg.norm(correction[r])>.036:gates[r]=True | |
| if opening and int(_last_kind[r])==4 and pot[4] and cubes[r,2]: | |
| delta=(pot[:2]-cubes[r,:2])*np.array([480,640]) | |
| held=True | |
| if live[r,2] and _wrists[best,r,2] and _targets['cubes'][best,r,2]: | |
| rel=cubes[r,:2]*[480,640]-live[r,:2] | |
| refrel=_targets['cubes'][best,r,:2]*[480,640]-_wrists[best,r,:2] | |
| held=np.linalg.norm(rel-refrel)<35 | |
| if held and np.linalg.norm(delta)>35: | |
| gates[r]=True;correction[r]=.0045*delta*np.array([1,-1]) | |
| a[7*r:7*r+2]+=np.clip(correction[r],-.06,.06) | |
| if gates.any() and _gate_count<30: | |
| # Pause the reference clock and other motion; no repeated descent while aligning. | |
| a=hold();a[[6,13]]=_last_grip | |
| for r in range(2): | |
| if gates[r]:a[7*r:7*r+2]=np.clip(correction[r],-.06,.06) | |
| _gate_count+=1 | |
| else: | |
| _gate_count=0;_frame+=1;_last_kind=kind | |
| a=np.clip(np.nan_to_num(a),-1,1).astype(np.float32);_last_grip=a[[6,13]].copy() | |
| return a | |
| reset_episode() | |