"""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)