File size: 6,105 Bytes
e25638c | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 | """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)
|