Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
4.68 kB
"""D20 observation-conditioned retrieval with bounded terminal-state recovery."""
from pathlib import Path
import numpy as np
import cv2
ROBOTS = ('panda_leaphand-1', 'panda_leaphand_left-0')
CAMERAS = ('front_camera', 'top_camera')
_bank = None
_calls = 0
_still = 0
_pull = 0
_axis = np.array([0.,1.],dtype=np.float32)
def visual_features(image):
image = np.asarray(image)
if image.ndim == 4 and image.shape[0] == 1:
image = image[0]
if image.ndim != 3 or image.shape[2] != 3 or image.dtype != np.uint8:
raise ValueError('RGB must be uint8 HWC or 1HWC')
x = image.astype(np.float32) / 255.
# Color cue is a soft blue dominance map, not a claim of object identity.
blue = np.maximum(0., x[:,:,2] - np.maximum(x[:,:,0], x[:,:,1]))
return np.concatenate((cv2.resize(x, (24,24), interpolation=cv2.INTER_AREA).ravel(),
cv2.resize(blue, (32,32), interpolation=cv2.INTER_AREA).ravel())).astype(np.float32)
def reset_episode():
global _calls, _still, _pull, _axis
_calls = 0
_still = 0
_pull = 0
_axis = np.array([0.,1.],dtype=np.float32)
def _load():
global _bank
if _bank is None:
with np.load(Path(__file__).with_name('reference_bank.npz'), allow_pickle=False) as z:
_bank = {k:z[k] for k in z.files}
return _bank
def act(obs):
"""Return contract-ordered 44-vector; None is budget exhaustion, not success."""
global _calls, _still, _pull, _axis
step = int(obs.get('step_id', _calls + 1))
if step >= 400 or _calls >= 399:
return None
_calls += 1
try:
prop = obs['proprioception']
q = np.concatenate([np.asarray(prop[f'agent/{r}/qpos'], dtype=np.float32).reshape(-1) for r in ROBOTS])
v = np.concatenate([np.asarray(prop[f'agent/{r}/qvel'], dtype=np.float32).reshape(-1) for r in ROBOTS])
if q.shape != (46,) or v.shape != (46,) or not np.isfinite(q).all() or not np.isfinite(v).all():
return np.zeros(44, dtype=np.float32)
visuals = [visual_features(obs['rgb'][c]) for c in CAMERAS]
except (KeyError, ValueError, TypeError):
return np.zeros(44, dtype=np.float32)
if _pull == 0:
im = np.asarray(obs['rgb']['top_camera']).squeeze().astype(np.float32)
ys,xs = np.where((im[:,:,2]-im[:,:,0]>25)&(im[:,:,2]-im[:,:,1]>15))
if len(xs)>20:
xy=np.column_stack((xs,ys)); vals,vecs=np.linalg.eigh(np.cov(xy.T)); direction=vecs[:,-1]
if direction[1]>0: direction=-direction
_axis=np.array([direction[0],-direction[1]],dtype=np.float32)
bank = _load()
scale = np.tile(np.r_[np.full(7,.15),np.full(16,.3)],2)
d = np.mean(((bank['q']-q)/scale)**2,axis=1)
d += .005 * np.mean(((bank['v']-v)/.5)**2,axis=1)
for i,f in enumerate(visuals):
diff = bank[f'image_{i}']-f
d += .6*np.mean((diff[:,:1728]/.15)**2,axis=1)
d += 2.*np.mean((diff[:,1728:]/.04)**2,axis=1)
if 'pose' not in bank: bank['pose']=blue_pose(bank['image_1'])
d += .5*np.mean(((bank['pose']-blue_pose(visuals[1]))/[.1,.1,.3,.15])**2,axis=1)
# Re-select from all observations at every step. No demo ID/time/seed lookup.
idx = np.argpartition(d, 4)[:5]
idx = idx[np.argsort(d[idx])]
# Avoid blending contradictory modes of coordinated bimanual action.
aa = bank['actions'][idx]
compatible = np.linalg.norm(aa-aa[0],axis=1) < .2
idx = idx[compatible]
weights = np.exp(-(d[idx]-d[idx[0]])/.08)
action = np.sum(bank['actions'][idx]*weights[:,None],axis=0)/weights.sum()
if not np.isfinite(action).all():
return np.zeros(44,dtype=np.float32)
# Faster approach remains re-localized from current observations every step.
action *= 2.0
arm_ids = [0,1,2,22,23,24]
closed = min(np.mean(q[7:23]),np.mean(q[30:46])) > .55
_still = _still + 1 if closed and np.linalg.norm(action[arm_ids]) < .02 else 0
if _still >= 5 and _pull == 0:
_pull = 1
if _pull:
action[arm_ids] = 0.
if _pull <= 35:
action[:2] = .12 * _axis
action[22:24] = -.12 * _axis
_pull += 1
if _pull > 41:
return None
return np.clip(action,-1.,1.).astype(np.float32)
def blue_pose(f):
f=np.atleast_2d(f)
yy,xx=np.mgrid[:32,:32];xy=np.column_stack((xx.ravel()/31,yy.ravel()/31))
w=np.maximum(f[:,1728:]-.015,0);w=w/(w.sum(-1,keepdims=True)+1e-8)
c=w@xy;dx=xy[None,:,:]-c[:,None,:]
cov=np.einsum('ni,nij,nik->njk',w,dx,dx); vals,vecs=np.linalg.eigh(cov)
v=vecs[:,:,-1];v*=np.where(v[:,1:2]<0,-1,1)
return np.column_stack((c,v[:,0],np.sqrt(np.maximum(0,vals[:,-1]))))