"""D20 observation-conditioned lookup with bounded image feedback. No simulator, kinematics package, neural weights, seed, or timed demo replay. """ from pathlib import Path import cv2 import numpy as np CAMERAS = ('top_camera', 'front_camera') _BANK = None _last_action = None _unsupported = 0 _target_y = .5 last_diagnostics = {} def reset_episode(): global _last_action, last_diagnostics, _unsupported, _target_y _last_action = None _unsupported = 0 _target_y = .5 last_diagnostics = {'status': 'reset'} def features(obs): """Fixed hand-designed metric, shared verbatim by compiler and runtime.""" parts = [] blue_counts = [] for camera in CAMERAS: im = np.asarray(obs['rgb'][camera]) if im.ndim == 4 and im.shape[0] == 1: im = im[0] if im.ndim != 3 or im.shape[2] != 3 or im.dtype != np.uint8: raise ValueError('Expected uint8 HWC or 1HWC RGB') r, g, b = [im[..., i].astype(np.float32) for i in range(3)] masks = [(b > 65) & (b > r * 1.6 + 15) & (b > g * 1.4 + 15), (r > 65) & (r > b * 1.5 + 15) & (r > g * 1.4 + 15)] blue_counts.append(int(masks[0].sum())) # Spatial masks preserve visible box shape and target location. Missing # red pixels remain a visibility feature; target may be occluded. for mask in masks: small = cv2.resize(mask.astype(np.float32), (16, 16), interpolation=cv2.INTER_AREA) parts.append(small.ravel() * (8.0 / 16.0)) yy, xx = np.nonzero(mask) if len(xx) >= 8: x, y = xx / im.shape[1], yy / im.shape[0] geom = [x.mean(), y.mean(), x.std(), y.std(), np.mean((x-x.mean())*(y-y.mean())), len(xx)/mask.size, 1.0] else: geom = [0.] * 7 parts.append(np.asarray(geom, np.float32) * np.array([12,12,6,6,20,10,1],np.float32)) small_rgb = cv2.resize(im, (16,16), interpolation=cv2.INTER_AREA).astype(np.float32)/255 parts.append(small_rgb.ravel() * (1.0 / np.sqrt(768))) for kind, scale in [('qpos', .20), ('qvel', 1.0)]: q = [] for robot in range(2): v = np.asarray(obs['proprioception'][f'agent/panda-{robot}/{kind}'],dtype=np.float32).reshape(-1) if v.size != 9 or not np.isfinite(v).all(): raise ValueError('Expected nine finite proprioceptive values per robot') q.extend(v[:7]) parts.append(np.array(q,np.float32)/(scale*np.sqrt(14))) f = np.concatenate(parts).astype(np.float32) if not np.isfinite(f).all(): raise ValueError('Nonfinite features') return f, min(blue_counts) >= 8 def _load(): global _BANK if _BANK is None: with np.load(Path(__file__).with_name('demo_bank.npz'), allow_pickle=False) as d: _BANK = {k: d[k] for k in d.files} return _BANK def box_pose(im): r,g,b=[im[...,i].astype(float) for i in range(3)] mask=((b>65)&(b>1.6*r+15)&(b>1.4*g+15)).astype(np.uint8) contours,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) if not contours:return None contour=max(contours,key=cv2.contourArea) (x,y),(w,h),angle=cv2.minAreaRect(contour) pts=cv2.boxPoints(((x,y),(w,h),angle));edges=np.roll(pts,-1,axis=0)-pts;v=edges[np.argmax(np.linalg.norm(edges,axis=1))] if v[1]<0:v=-v tilt=np.arctan2(v[0],v[1]) return dict(x=float(x/im.shape[1]),y=float(y/im.shape[0]),tilt=float(tilt),area=float(cv2.contourArea(contour)/mask.size)) def act(obs): global _last_action, last_diagnostics, _unsupported, _target_y # Native horizon includes the runner-owned reset hold. No extra hold here. if int(obs.get('step_id', 1)) >= 700: last_diagnostics = {'status': 'native_horizon'} return None try: f, visible = features(obs) except (KeyError, ValueError, TypeError): last_diagnostics = {'status': 'invalid_observation'} return None if _last_action is None: im=np.asarray(obs['rgb']['top_camera']).squeeze().astype(float) r,g,b=im[...,0],im[...,1],im[...,2] yy,xx=np.nonzero((r>65)&(r>1.5*b+15)&(r>1.4*g+15)) if len(yy)>100: _target_y=float((yy.min()+yy.max())/(2*im.shape[0])) bank = _load() distance = np.sum((bank['features']-f)**2,axis=1) idx = np.array([np.where(bank['demo_index']==di)[0][np.argmin(distance[bank['demo_index']==di])] for di in range(20)]) idx = idx[np.argsort(distance[idx])][:5] best = idx[0] # Reject gross extrapolation. This threshold is heuristic, not validated. if not visible or distance[best] > 16.0: action = np.zeros(14,np.float32); action[[6,13]] = -1 last_diagnostics = {'status': 'unsupported_hold', 'squared_distance': float(distance[best])} else: # Avoid averaging opposing contact/repositioning actions. Both robot # commands always come from the same paired observation/action record. compatible = np.max(np.abs(bank['actions'][idx]-bank['actions'][best]),axis=1) <= .12 idx = idx[compatible] weights = np.exp(-(distance[idx]-distance[best])/.35) action = np.average(bank['actions'][idx], axis=0, weights=weights) action = np.clip(action,-1,1).astype(np.float32) last_diagnostics = {'status': 'lookup', 'squared_distance': float(distance[best]), 'reference_demo': int(bank['demo_index'][best]), 'reference_frame': int(bank['frame_index'][best])} pose = box_pose(np.asarray(obs['rgb']['top_camera']).squeeze()) if (last_diagnostics['status']=='lookup' and pose is not None and .54 < pose['x'] < .83 and pose['area'] > .045 and abs(pose['tilt']) < .65 and min(action[0], action[7]) > .035 and min(action[2], action[9]) > -.05): tilt = pose['tilt'] correction = np.clip(.18 * np.sign(tilt)*max(abs(tilt)-.06,0),-.06,.06) action[0] = np.clip(action[0] + correction, 0, .15) action[7] = np.clip(action[7] - correction, 0, .15) last_diagnostics['yaw_correction'] = float(correction) last_diagnostics['box_tilt'] = float(tilt) lateral=float(np.clip(.4*(pose['y']-_target_y),-.035,.035)) action[1] = np.clip(action[1]+lateral,-.15,.15) action[8] = np.clip(action[8]+lateral,-.15,.15) last_diagnostics['lateral_correction'] = lateral _unsupported = _unsupported+1 if last_diagnostics['status']=='unsupported_hold' else 0 if _unsupported >= 40: last_diagnostics['status']='unsupported_abort' return None _last_action = action.copy() return action