"""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()