"""D20 observation-conditioned retrieval controller. No simulator or weights.""" from pathlib import Path import numpy as np import cv2 CAMERAS = ('top_camera', 'front_camera') def image_feature(rgb): parts = [] for camera in CAMERAS: im = np.asarray(rgb[camera]) 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]) < 2: raise ValueError('Expected RGB HWC or 1HWC') if not np.isfinite(im).all(): raise ValueError('Nonfinite RGB') parts.append(cv2.resize(im.astype(np.float32), (16, 16), interpolation=cv2.INTER_AREA).ravel() / 255.) return np.concatenate(parts) _bank = None _phase = None _last_step = None _diagnostics = {} def reset_episode(): global _phase, _last_step, _diagnostics _phase = None _last_step = None _diagnostics = {} def _load(): global _bank if _bank is None: with np.load(Path(__file__).with_name('reference_bank.npz'), allow_pickle=False) as f: _bank = {k: f[k] for k in f.files} return _bank def act(obs): """Return 6 root-frame arm deltas then 16 dex deltas, in one 22-vector. All retrieval is recomputed from live legal observations. Temporal continuity is a soft phase prior, never a selected-demo playback cursor. None means horizon exhaustion or unusable input, and does not assert task completion. """ global _phase, _last_step, _diagnostics if 'observation' in obs: envelope = obs obs = dict(envelope['observation']) obs['step_id'] = envelope.get('step_id', 1 if _last_step is None else _last_step + 1) step = int(obs.get('step_id', 1 if _last_step is None else _last_step + 1)) if step >= 500: return None # Runner already charged one native hold step. if _last_step is not None and step < _last_step: reset_episode() try: 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.size != 23 or v.size != 23 or not np.isfinite(q).all() or not np.isfinite(v).all(): return None visual = image_feature(obs['rgb']) except (KeyError, ValueError, TypeError): return None bank = _load() # Coarse proprioceptive candidates; current RGB reranks every invocation. dq = (bank['q'] - q) / bank['q_scale'] dv = (bank['v'] - v) / bank['v_scale'] cost = np.mean(dq[:, :7] ** 2, axis=1) + .6 * np.mean(dq[:, 7:] ** 2, axis=1) cost += .08 * np.mean(np.minimum(dv ** 2, 25), axis=1) if _phase is not None: # A bounded one-step expectation resolves nearly stationary grasp stages. cost += .35 * (bank['phase'] - _phase) ** 2 pool = np.argpartition(cost, min(95, len(cost)-1))[:96] image_error = np.mean((bank['visual'][pool] - visual) ** 2, axis=1) score = cost[pool] + 80. * image_error order = np.argsort(score) anchor = pool[order[0]] # Do not average actions from opposite sides of a grasp transition. compatible = pool[np.floor(bank['phase'][pool]) == np.floor(bank['phase'][anchor])] vis = np.mean((bank['visual'][compatible] - visual) ** 2, axis=1) total = cost[compatible] + 80. * vis chosen = np.argsort(total)[:5] ids = compatible[chosen] weights = np.exp(-(total[chosen] - total[chosen[0]]) / .08) weights /= weights.sum() action = weights @ bank['actions'][ids] estimate = float(weights @ bank['phase'][ids]) increment = float(weights @ bank['phase_rate'][ids]) if _phase is None: _phase = estimate + increment else: _phase = float(np.clip(.8*_phase + .2*estimate + increment, 0, 7.99)) _last_step = step _diagnostics = {'phase': _phase, 'reference_distance': float(total[chosen[0]]), 'visual_error': float(image_error[order[0]])} if 5.0 <= _phase < 6.0: action[6:] *= .5 # Exact action-part order: one width-22 part, no mimic gripper component. return np.clip(action, -1., 1.).astype(np.float32)