Download astra/DualArmLiquidMixing-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 5.62 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmLiquidMixing-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmLiquidMixing-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmLiquidMixing-v1/controller.py
5.62 kB
| """D20 observation-conditioned retrieval with bounded E20 local recovery. | |
| No scene IDs, seeds, environment imports, kinematic oracle, or trained weights. | |
| """ | |
| from pathlib import Path | |
| import importlib.util | |
| import numpy as np | |
| import cv2,json | |
| _CFG_PATH=Path(__file__).with_name("config.json") | |
| _CFG=json.loads(_CFG_PATH.read_text()) if _CFG_PATH.exists() else {} | |
| _local=None | |
| _calib=None | |
| _spec=importlib.util.spec_from_file_location('_d20_features',Path(__file__).with_name('features.py')) | |
| _f=importlib.util.module_from_spec(_spec);_spec.loader.exec_module(_f) | |
| _KEYS=('panda_leaphand-0','panda_leaphand_left-1') | |
| _bank=None | |
| _state=None | |
| def reset_episode(): | |
| global _state | |
| _state={'cursor':np.ones(20,dtype=np.int32),'last':0,'stable':0,'branch':None,'diagnostic':'observation matching','calls':0,'stalled':0,'last_progress':0} | |
| def act(obs): | |
| global _bank,_local,_calib | |
| if _state is None:reset_episode() | |
| if 'observation' in obs: | |
| wrapper=obs;obs=dict(obs['observation']);obs['step_id']=wrapper.get('step_id',1) | |
| _state['calls']+=1 | |
| step=int(obs.get('step_id',_state['calls'])) | |
| if _state['calls']>=int(_CFG.get('max_calls',700)):return None | |
| if step>=800:return None # runner-owned initial hold already consumes step zero | |
| if _bank is None: | |
| with np.load(Path(__file__).with_name('bank.npz'),allow_pickle=False) as data: | |
| _bank={k:data[k] for k in data.files} | |
| if _local is None: | |
| with np.load(Path(__file__).with_name('localization_bank.npz'),allow_pickle=False) as z:_local={k:z[k] for k in z.files} | |
| if _calib is None: | |
| with np.load(Path(__file__).with_name('recovery_calibration.npz'),allow_pickle=False) as z:_calib=z['inverse'].copy() | |
| try: | |
| p=obs['proprioception'] | |
| q=np.concatenate([np.asarray(p['agent/'+k+'/qpos']).reshape(-1) for k in _KEYS]).astype(np.float32) | |
| v=np.concatenate([np.asarray(p['agent/'+k+'/qvel']).reshape(-1) for k in _KEYS]).astype(np.float32) | |
| x=_f.visual(obs['rgb']) | |
| top=cv2.resize(np.asarray(obs['rgb']['top_camera']).squeeze(),(240,320),interpolation=cv2.INTER_AREA) | |
| y=cv2.resize(top[125:195,155:240],(16,16),interpolation=cv2.INTER_AREA).astype(np.float32).reshape(-1)/255. | |
| cp=_f.cups(top).reshape(-1) | |
| if q.shape!=(46,) or v.shape!=(46,) or not np.isfinite(q).all() or not np.isfinite(v).all():raise ValueError('Invalid proprioception') | |
| except (KeyError,ValueError,TypeError): | |
| _state['diagnostic']='missing or invalid legal observations' | |
| return np.zeros(44,dtype=np.float32) | |
| # The instruction is available, but numeric mixture targets have no authorized | |
| # per-demo labels. Do not fabricate a mapping from words to liquid quantities. | |
| _state['instruction']=str(obs.get('instruction','')) | |
| arm=np.r_[0:7,23:30];hand=np.r_[7:23,30:46] | |
| matches=[] | |
| green_first={3,6,9,13,15,17,18} | |
| for i in range(20): | |
| branch=i in green_first | |
| if _state['branch'] is not None and branch!=_state['branch']:continue | |
| n=len(_bank[f'a{i}']);c=int(_state['cursor'][i]) | |
| ids=np.arange(max(1,c-4),min(n,c+13)) | |
| dq=_bank[f'q{i}'][ids]-q | |
| qcost=np.mean((dq[:,arm]/.18)**2,axis=1)+.15*np.mean((dq[:,hand]/.3)**2,axis=1) | |
| vcost=.03*np.mean((_bank[f'v{i}'][ids]-v)**2,axis=1) | |
| xcost=np.mean((_bank[f'x{i}'][ids]-x)**2,axis=1)/.012 | |
| timecost=.003*(ids-(c+1))**2 | |
| ccost=np.nan_to_num(np.mean(np.minimum(((_local[f'c{i}'][ids]-cp)/4.)**2,4.),axis=1),nan=0.) | |
| ycost=np.minimum(np.mean((_local[f'y{i}'][ids]-y)**2,axis=1)/.012,2.) | |
| score=qcost+vcost+xcost+timecost+float(_CFG.get('cup_weight',.03))*ccost+float(_CFG.get('target_weight',.15))*ycost | |
| j=int(np.argmin(score));t=int(ids[j]) | |
| matches.append((float(score[j]),i,t,float(qcost[j]),float(xcost[j]))) | |
| score,i,t,qcost,xcost=min(matches) | |
| if qcost>16 or xcost>8: | |
| _state['diagnostic']='outside demonstration support; holding' | |
| _state['stable']=0 | |
| return np.zeros(44,dtype=np.float32) | |
| # Advance only with observed support; no clock-indexed action playback. | |
| for _,di,ti,qc,xc in matches: | |
| if qc<16 and xc<8:_state['cursor'][di]=max(int(_state['cursor'][di]),ti) | |
| if t>65:_state['branch']=i in green_first | |
| _state['match']={'demo':int(i),'frame':int(t),'qcost':qcost,'xcost':xcost,'score':score} | |
| _state['stalled']=_state['stalled']+1 if t<=_state['last_progress'] else 0 | |
| _state['last_progress']=max(t,_state['last_progress']) | |
| if _state['stalled']>int(_CFG.get('stall_limit',100)):return None | |
| a=_bank[f'a{i}'][t].copy() | |
| if t<=10: | |
| for ri in range(2): | |
| err=_bank[f'q{i}'][t,23*ri:23*ri+7]-q[23*ri:23*ri+7] | |
| if np.linalg.norm(err)<.12: | |
| a[22*ri:22*ri+6]+=np.clip(float(_CFG.get('recovery_gain',.25))*(_calib[ri]@err),-.04,.04) | |
| if t>80: | |
| a[np.r_[0:6,22:28]]*=float(_CFG.get('motion_gain',1.0)) | |
| for start in (3,25): | |
| norm=np.linalg.norm(a[start:start+3]) | |
| if norm>1:a[start:start+3]/=norm | |
| if _state['calls']%40==0:print(json.dumps({'call':_state['calls'],'match':_state['match'],'stalled':_state['stalled']}),flush=True) | |
| near_end=t>=len(_bank[f'a{i}'])-22 | |
| stable=near_end and np.max(np.abs(v[arm]))<.12 and qcost<.2 and xcost<.2 | |
| _state['stable']=_state['stable']+1 if stable else 0 | |
| _state['diagnostic']='terminal resemblance (not verified success)' if stable else 'observation-conditioned action' | |
| _state['last']=step | |
| if _state['stable']>=20:return None | |
| return np.clip(a,-1.,1.).astype(np.float32) | |
| reset_episode() | |