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