"""Unrehearsed D20 observation-conditioned action interpolation; no neural model.""" from pathlib import Path import numpy as np import cv2 ROBOTS = ('panda_leaphand-1', 'panda_leaphand_left-0') CAMERAS = ('front_camera', 'top_camera') _memory = None def reset_episode(): """No trajectory index or scene identity is retained between calls/episodes.""" global _initial_target _initial_target = None def image_features(rgb): appearance, targets, visible = [], [], [] for cam in CAMERAS: im = np.asarray(rgb[cam]) 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') appearance.extend((cv2.resize(im, (32, 24), interpolation=cv2.INTER_AREA).astype(np.float32)/255).ravel()) h, w = im.shape[:2] hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV) mask = cv2.inRange(hsv, np.array([95,70,45]), np.array([135,255,255])) # Spatial ordering is image-relative, not a robot/world axis conversion. for half in range(2): region = mask.copy() if cam == 'front_camera': region[:int(.30*h)] = 0 region[int(.82*h):] = 0 region[:, :w//2 if half else 0] = 0 if half == 0: region[:, w//2:] = 0 else: if half == 0: region[h//2:] = 0 else: region[:h//2] = 0 count, labels, stats, centers = cv2.connectedComponentsWithStats(region) ids = [i for i in range(1,count) if stats[i,cv2.CC_STAT_AREA] >= 3] if ids: i = max(ids, key=lambda j: stats[j,cv2.CC_STAT_AREA]) targets.extend(centers[i]/[w,h]); visible.extend([1,1]) else: targets.extend([0,0]); visible.extend([0,0]) return (np.asarray(appearance,np.float32), np.asarray(targets,np.float32), np.asarray(visible,np.float32)) def _load(): global _memory if _memory is None: with np.load(Path(__file__).with_name('demo_memory.npz'), allow_pickle=False) as f: _memory = {k:f[k] for k in f.files} return _memory _initial_target = None _regdata = None def act(obs): global _initial_target, _regdata if int(obs.get('step_id',1)) >= 200: return None out = np.zeros(44,np.float32) try: q = np.concatenate([np.asarray(obs['proprioception']['agent/'+r+'/qpos']).reshape(-1) for r in ROBOTS]) _, target, valid = image_features(obs['rgb']) except (KeyError,ValueError,TypeError): return out if _initial_target is None: if valid.sum()!=8: return out _initial_target = target.copy() m = _load() if _regdata is None: initial = m['target'][m['frame_index']==0] _regdata = initial[m['demo_index']] for arm,ts in [(0,[2,3,4,5]),(1,[0,1,6,7])]: qs=slice(arm*23,arm*23+7) # Locally affine non-neural imitation from measured joints and fixed target localization. dx=np.concatenate([(m['q'][:,qs]-q[qs])/.15,(_regdata[:,ts]-_initial_target[ts])/.025],axis=1) dist=np.mean(dx[:,:7]**2,axis=1)+np.mean(dx[:,7:]**2,axis=1)*2 ids=np.argpartition(dist,119)[:120] x=np.column_stack([np.ones(len(ids)),dx[ids]]) w=np.exp(-(dist[ids]-dist[ids].min())/max(.1,float(np.median(dist[ids])))) reg=np.eye(x.shape[1])*.015;reg[0,0]=.00001 y=m['action'][ids,arm*22:arm*22+3] coef=np.linalg.solve(x.T@(w[:,None]*x)+reg,x.T@(w[:,None]*y)) out[arm*22:arm*22+3]=np.clip(coef[0],-.06,.06) return out