Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
4.18 kB
"""D20 observation-conditioned retrieval controller. No simulator or weights."""
from pathlib import Path
import numpy as np
import cv2
CAMERAS = ('top_camera', 'front_camera')
def image_feature(rgb):
parts = []
for camera in CAMERAS:
im = np.asarray(rgb[camera])
if im.ndim == 4 and im.shape[0] == 1:
im = im[0]
if im.ndim != 3 or im.shape[2] != 3 or min(im.shape[:2]) < 2:
raise ValueError('Expected RGB HWC or 1HWC')
if not np.isfinite(im).all():
raise ValueError('Nonfinite RGB')
parts.append(cv2.resize(im.astype(np.float32), (16, 16),
interpolation=cv2.INTER_AREA).ravel() / 255.)
return np.concatenate(parts)
_bank = None
_phase = None
_last_step = None
_diagnostics = {}
def reset_episode():
global _phase, _last_step, _diagnostics
_phase = None
_last_step = None
_diagnostics = {}
def _load():
global _bank
if _bank is None:
with np.load(Path(__file__).with_name('reference_bank.npz'), allow_pickle=False) as f:
_bank = {k: f[k] for k in f.files}
return _bank
def act(obs):
"""Return 6 root-frame arm deltas then 16 dex deltas, in one 22-vector.
All retrieval is recomputed from live legal observations. Temporal continuity
is a soft phase prior, never a selected-demo playback cursor. None means
horizon exhaustion or unusable input, and does not assert task completion.
"""
global _phase, _last_step, _diagnostics
if 'observation' in obs:
envelope = obs
obs = dict(envelope['observation'])
obs['step_id'] = envelope.get('step_id', 1 if _last_step is None else _last_step + 1)
step = int(obs.get('step_id', 1 if _last_step is None else _last_step + 1))
if step >= 500:
return None # Runner already charged one native hold step.
if _last_step is not None and step < _last_step:
reset_episode()
try:
p = obs['proprioception']
q = np.asarray(p['agent/qpos'], dtype=np.float32).reshape(-1)
v = np.asarray(p['agent/qvel'], dtype=np.float32).reshape(-1)
if q.size != 23 or v.size != 23 or not np.isfinite(q).all() or not np.isfinite(v).all():
return None
visual = image_feature(obs['rgb'])
except (KeyError, ValueError, TypeError):
return None
bank = _load()
# Coarse proprioceptive candidates; current RGB reranks every invocation.
dq = (bank['q'] - q) / bank['q_scale']
dv = (bank['v'] - v) / bank['v_scale']
cost = np.mean(dq[:, :7] ** 2, axis=1) + .6 * np.mean(dq[:, 7:] ** 2, axis=1)
cost += .08 * np.mean(np.minimum(dv ** 2, 25), axis=1)
if _phase is not None:
# A bounded one-step expectation resolves nearly stationary grasp stages.
cost += .35 * (bank['phase'] - _phase) ** 2
pool = np.argpartition(cost, min(95, len(cost)-1))[:96]
image_error = np.mean((bank['visual'][pool] - visual) ** 2, axis=1)
score = cost[pool] + 80. * image_error
order = np.argsort(score)
anchor = pool[order[0]]
# Do not average actions from opposite sides of a grasp transition.
compatible = pool[np.floor(bank['phase'][pool]) == np.floor(bank['phase'][anchor])]
vis = np.mean((bank['visual'][compatible] - visual) ** 2, axis=1)
total = cost[compatible] + 80. * vis
chosen = np.argsort(total)[:5]
ids = compatible[chosen]
weights = np.exp(-(total[chosen] - total[chosen[0]]) / .08)
weights /= weights.sum()
action = weights @ bank['actions'][ids]
estimate = float(weights @ bank['phase'][ids])
increment = float(weights @ bank['phase_rate'][ids])
if _phase is None:
_phase = estimate + increment
else:
_phase = float(np.clip(.8*_phase + .2*estimate + increment, 0, 7.99))
_last_step = step
_diagnostics = {'phase': _phase, 'reference_distance': float(total[chosen[0]]),
'visual_error': float(image_error[order[0]])}
if 5.0 <= _phase < 6.0:
action[6:] *= .5
# Exact action-part order: one width-22 part, no mimic gripper component.
return np.clip(action, -1., 1.).astype(np.float32)