Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
3.16 kB
"""D20 observation-conditioned nonparametric controller; E20 terminal guard."""
from pathlib import Path
import json
import cv2
import numpy as np
ROOT = Path(__file__).resolve().parent
CONFIG = json.loads((ROOT / 'config.json').read_text())
_bank = None
_bad = 0
_terminal = 0
def reset_episode():
global _bad, _terminal
_bad = 0
_terminal = 0
def image_feature(rgb):
out = []
for camera in CONFIG['cameras']:
x = np.asarray(rgb[camera])
if x.ndim == 4 and x.shape[0] == 1:
x = x[0]
if x.ndim != 3 or x.shape[2] != 3 or x.dtype != np.uint8:
raise ValueError('Expected uint8 HWC RGB')
# Whole view plus central manipulation area; normalized coordinates.
h, w = x.shape[:2]
crops = [x, x[int(.3*h):int(.95*h), int(.25*w):int(.85*w)]]
out.extend(cv2.resize(c, (24,24), interpolation=cv2.INTER_AREA).astype(np.float32).ravel()/255 for c in crops)
return np.concatenate(out)
def proprio_feature(proprio):
values = [np.asarray(proprio[p], dtype=np.float32).reshape(-1) for p in CONFIG['proprio_paths']]
if any(v.shape != (23,) for v in values):
raise ValueError('Expected 23 coordinates per proprioceptive field')
x = np.concatenate(values)
if not np.isfinite(x).all():
raise ValueError('Nonfinite proprioception')
return x
def act(obs):
global _bank, _bad, _terminal
if int(obs['step_id']) >= CONFIG['native_horizon']:
return None # Initial hold has already consumed native step one.
if _bank is None:
with np.load(ROOT / 'demo_bank.npz', allow_pickle=False) as data:
_bank = {k:data[k] for k in data.files}
# Floating-point residue is not a useful motion label.
keep = np.max(np.abs(_bank['actions']),axis=1) > 1e-5
for key in ('proprio','visual','actions','source','frame'):
_bank[key] = _bank[key][keep]
try:
q = proprio_feature(obs['proprioception'])
v = image_feature(obs['rgb'])
except (KeyError, ValueError, TypeError):
_bad += 1
return None if _bad >= 10 else np.zeros(44, dtype=np.float32)
dq = np.mean(((_bank['proprio']-q)/_bank['scale'])**2, axis=1)
# Recompute candidates from present measurements on every call. No scene/seed lookup.
n = int(CONFIG.get('neighbors',64))
ids = np.argpartition(dq, min(n-1,len(dq)-1))[:n]
dv = np.mean((_bank['visual'][ids]-v)**2, axis=1)
scores = dq[ids] + float(CONFIG.get('visual_weight',12))*dv
best = int(np.argmin(scores))
if dq[ids[best]] > 9.0 or dv[best] > .10:
_bad += 1
return None if _bad >= 10 else np.zeros(44,dtype=np.float32)
_bad = 0
# A single joint/visual neighbor keeps the two arms' labels synchronized.
i = ids[best]
src = _bank['source'][i]
last = _bank['frame'][_bank['source']==src].max()
_terminal = _terminal + 1 if _bank['frame'][i] >= last-1 else 0
if _terminal >= int(CONFIG.get('terminal_limit',6)):
return None
action = _bank['actions'][i].copy()
return np.clip(action, CONFIG['action_low'], CONFIG['action_high']).astype(np.float32)