Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
6.69 kB
"""D20 observation-conditioned lookup with bounded image feedback.
No simulator, kinematics package, neural weights, seed, or timed demo replay.
"""
from pathlib import Path
import cv2
import numpy as np
CAMERAS = ('top_camera', 'front_camera')
_BANK = None
_last_action = None
_unsupported = 0
_target_y = .5
last_diagnostics = {}
def reset_episode():
global _last_action, last_diagnostics, _unsupported, _target_y
_last_action = None
_unsupported = 0
_target_y = .5
last_diagnostics = {'status': 'reset'}
def features(obs):
"""Fixed hand-designed metric, shared verbatim by compiler and runtime."""
parts = []
blue_counts = []
for camera in CAMERAS:
im = np.asarray(obs['rgb'][camera])
if im.ndim == 4 and im.shape[0] == 1:
im = im[0]
if im.ndim != 3 or im.shape[2] != 3 or im.dtype != np.uint8:
raise ValueError('Expected uint8 HWC or 1HWC RGB')
r, g, b = [im[..., i].astype(np.float32) for i in range(3)]
masks = [(b > 65) & (b > r * 1.6 + 15) & (b > g * 1.4 + 15),
(r > 65) & (r > b * 1.5 + 15) & (r > g * 1.4 + 15)]
blue_counts.append(int(masks[0].sum()))
# Spatial masks preserve visible box shape and target location. Missing
# red pixels remain a visibility feature; target may be occluded.
for mask in masks:
small = cv2.resize(mask.astype(np.float32), (16, 16), interpolation=cv2.INTER_AREA)
parts.append(small.ravel() * (8.0 / 16.0))
yy, xx = np.nonzero(mask)
if len(xx) >= 8:
x, y = xx / im.shape[1], yy / im.shape[0]
geom = [x.mean(), y.mean(), x.std(), y.std(),
np.mean((x-x.mean())*(y-y.mean())), len(xx)/mask.size, 1.0]
else:
geom = [0.] * 7
parts.append(np.asarray(geom, np.float32) * np.array([12,12,6,6,20,10,1],np.float32))
small_rgb = cv2.resize(im, (16,16), interpolation=cv2.INTER_AREA).astype(np.float32)/255
parts.append(small_rgb.ravel() * (1.0 / np.sqrt(768)))
for kind, scale in [('qpos', .20), ('qvel', 1.0)]:
q = []
for robot in range(2):
v = np.asarray(obs['proprioception'][f'agent/panda-{robot}/{kind}'],dtype=np.float32).reshape(-1)
if v.size != 9 or not np.isfinite(v).all():
raise ValueError('Expected nine finite proprioceptive values per robot')
q.extend(v[:7])
parts.append(np.array(q,np.float32)/(scale*np.sqrt(14)))
f = np.concatenate(parts).astype(np.float32)
if not np.isfinite(f).all():
raise ValueError('Nonfinite features')
return f, min(blue_counts) >= 8
def _load():
global _BANK
if _BANK is None:
with np.load(Path(__file__).with_name('demo_bank.npz'), allow_pickle=False) as d:
_BANK = {k: d[k] for k in d.files}
return _BANK
def box_pose(im):
r,g,b=[im[...,i].astype(float) for i in range(3)]
mask=((b>65)&(b>1.6*r+15)&(b>1.4*g+15)).astype(np.uint8)
contours,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
if not contours:return None
contour=max(contours,key=cv2.contourArea)
(x,y),(w,h),angle=cv2.minAreaRect(contour)
pts=cv2.boxPoints(((x,y),(w,h),angle));edges=np.roll(pts,-1,axis=0)-pts;v=edges[np.argmax(np.linalg.norm(edges,axis=1))]
if v[1]<0:v=-v
tilt=np.arctan2(v[0],v[1])
return dict(x=float(x/im.shape[1]),y=float(y/im.shape[0]),tilt=float(tilt),area=float(cv2.contourArea(contour)/mask.size))
def act(obs):
global _last_action, last_diagnostics, _unsupported, _target_y
# Native horizon includes the runner-owned reset hold. No extra hold here.
if int(obs.get('step_id', 1)) >= 700:
last_diagnostics = {'status': 'native_horizon'}
return None
try:
f, visible = features(obs)
except (KeyError, ValueError, TypeError):
last_diagnostics = {'status': 'invalid_observation'}
return None
if _last_action is None:
im=np.asarray(obs['rgb']['top_camera']).squeeze().astype(float)
r,g,b=im[...,0],im[...,1],im[...,2]
yy,xx=np.nonzero((r>65)&(r>1.5*b+15)&(r>1.4*g+15))
if len(yy)>100: _target_y=float((yy.min()+yy.max())/(2*im.shape[0]))
bank = _load()
distance = np.sum((bank['features']-f)**2,axis=1)
idx = np.array([np.where(bank['demo_index']==di)[0][np.argmin(distance[bank['demo_index']==di])] for di in range(20)])
idx = idx[np.argsort(distance[idx])][:5]
best = idx[0]
# Reject gross extrapolation. This threshold is heuristic, not validated.
if not visible or distance[best] > 16.0:
action = np.zeros(14,np.float32); action[[6,13]] = -1
last_diagnostics = {'status': 'unsupported_hold', 'squared_distance': float(distance[best])}
else:
# Avoid averaging opposing contact/repositioning actions. Both robot
# commands always come from the same paired observation/action record.
compatible = np.max(np.abs(bank['actions'][idx]-bank['actions'][best]),axis=1) <= .12
idx = idx[compatible]
weights = np.exp(-(distance[idx]-distance[best])/.35)
action = np.average(bank['actions'][idx], axis=0, weights=weights)
action = np.clip(action,-1,1).astype(np.float32)
last_diagnostics = {'status': 'lookup', 'squared_distance': float(distance[best]),
'reference_demo': int(bank['demo_index'][best]),
'reference_frame': int(bank['frame_index'][best])}
pose = box_pose(np.asarray(obs['rgb']['top_camera']).squeeze())
if (last_diagnostics['status']=='lookup' and pose is not None
and .54 < pose['x'] < .83 and pose['area'] > .045
and abs(pose['tilt']) < .65
and min(action[0], action[7]) > .035
and min(action[2], action[9]) > -.05):
tilt = pose['tilt']
correction = np.clip(.18 * np.sign(tilt)*max(abs(tilt)-.06,0),-.06,.06)
action[0] = np.clip(action[0] + correction, 0, .15)
action[7] = np.clip(action[7] - correction, 0, .15)
last_diagnostics['yaw_correction'] = float(correction)
last_diagnostics['box_tilt'] = float(tilt)
lateral=float(np.clip(.4*(pose['y']-_target_y),-.035,.035))
action[1] = np.clip(action[1]+lateral,-.15,.15)
action[8] = np.clip(action[8]+lateral,-.15,.15)
last_diagnostics['lateral_correction'] = lateral
_unsupported = _unsupported+1 if last_diagnostics['status']=='unsupported_hold' else 0
if _unsupported >= 40:
last_diagnostics['status']='unsupported_abort'
return None
_last_action = action.copy()
return action