"""D20 compiled controller: nonparametric observation-conditioned action lookup.""" from pathlib import Path import numpy as np import cv2 ROOT = Path(__file__).resolve().parent ROBOTS = ('panda_leaphand-1', 'panda_leaphand_left-0') CAMERAS = ('top_camera', 'front_camera') PATHS = tuple('agent/' + r + '/' + k for r in ROBOTS for k in ('qpos', 'qvel')) _bank = None _phase = 0.0 _terminal_count = 0 last_diagnostic = {} def visual_features(rgb): """Fixed, untrained RGB and color occupancy descriptors; no metric calibration.""" out = [] 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[-1] != 3 or im.dtype != np.uint8: raise ValueError('Expected uint8 HWC or 1HWC RGB') f = im.astype(np.float32) / 255.0 r, g, b = f.transpose(2, 0, 1) yellow = ((r > .65) & (g > .52) & (b < .40)).astype(np.float32) white = ((f.min(axis=2) > .65) & ((f.max(axis=2)-f.min(axis=2)) < .18)).astype(np.float32) out.append(cv2.resize(f, (12, 16), interpolation=cv2.INTER_AREA).ravel()) for mask in (yellow, white): out.append(2 * cv2.resize(mask, (12, 16), interpolation=cv2.INTER_AREA).ravel()) return np.concatenate(out).astype(np.float32) def proprio_features(proprioception): parts = [np.asarray(proprioception[p], dtype=np.float32).reshape(-1) for p in PATHS] if any(p.shape != (23,) for p in parts): raise ValueError('Expected 23 joint coordinates/velocities per robot') q = np.concatenate([parts[0], parts[2]]) v = np.concatenate([parts[1], parts[3]]) if not (np.isfinite(q).all() and np.isfinite(v).all()): raise ValueError('Nonfinite proprioception') return q, v def reset_episode(): global _phase, _terminal_count, last_diagnostic _phase = 0.0 _terminal_count = 0 last_diagnostic = {} def act(obs): global _bank, _phase, _terminal_count, last_diagnostic step = int(obs['step_id']) # The runner already spent the initial native step. No additional reset hold. if step >= 400: last_diagnostic = {'stop_reason': 'native_horizon'} return None if _bank is None: with np.load(ROOT / 'demo_bank.npz', allow_pickle=False) as z: _bank = {k: z[k] for k in z.files} try: x = visual_features(obs['rgb']) q, v = proprio_features(obs['proprioception']) except (KeyError, ValueError, TypeError): last_diagnostic = {'stop_reason': 'invalid_required_observation'} return None b = _bank # Scene-sensitive RGB participates on every call; no episode/seed identity lookup. dv = np.mean((b['visual'] - x) ** 2, axis=1) dq = np.mean(((b['q'] - q) / b['q_scale']) ** 2, axis=1) dvel = np.mean(((b['v'] - v) / b['v_scale']) ** 2, axis=1) # A weak local phase prior breaks repeated-state ties. It does not schedule actions. target_phase = min(1.0, _phase + 1 / 300) distance = 16 * dv + .6 * dq + .04 * dvel + .12 * (b['phase'] - target_phase) ** 2 # Prevent confusing initial/released hand configurations; permit slow recovery. eligible = (b['phase'] >= _phase - .08) & (b['phase'] <= _phase + .18) ids = np.flatnonzero(eligible) if len(ids) == 0: ids = np.arange(len(distance)) best = ids[np.argsort(distance[ids])[:5]] w = np.exp(-(distance[best] - distance[best[0]]) / .025) w /= w.sum() observed_phase = float(w @ b['phase'][best]) _phase = float(np.clip(observed_phase, max(0, _phase - .015), min(1, _phase + .025))) # Endpoint similarity is a heuristic, never a success assertion. near_end = _phase > .975 and float(distance[best[0]]) < .12 and np.max(abs(v)) < .3 _terminal_count = _terminal_count + 1 if near_end else 0 last_diagnostic = {'phase': _phase, 'distance': float(distance[best[0]]), 'endpoint_streak': _terminal_count, 'demo': int(b['demo_index'][best[0]]), 'frame': int(b['frame_index'][best[0]])} # Endpoint similarity is recorded only; keep observing through settling. a = w @ b['actions'][best] if a.shape != (44,) or not np.isfinite(a).all(): raise ValueError('Invalid compiled action') # Exact contract concatenation: right-key 22, then left-key 22. return np.clip(a, -1.0, 1.0).astype(np.float32)