"""D20 observation-conditioned nonparametric controller; E20 terminal guard.""" from pathlib import Path import json import cv2 import numpy as np ROOT = Path(__file__).resolve().parent CONFIG = json.loads((ROOT / 'config.json').read_text()) _bank = None _bad = 0 _terminal = 0 def reset_episode(): global _bad, _terminal _bad = 0 _terminal = 0 def image_feature(rgb): out = [] for camera in CONFIG['cameras']: x = np.asarray(rgb[camera]) if x.ndim == 4 and x.shape[0] == 1: x = x[0] if x.ndim != 3 or x.shape[2] != 3 or x.dtype != np.uint8: raise ValueError('Expected uint8 HWC RGB') # Whole view plus central manipulation area; normalized coordinates. h, w = x.shape[:2] crops = [x, x[int(.3*h):int(.95*h), int(.25*w):int(.85*w)]] out.extend(cv2.resize(c, (24,24), interpolation=cv2.INTER_AREA).astype(np.float32).ravel()/255 for c in crops) return np.concatenate(out) def proprio_feature(proprio): values = [np.asarray(proprio[p], dtype=np.float32).reshape(-1) for p in CONFIG['proprio_paths']] if any(v.shape != (23,) for v in values): raise ValueError('Expected 23 coordinates per proprioceptive field') x = np.concatenate(values) if not np.isfinite(x).all(): raise ValueError('Nonfinite proprioception') return x def act(obs): global _bank, _bad, _terminal if int(obs['step_id']) >= CONFIG['native_horizon']: return None # Initial hold has already consumed native step one. if _bank is None: with np.load(ROOT / 'demo_bank.npz', allow_pickle=False) as data: _bank = {k:data[k] for k in data.files} # Floating-point residue is not a useful motion label. keep = np.max(np.abs(_bank['actions']),axis=1) > 1e-5 for key in ('proprio','visual','actions','source','frame'): _bank[key] = _bank[key][keep] try: q = proprio_feature(obs['proprioception']) v = image_feature(obs['rgb']) except (KeyError, ValueError, TypeError): _bad += 1 return None if _bad >= 10 else np.zeros(44, dtype=np.float32) dq = np.mean(((_bank['proprio']-q)/_bank['scale'])**2, axis=1) # Recompute candidates from present measurements on every call. No scene/seed lookup. n = int(CONFIG.get('neighbors',64)) ids = np.argpartition(dq, min(n-1,len(dq)-1))[:n] dv = np.mean((_bank['visual'][ids]-v)**2, axis=1) scores = dq[ids] + float(CONFIG.get('visual_weight',12))*dv best = int(np.argmin(scores)) if dq[ids[best]] > 9.0 or dv[best] > .10: _bad += 1 return None if _bad >= 10 else np.zeros(44,dtype=np.float32) _bad = 0 # A single joint/visual neighbor keeps the two arms' labels synchronized. i = ids[best] src = _bank['source'][i] last = _bank['frame'][_bank['source']==src].max() _terminal = _terminal + 1 if _bank['frame'][i] >= last-1 else 0 if _terminal >= int(CONFIG.get('terminal_limit',6)): return None action = _bank['actions'][i].copy() return np.clip(action, CONFIG['action_low'], CONFIG['action_high']).astype(np.float32)