Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
6.11 kB
"""Unrehearsed D20 draft: nonparametric RGB/proprio action retrieval.
No environment imports, learned neural weights, seed lookup, or timed replay.
"""
from pathlib import Path
import numpy as np
import cv2
from localizer import Localizer
CAMERAS = ('front_camera', 'left_camera', 'robot_camera', 'top_camera')
# Normalized crops selected by visual inspection; extrinsics remain unknown.
CROPS = ((.18,.32,.65,.90), (.28,.25,.95,.95), (0.,0.,1.,1.), (.10,.40,.95,.98))
def image_features(rgb):
out = []
for name, (x0,y0,x1,y1) in zip(CAMERAS,CROPS):
im = np.asarray(rgb[name])
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]) < 16 or im.dtype != np.uint8:
raise ValueError('Expected uint8 RGB HWC or 1HWC')
h,w = im.shape[:2]
crop = im[int(y0*h):int(y1*h),int(x0*w):int(x1*w)]
small = cv2.resize(crop,(12,12),interpolation=cv2.INTER_AREA).astype(np.float32)/255.
# Red excess helps distinguish the chips package; it is not an object detector.
red = np.maximum(small[:,:,0] - np.maximum(small[:,:,1],small[:,:,2]),0.)
out.append(np.concatenate([small,red[:,:,None]],axis=2).reshape(-1))
return np.stack(out)
class Controller:
def __init__(self):
with np.load(Path(__file__).with_name('reference_bank.npz'),allow_pickle=False) as z:
self.bank = {k:z[k] for k in z.files}
self.localizer = Localizer()
self.reset_episode()
def reset_episode(self):
self.target_hint = None
self.scene_penalty = np.zeros(20,dtype=np.float32)
self.calls = 0
self.last_index = None
self.visits = np.zeros(len(self.bank['actions']),dtype=np.int32)
self.status = 'reset; untested draft'
def act(self, obs):
# The charged initial hold is owned by the runner. No warmup is inserted.
try:
self.calls += 1
if 'observation' in obs:
outer = obs
obs = dict(outer['observation'])
obs['step_id'] = outer.get('step_id', self.calls)
step = int(obs.get('step_id', self.calls))
if step >= 650:
self.status = 'native horizon reached; no success claim'
return None
if step <= 21:
self.status = 'settling clutter; legal zero deltas'
return np.zeros(22,dtype=np.float32)
if step < 1:
raise ValueError('step_id must start at one after runner hold')
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.shape != (23,) or v.shape != (23,) or not np.isfinite(q).all() or not np.isfinite(v).all():
raise ValueError('Invalid proprioception')
f = image_features(obs['rgb'])
except (KeyError,ValueError,TypeError,OverflowError):
self.status = 'invalid legal observation; zero deltas'
return np.zeros(22,dtype=np.float32)
b = self.bank
if self.target_hint is None and step <= 80:
found = self.localizer.locate(obs['rgb']['left_camera'])
if found is not None and found['matches'] >= 5:
self.target_hint = found
lb = self.localizer.bank
axes = np.array([lb[f'axis_{i}'] for i in range(20)])
delta = (lb['centers']-np.array(found['center']))/100.
self.scene_penalty = .5*np.sum(delta*delta,axis=1) + .7*(1.-np.abs(axes@np.array(found['axis'])))
qerr = ((b['qpos']-q)/b['qscale'])**2
arm = np.mean(qerr[:,:7],axis=1)
hand = np.mean(qerr[:,7:],axis=1)
vel = np.mean(np.minimum(((b['qvel']-v)/b['vscale'])**2,9.),axis=1)
vd = np.mean((b['features']-f)**2,axis=2)
# Wrist camera has rendering artifacts and receives lower weight.
visual = vd @ np.array([.30,.30,.10,.30],dtype=np.float32)
red_dist = np.mean((b['features'][:,:,3::4]-f[:,3::4])**2,axis=(1,2))
cost = 12.*visual + 30.*red_dist + .25*arm + .18*hand + .015*vel
cost += self.scene_penalty[b['demo']]
base_cost = cost.copy()
if self.last_index is not None:
j = self.last_index
same = b['demo'] == b['demo'][j]
distance = np.abs(b['frame']-b['frame'][j])
cost += np.where(same,.015*np.minimum(distance/40.,4.),.12)
cost += .001*np.minimum(self.visits,80)
cost += np.where(np.isin(b['demo'],[0,3,7,12,19]),100.,0.)
j = int(np.argmin(cost))
# Deliberately uncalibrated support checks; do not improvise large motions.
arm_rms = float(np.sqrt(np.mean((b['qpos'][j,:7]-q[:7])**2)))
if visual[j] > .12 or arm_rms > .65 or base_cost[j] > 2.5:
self.last_index = None
self.status = 'outside reference coverage; zero deltas'
return np.zeros(22,dtype=np.float32)
# Only blend nearby samples with compatible arm commands and hand pose.
eligible = np.flatnonzero((np.linalg.norm(b['actions'][:,:6]-b['actions'][j,:6],axis=1)<.12)
& (np.mean((b['qpos'][:,7:]-b['qpos'][j,7:])**2,axis=1)<.04))
ids = np.array([j])
weight = np.exp(-(cost[ids]-np.min(cost[ids]))/.025)
action = np.sum(b['actions'][ids]*weight[:,None],axis=0)/weight.sum()
self.visits[j] += 1
self.last_index = j
self.status = 'observation-conditioned retrieval; completion unknown'
# Single contract action part: arm translation, arm rotation, 16 dex deltas.
return np.clip(action,-1.,1.).astype(np.float32)
_controller = None
def reset_episode():
global _controller
if _controller is None:
_controller = Controller()
else:
_controller.reset_episode()
def act(obs):
global _controller
if _controller is None:
reset_episode()
return _controller.act(obs)