"""D20 observation-conditioned exemplar controller with bounded grasp recovery; no learned network.""" from pathlib import Path import numpy as np import cv2 CAMERAS = ('front_camera', 'left_camera', 'robot_camera_0', 'robot_camera_1', 'top_camera') PATHS = tuple(f'agent/mobile_panda-{r}/{kind}' for r in (0, 1) for kind in ('qpos', 'qvel')) # Order in the contract: robot 0 [base(3), arm(6), gripper], then robot 1. BASE = np.array([0, 1, 2, 10, 11, 12]) NORMALIZED = np.array([3,4,5,6,7,8,9,13,14,15,16,17,18,19]) _bank = None _scene_ready = False _object_votes = [] _object_mode = None _object_centers = [] _object_adjusted = False _scene_target = None _grasp_settle = 0 _grasp_target = None _grasp_retry = False _progress = 0.0 _terminal_count = 0 _last_step = 0 _last_action = None def visual_feature(rgb): parts = [] for camera in CAMERAS: a = np.asarray(rgb[camera]) if a.ndim == 4 and a.shape[0] == 1: a = a[0] if a.ndim != 3 or a.shape[2] != 3 or a.dtype != np.uint8: raise ValueError('Expected uint8 HWC or 1HWC RGB') parts.append(cv2.resize(a, (16, 12), interpolation=cv2.INTER_AREA).reshape(-1)) return np.concatenate(parts) def reset_episode(): global _progress, _terminal_count, _last_step, _last_action, _bank, _scene_ready, _object_votes, _object_mode global _grasp_settle, _grasp_target, _grasp_retry _grasp_target, _grasp_retry = None, False _grasp_settle = 0 global _object_centers, _object_adjusted, _scene_target _object_centers, _object_adjusted, _scene_target = [], False, None _bank, _scene_ready = None, False _object_votes, _object_mode = [], None _progress, _terminal_count, _last_step, _last_action = 0.0, 0, 0, None def _load(): global _bank if _bank is None: with np.load(Path(__file__).with_name('exemplars.npz'), allow_pickle=False) as f: _bank = {k: f[k] for k in f.files} return _bank def localize_scene(obs, b): global _scene_ready, _scene_target if _scene_ready: return b a = np.asarray(obs['rgb']['top_camera']).squeeze() a = cv2.resize(a,(240,320),interpolation=cv2.INTER_AREA) mask = (np.min(a,axis=2)>225).astype('uint8'); mask[130:]=0 n, labels, stats, cent = cv2.connectedComponentsWithStats(mask) if n < 2: return b i = 1+np.argmax(stats[1:,4]); x,y,w,h,area=stats[i] if area<500 or w<20: return b g=np.load(Path(__file__).with_name('geometry.npz')) target=np.array([x+w/2,y+h,1])@g['W'] _scene_target=target.copy() offsets=target-g['targets'][b['demo'].astype(int)] ph=b['phase'] f0=np.minimum(ph/.5,1)*np.clip((4-ph)/.45,0,1) f1=np.clip((ph-1.42)/.4,0,1) for j in (0,1): b['q'][:,j] += offsets[:,j]*f0 b['a'][:,j] += offsets[:,j]*f0 b['q'][:,12+j] += offsets[:,j]*f1 b['a'][:,10+j] += offsets[:,j]*f1 _scene_ready=True return b def detect_object(obs): global _object_mode, _object_votes if not (1.98 < _progress < 2.5): return a=cv2.resize(np.asarray(obs['rgb']['front_camera']).squeeze(),(320,240),interpolation=cv2.INTER_AREA).astype(float) r,g,bl=a.transpose(2,0,1) mask=(bl>r+3)&(bl>g+2)&(r<60)&(g<60)&(bl<100) mask[:60]=False;mask[132:]=False;mask[:,:110]=False;mask[:,185:]=False y,x=np.where(mask) if len(y)>=5: _object_centers.append([float(np.median(x)),float(np.median(y))]) _object_votes.append(int(np.median(y)<110)) if len(_object_votes)>=3: _object_mode=int(np.mean(_object_votes[-15:])>=.5) def refine_object(b): global _object_adjusted if _object_adjusted or _object_mode is None or _scene_target is None or _progress<2.25: return og=np.load(Path(__file__).with_name('object_geometry.npz')) center=np.median(_object_centers[-15:],axis=0) target=np.r_[center,_scene_target,1]@og['weights'][_object_mode] demos=np.flatnonzero(og['mode']==_object_mode) # Restrict interpolation to the observed within-shelf pose envelope. target[3:]=np.clip(target[3:],og['targets'][demos,3:].min(0),og['targets'][demos,3:].max(0)) target[:3]=np.clip(target[:3],b['base_low'][:3],b['base_high'][:3]) for di in demos: ids=np.flatnonzero(b['demo']==di) grasp=ids[np.flatnonzero(b['phase'][ids]>=3)[0]] offset=target-b['q'][grasp,:10] f=np.clip((b['phase'][ids]-2.08)/.62,0,1)*np.clip((3.6-b['phase'][ids])/.4,0,1) b['q'][ids,:10]+=f[:,None]*offset b['a'][ids,:3]+=f[:,None]*offset[:3] _object_adjusted=True def _hold(q): a = np.zeros(20, dtype=np.float32) for r in (0, 1): a[10*r:10*r+3] = q[12*r:12*r+3] a[10*r+9] = np.clip(2*(q[12*r+10]+0.01)/0.05-1, -1, 1) return a def act(obs): """Return a 20-vector or None; step 1 follows the runner-owned hold. Base bounds are absent in the supplied contract. Base outputs lie within the observed demonstration envelope, except measured-position fallback holds. Neither constitutes verification of unknown native limits. """ global _progress, _terminal_count, _last_step, _last_action, _grasp_settle, _grasp_target, _grasp_retry step = int(obs.get('step_id', 1)) if step < _last_step: reset_episode() if step == _last_step and _last_action is not None: return _last_action.copy() # 650 includes runner's initial hold: at most 649 controller actions. if step >= 650: return None try: p = obs['proprioception'] arrays = [np.asarray(p[path], dtype=np.float32).reshape(-1) for path in PATHS] if any(a.size != 12 or not np.isfinite(a).all() for a in arrays): return None q = np.concatenate((arrays[0], arrays[2])) v = np.concatenate((arrays[1], arrays[3])) except (KeyError, TypeError, ValueError): return None b = _load() try: b = localize_scene(obs,b) except (KeyError, ValueError, cv2.error): pass hold = _hold(q) try: image = visual_feature(obs['rgb']).astype(np.float32)/255.0 except (KeyError, TypeError, ValueError, cv2.error): _last_step, _last_action = step, hold return hold.copy() try: detect_object(obs) except (KeyError,ValueError,cv2.error): pass refine_object(b) # Progress is inferred from current observations across all 20 demos; # it is never set from an episode ID, hidden seed, or step schedule. eligible = np.flatnonzero((b['phase'] >= _progress-0.015) & (b['phase'] <= _progress+0.45)) if _object_mode is not None and _progress>2.08: og=np.load(Path(__file__).with_name('object_geometry.npz')) eligible=eligible[og['mode'][b['demo'][eligible].astype(int)]==_object_mode] dq = (b['q'][eligible]-q)/b['qscale'] qcost = np.mean(np.minimum(dq*dq, 100), axis=1) dv = (b['v'][eligible]-v)/b['vscale'] vcost = np.mean(np.minimum(dv*dv, 25), axis=1) preliminary = qcost + 0.03*vcost k = min(96, len(eligible)) short = np.argpartition(preliminary, k-1)[:k] ids = eligible[short] visual = np.mean((b['rgb'][ids].astype(np.float32)/255.0-image)**2, axis=1) continuity = 0.12*(b['phase'][ids]-(_progress+0.025))**2 cost = preliminary[short] + 12*visual + continuity best_local = int(np.argmin(cost)); best = ids[best_local] # Conservative, uncalibrated rejection; does not certify safety/success. if qcost[short[best_local]] > 2.5 or visual[best_local] > 0.09: _terminal_count = 0 _last_step, _last_action = step, hold return hold.copy() _progress = min(4.99, max(_progress + 0.004, float(b['phase'][best]))) # Never blend conflicting gripper commands or widely separated phases. compatible = (np.all(b['a'][ids][:,[9,19]] == b['a'][best,[9,19]], axis=1) & (np.abs(b['phase'][ids]-_progress) < 0.045)) selected = np.flatnonzero(compatible) selected = selected[np.argsort(cost[selected])[:4]] weights = np.exp(-(cost[selected]-cost[selected].min())/0.035) weights /= weights.sum() # Small demonstrated lookahead avoids stationary nearest-neighbor fixed points. future = np.minimum(ids[selected]+2, len(b['a'])-1) future = np.where(b['demo'][future] == b['demo'][ids[selected]], future, ids[selected]) a = weights @ b['a'][future] a[[9,19]] = b['a'][best,[9,19]] if 2.05 < _progress < 3.5: pm=np.load(Path(__file__).with_name('pose_maps.npz')) mi=int(np.argmin(abs(pm['centers']-_progress))) qref=weights @ b['q'][ids[selected]][:,3:10] correction=(qref-q[3:10])@pm['maps'][mi] a[3:9] += np.clip(correction,[-.04,-.04,-.04,-.02,-.02,-.02],[.04,.04,.04,.02,.02,.02]) # Advance demonstrated support-base placement while the handle is opening. if 1.35 < _progress < 1.99: support=np.flatnonzero((b['demo']==b['demo'][best])&(b['phase']>=min(1.99,_progress+.18))) if len(support): a[10:13]=b['a'][support[0],10:13] # Fixed grasp-pose servo, followed by closure dwell and one empty-grasp retry. if 2.96 < _progress < 3.18 and _grasp_settle < 50 and _object_mode is not None: if _grasp_target is None: og=np.load(Path(__file__).with_name('object_geometry.npz')) gi=np.flatnonzero((b['phase']>=3.03)&(b['phase']<=3.06)&(og['mode'][b['demo'].astype(int)]==_object_mode)) err=np.mean(((b['q'][gi,:10]-q[:10])/b['qscale'][:10])**2,axis=1) _grasp_target=b['q'][gi[np.argmin(err)]].copy() ref=_grasp_target pm=np.load(Path(__file__).with_name('pose_maps.npz')) mi=int(np.argmin(abs(pm['centers']-3))) a[:3]=ref[:3] a[3:9]=np.clip(2*(ref[3:10]-q[3:10])@pm['maps'][mi],-.1,.1) a[9]=1 if _grasp_settle<35 else -1 _progress=2.97 _grasp_settle+=1 if _grasp_settle==50: if q[10]<.004 and not _grasp_retry: _grasp_retry=True _grasp_target[0]-=.025 _grasp_settle=15 else: _progress=3.06 # Base targets remain absolute. Rate limit distance from measured base. current_base = q[[0,1,2,12,13,14]] a[BASE] = current_base + np.clip(a[BASE]-current_base, -b['base_step_cap'], b['base_step_cap']) a[BASE] = np.clip(a[BASE], b['base_low'], b['base_high']) a[NORMALIZED] = np.clip(a[NORMALIZED], -1, 1) # Terminal similarity is only a stop heuristic, never a success signal. if _progress > 4.85 and a[9] > 0 and visual[best_local] < 0.012 and qcost[short[best_local]] < 0.15: _terminal_count += 1 else: _terminal_count = 0 if _terminal_count >= 8: return None _last_step, _last_action = step, a.astype(np.float32) return _last_action.copy()