Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
4.54 kB
"""Task-local offline reference controller with image feedback and grip gates."""
from pathlib import Path
import importlib.util
import numpy as np
ROOT=Path(__file__).resolve().parent
def module(name):
s=importlib.util.spec_from_file_location('_'+name,ROOT/(name+'.py'));m=importlib.util.module_from_spec(s);s.loader.exec_module(m);return m
features=module('features');geometry=module('geometry')
_memory=None
_targets=None
_wrists=None
_initial=None
def reset_episode():
global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count
_frame=1;_selected=None;_ids=None;_last_grip=np.ones(2,np.float32);_last_kind=np.zeros(2,np.int8);_wrist_previous=None;_gate_count=0
def load():
global _memory,_targets,_wrists,_initial
if _memory is None:
with np.load(ROOT/'memory.npz',allow_pickle=False) as f:_memory={k:f[k] for k in f.files}
with np.load(ROOT/'targets.npz',allow_pickle=False) as f:_targets={k:f[k] for k in f.files}
_wrists=np.load(ROOT/'wrists.npz',allow_pickle=False)['wrist']
_initial=np.load(ROOT/'initial_geometry.npz',allow_pickle=False)['pot']
def hold(q=None):
a=np.zeros(14,np.float32);a[[6,13]]=_last_grip if q is None else np.clip(2*(q[[7,16]]+.01)/.05-1,-1,1);return a
def act(obs):
global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count
if 'observation' in obs:
outer=obs;obs=dict(outer['observation']);obs['step_id']=outer.get('step_id',obs.get('step_id',1))
if int(obs.get('step_id',1))>=1000:return None
try:q,v=features.proprio(obs['proprioception'])
except (KeyError,TypeError,ValueError):return hold()
try:
rgb=obs['rgb']
if any(np.std(features.rgb_array(rgb[c]))<2 for c in features.CAMERAS):return hold(q)
im=features.visual(rgb)
except (KeyError,TypeError,ValueError):return hold(q)
load();m=_memory;pot=geometry.pot_geometry(rgb['top_camera']);cubes=geometry.cube_positions(rgb['top_camera'])
if _selected is None:
ids=np.flatnonzero(m['frame']==0);delta=_initial-pot
prior=200*delta[:,0]**2+350*delta[:,1]**2+.75*np.sum(delta[:,2:4]**2,axis=1)
if not pot[4]:prior=np.zeros(len(_initial))
score=35*np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1)+prior[m['demo'][ids]]
_selected=int(m['demo'][ids[np.argmin(score)]]);_ids=np.flatnonzero(m['demo']==_selected)
if _frame>int(m['frame'][_ids[-1]])+25:return None
# Keep demonstrated order; observation matching refines only a narrow local window.
ids=_ids[np.abs(m['frame'][_ids]-min(_frame,m['frame'][_ids[-1]]))<=2]
scales=np.tile([.35]*7+[.025,.025],2)
qd=np.mean(((m['q'][ids]-q)/scales)**2,axis=1)
vd=np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1)
score=qd+20*vd+.08*(m['frame'][ids]-_frame)**2
best=int(ids[np.argmin(score)]);a=m['action'][best].copy();kind=_targets['kind'][best].copy()
live=geometry.track_wrists(rgb['top_camera'],_wrist_previous)
if _wrist_previous is None:_wrist_previous=np.array([[240,55],[240,582]],np.float32)
correction=np.zeros((2,2),np.float32);gates=np.zeros(2,bool)
for r in range(2):
closing=_last_grip[r]>0 and a[6+7*r]<0
opening=_last_grip[r]<0 and a[6+7*r]>0
active_kind=int(kind[r])
if (closing or opening) and active_kind==0:active_kind=int(_last_kind[r])
if live[r,2]:
_wrist_previous[r]=live[r,:2]
if _wrists[best,r,2]:
correction[r]=.0045*(_wrists[best,r,:2]-live[r,:2])*np.array([1,-1])
correction[r]+=geometry.target_offset(active_kind,r,pot,cubes,_targets['pot'][best],_targets['cubes'][best])
# Small horizontal correction before a contact-changing command.
if closing and active_kind in (1,2,3) and np.linalg.norm(correction[r])>.036:gates[r]=True
if opening and int(_last_kind[r])==4 and pot[4] and cubes[r,2]:
delta=(pot[:2]-cubes[r,:2])*np.array([480,640])
held=True
if live[r,2] and _wrists[best,r,2] and _targets['cubes'][best,r,2]:
rel=cubes[r,:2]*[480,640]-live[r,:2]
refrel=_targets['cubes'][best,r,:2]*[480,640]-_wrists[best,r,:2]
held=np.linalg.norm(rel-refrel)<35
if held and np.linalg.norm(delta)>35:
gates[r]=True;correction[r]=.0045*delta*np.array([1,-1])
a[7*r:7*r+2]+=np.clip(correction[r],-.06,.06)
if gates.any() and _gate_count<30:
# Pause the reference clock and other motion; no repeated descent while aligning.
a=hold();a[[6,13]]=_last_grip
for r in range(2):
if gates[r]:a[7*r:7*r+2]=np.clip(correction[r],-.06,.06)
_gate_count+=1
else:
_gate_count=0;_frame+=1;_last_kind=kind
a=np.clip(np.nan_to_num(a),-1,1).astype(np.float32);_last_grip=a[[6,13]].copy()
return a
reset_episode()