Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
10.8 kB
"""D20 observation-conditioned exemplar controller with bounded grasp recovery; no learned network."""
from pathlib import Path
import numpy as np
import cv2
CAMERAS = ('front_camera', 'left_camera', 'robot_camera_0', 'robot_camera_1', 'top_camera')
PATHS = tuple(f'agent/mobile_panda-{r}/{kind}' for r in (0, 1) for kind in ('qpos', 'qvel'))
# Order in the contract: robot 0 [base(3), arm(6), gripper], then robot 1.
BASE = np.array([0, 1, 2, 10, 11, 12])
NORMALIZED = np.array([3,4,5,6,7,8,9,13,14,15,16,17,18,19])
_bank = None
_scene_ready = False
_object_votes = []
_object_mode = None
_object_centers = []
_object_adjusted = False
_scene_target = None
_grasp_settle = 0
_grasp_target = None
_grasp_retry = False
_progress = 0.0
_terminal_count = 0
_last_step = 0
_last_action = None
def visual_feature(rgb):
parts = []
for camera in CAMERAS:
a = np.asarray(rgb[camera])
if a.ndim == 4 and a.shape[0] == 1:
a = a[0]
if a.ndim != 3 or a.shape[2] != 3 or a.dtype != np.uint8:
raise ValueError('Expected uint8 HWC or 1HWC RGB')
parts.append(cv2.resize(a, (16, 12), interpolation=cv2.INTER_AREA).reshape(-1))
return np.concatenate(parts)
def reset_episode():
global _progress, _terminal_count, _last_step, _last_action, _bank, _scene_ready, _object_votes, _object_mode
global _grasp_settle, _grasp_target, _grasp_retry
_grasp_target, _grasp_retry = None, False
_grasp_settle = 0
global _object_centers, _object_adjusted, _scene_target
_object_centers, _object_adjusted, _scene_target = [], False, None
_bank, _scene_ready = None, False
_object_votes, _object_mode = [], None
_progress, _terminal_count, _last_step, _last_action = 0.0, 0, 0, None
def _load():
global _bank
if _bank is None:
with np.load(Path(__file__).with_name('exemplars.npz'), allow_pickle=False) as f:
_bank = {k: f[k] for k in f.files}
return _bank
def localize_scene(obs, b):
global _scene_ready, _scene_target
if _scene_ready: return b
a = np.asarray(obs['rgb']['top_camera']).squeeze()
a = cv2.resize(a,(240,320),interpolation=cv2.INTER_AREA)
mask = (np.min(a,axis=2)>225).astype('uint8'); mask[130:]=0
n, labels, stats, cent = cv2.connectedComponentsWithStats(mask)
if n < 2: return b
i = 1+np.argmax(stats[1:,4]); x,y,w,h,area=stats[i]
if area<500 or w<20: return b
g=np.load(Path(__file__).with_name('geometry.npz'))
target=np.array([x+w/2,y+h,1])@g['W']
_scene_target=target.copy()
offsets=target-g['targets'][b['demo'].astype(int)]
ph=b['phase']
f0=np.minimum(ph/.5,1)*np.clip((4-ph)/.45,0,1)
f1=np.clip((ph-1.42)/.4,0,1)
for j in (0,1):
b['q'][:,j] += offsets[:,j]*f0
b['a'][:,j] += offsets[:,j]*f0
b['q'][:,12+j] += offsets[:,j]*f1
b['a'][:,10+j] += offsets[:,j]*f1
_scene_ready=True
return b
def detect_object(obs):
global _object_mode, _object_votes
if not (1.98 < _progress < 2.5): return
a=cv2.resize(np.asarray(obs['rgb']['front_camera']).squeeze(),(320,240),interpolation=cv2.INTER_AREA).astype(float)
r,g,bl=a.transpose(2,0,1)
mask=(bl>r+3)&(bl>g+2)&(r<60)&(g<60)&(bl<100)
mask[:60]=False;mask[132:]=False;mask[:,:110]=False;mask[:,185:]=False
y,x=np.where(mask)
if len(y)>=5:
_object_centers.append([float(np.median(x)),float(np.median(y))])
_object_votes.append(int(np.median(y)<110))
if len(_object_votes)>=3: _object_mode=int(np.mean(_object_votes[-15:])>=.5)
def refine_object(b):
global _object_adjusted
if _object_adjusted or _object_mode is None or _scene_target is None or _progress<2.25: return
og=np.load(Path(__file__).with_name('object_geometry.npz'))
center=np.median(_object_centers[-15:],axis=0)
target=np.r_[center,_scene_target,1]@og['weights'][_object_mode]
demos=np.flatnonzero(og['mode']==_object_mode)
# Restrict interpolation to the observed within-shelf pose envelope.
target[3:]=np.clip(target[3:],og['targets'][demos,3:].min(0),og['targets'][demos,3:].max(0))
target[:3]=np.clip(target[:3],b['base_low'][:3],b['base_high'][:3])
for di in demos:
ids=np.flatnonzero(b['demo']==di)
grasp=ids[np.flatnonzero(b['phase'][ids]>=3)[0]]
offset=target-b['q'][grasp,:10]
f=np.clip((b['phase'][ids]-2.08)/.62,0,1)*np.clip((3.6-b['phase'][ids])/.4,0,1)
b['q'][ids,:10]+=f[:,None]*offset
b['a'][ids,:3]+=f[:,None]*offset[:3]
_object_adjusted=True
def _hold(q):
a = np.zeros(20, dtype=np.float32)
for r in (0, 1):
a[10*r:10*r+3] = q[12*r:12*r+3]
a[10*r+9] = np.clip(2*(q[12*r+10]+0.01)/0.05-1, -1, 1)
return a
def act(obs):
"""Return a 20-vector or None; step 1 follows the runner-owned hold.
Base bounds are absent in the supplied contract. Base outputs lie within
the observed demonstration envelope, except measured-position fallback
holds. Neither constitutes verification of unknown native limits.
"""
global _progress, _terminal_count, _last_step, _last_action, _grasp_settle, _grasp_target, _grasp_retry
step = int(obs.get('step_id', 1))
if step < _last_step:
reset_episode()
if step == _last_step and _last_action is not None:
return _last_action.copy()
# 650 includes runner's initial hold: at most 649 controller actions.
if step >= 650:
return None
try:
p = obs['proprioception']
arrays = [np.asarray(p[path], dtype=np.float32).reshape(-1) for path in PATHS]
if any(a.size != 12 or not np.isfinite(a).all() for a in arrays):
return None
q = np.concatenate((arrays[0], arrays[2]))
v = np.concatenate((arrays[1], arrays[3]))
except (KeyError, TypeError, ValueError):
return None
b = _load()
try: b = localize_scene(obs,b)
except (KeyError, ValueError, cv2.error): pass
hold = _hold(q)
try:
image = visual_feature(obs['rgb']).astype(np.float32)/255.0
except (KeyError, TypeError, ValueError, cv2.error):
_last_step, _last_action = step, hold
return hold.copy()
try: detect_object(obs)
except (KeyError,ValueError,cv2.error): pass
refine_object(b)
# Progress is inferred from current observations across all 20 demos;
# it is never set from an episode ID, hidden seed, or step schedule.
eligible = np.flatnonzero((b['phase'] >= _progress-0.015) &
(b['phase'] <= _progress+0.45))
if _object_mode is not None and _progress>2.08:
og=np.load(Path(__file__).with_name('object_geometry.npz'))
eligible=eligible[og['mode'][b['demo'][eligible].astype(int)]==_object_mode]
dq = (b['q'][eligible]-q)/b['qscale']
qcost = np.mean(np.minimum(dq*dq, 100), axis=1)
dv = (b['v'][eligible]-v)/b['vscale']
vcost = np.mean(np.minimum(dv*dv, 25), axis=1)
preliminary = qcost + 0.03*vcost
k = min(96, len(eligible))
short = np.argpartition(preliminary, k-1)[:k]
ids = eligible[short]
visual = np.mean((b['rgb'][ids].astype(np.float32)/255.0-image)**2, axis=1)
continuity = 0.12*(b['phase'][ids]-(_progress+0.025))**2
cost = preliminary[short] + 12*visual + continuity
best_local = int(np.argmin(cost)); best = ids[best_local]
# Conservative, uncalibrated rejection; does not certify safety/success.
if qcost[short[best_local]] > 2.5 or visual[best_local] > 0.09:
_terminal_count = 0
_last_step, _last_action = step, hold
return hold.copy()
_progress = min(4.99, max(_progress + 0.004, float(b['phase'][best])))
# Never blend conflicting gripper commands or widely separated phases.
compatible = (np.all(b['a'][ids][:,[9,19]] == b['a'][best,[9,19]], axis=1) &
(np.abs(b['phase'][ids]-_progress) < 0.045))
selected = np.flatnonzero(compatible)
selected = selected[np.argsort(cost[selected])[:4]]
weights = np.exp(-(cost[selected]-cost[selected].min())/0.035)
weights /= weights.sum()
# Small demonstrated lookahead avoids stationary nearest-neighbor fixed points.
future = np.minimum(ids[selected]+2, len(b['a'])-1)
future = np.where(b['demo'][future] == b['demo'][ids[selected]], future, ids[selected])
a = weights @ b['a'][future]
a[[9,19]] = b['a'][best,[9,19]]
if 2.05 < _progress < 3.5:
pm=np.load(Path(__file__).with_name('pose_maps.npz'))
mi=int(np.argmin(abs(pm['centers']-_progress)))
qref=weights @ b['q'][ids[selected]][:,3:10]
correction=(qref-q[3:10])@pm['maps'][mi]
a[3:9] += np.clip(correction,[-.04,-.04,-.04,-.02,-.02,-.02],[.04,.04,.04,.02,.02,.02])
# Advance demonstrated support-base placement while the handle is opening.
if 1.35 < _progress < 1.99:
support=np.flatnonzero((b['demo']==b['demo'][best])&(b['phase']>=min(1.99,_progress+.18)))
if len(support): a[10:13]=b['a'][support[0],10:13]
# Fixed grasp-pose servo, followed by closure dwell and one empty-grasp retry.
if 2.96 < _progress < 3.18 and _grasp_settle < 50 and _object_mode is not None:
if _grasp_target is None:
og=np.load(Path(__file__).with_name('object_geometry.npz'))
gi=np.flatnonzero((b['phase']>=3.03)&(b['phase']<=3.06)&(og['mode'][b['demo'].astype(int)]==_object_mode))
err=np.mean(((b['q'][gi,:10]-q[:10])/b['qscale'][:10])**2,axis=1)
_grasp_target=b['q'][gi[np.argmin(err)]].copy()
ref=_grasp_target
pm=np.load(Path(__file__).with_name('pose_maps.npz'))
mi=int(np.argmin(abs(pm['centers']-3)))
a[:3]=ref[:3]
a[3:9]=np.clip(2*(ref[3:10]-q[3:10])@pm['maps'][mi],-.1,.1)
a[9]=1 if _grasp_settle<35 else -1
_progress=2.97
_grasp_settle+=1
if _grasp_settle==50:
if q[10]<.004 and not _grasp_retry:
_grasp_retry=True
_grasp_target[0]-=.025
_grasp_settle=15
else: _progress=3.06
# Base targets remain absolute. Rate limit distance from measured base.
current_base = q[[0,1,2,12,13,14]]
a[BASE] = current_base + np.clip(a[BASE]-current_base,
-b['base_step_cap'], b['base_step_cap'])
a[BASE] = np.clip(a[BASE], b['base_low'], b['base_high'])
a[NORMALIZED] = np.clip(a[NORMALIZED], -1, 1)
# Terminal similarity is only a stop heuristic, never a success signal.
if _progress > 4.85 and a[9] > 0 and visual[best_local] < 0.012 and qcost[short[best_local]] < 0.15:
_terminal_count += 1
else:
_terminal_count = 0
if _terminal_count >= 8:
return None
_last_step, _last_action = step, a.astype(np.float32)
return _last_action.copy()