"""D20 observation-conditioned nonparametric controller with bounded phase advance.""" from pathlib import Path import importlib.util import numpy as np _s=importlib.util.spec_from_file_location('_d20_features',Path(__file__).with_name('features.py')) _f=importlib.util.module_from_spec(_s);_s.loader.exec_module(_f) _bank=None _phase=None _calls=0 _bad=0 _last_step=0 def reset_episode(): global _phase,_calls,_bad,_last_step _phase=None;_calls=0;_bad=0;_last_step=0 def act(obs): global _bank,_phase,_calls,_bad,_last_step step=int(obs.get('step_id',_calls+1)) if step>=680 or _calls>=679: return None if _phase is not None and _phase>=.995: return np.zeros(44,dtype=np.float32) if step<_last_step: reset_episode() _last_step=step;_calls+=1 if _bank is None: with np.load(Path(__file__).with_name('bank.npz'),allow_pickle=False) as z: _bank={k:z[k] for k in z.files} b=_bank try: q=_f.proprio(obs['proprioception'],'qpos');v=_f.proprio(obs['proprioception'],'qvel');im=_f.visual(obs['rgb']) except (KeyError,ValueError,TypeError): _bad+=1 return None if _bad>=20 else np.zeros(44,dtype=np.float32) _bad=0 # Equal robot weighting; fingers are less reliable pose indicators under contact. scales=np.tile(np.r_[np.full(7,.22),np.full(16,.5)],2) dq=np.mean(((b['q']-q)/scales)**2,axis=1) dv=np.mean(np.minimum(((b['v']-v)/2)**2,9),axis=1) eligible=np.arange(len(dq)) if _phase is not None: eligible=eligible[(b['phase']>=max(0,_phase-.018)) & (b['phase']<=_phase+.009)] if len(eligible)<7: eligible=np.arange(len(dq)) shortlist=eligible[np.argsort((dq+.05*dv)[eligible])[:240]] score=dq[shortlist]+.05*dv[shortlist]+12*np.mean((b['img'][shortlist]-im)**2,axis=1) # Soft hysteresis disambiguates similar poses without selecting a fixed demo. if _phase is not None: dp=b['phase'][shortlist]-_phase score+=2*np.maximum(abs(dp)-.08,0)**2 ix=shortlist[np.argsort(score)[:7]] dist=np.sort(score)[:7] if dist[0]>12: return np.zeros(44,dtype=np.float32) w=np.exp(-(dist-dist.min())/.08);w/=w.sum() new_phase=float(w@b['phase'][ix]) _phase=new_phase if _phase is None else float(np.clip(new_phase,_phase+.0015,_phase+.0022)) a=w@b['a'][ix] # Hand action labels retain their recorded order; no unverified joint remapping. target=w@b['q'][ix] for j,offset in [(0,0),(22,23)]: # No direct finger correction: qpos and action ordering are unverified. a[j+3:j+6]=0 # every authorized label has zero rotation return np.clip(a,-1,1).astype(np.float32)