File size: 4,397 Bytes
458748e | 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 | """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)
|