Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
2.65 kB
"""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)