"""D20 nonparametric feedback policy with bounded phase progression. No environment imports.""" from pathlib import Path import numpy as np import cv2 CAMERAS = ('top_camera', 'front_camera') QPATHS = tuple(f'agent/panda_leaphand-{r}/qpos' for r in range(2)) VPATHS = tuple(f'agent/panda_leaphand-{r}/qvel' for r in range(2)) def visual_features(rgb): features = [] 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[2] != 3 or im.dtype != np.uint8: raise ValueError('Expected uint8 RGB HWC or 1HWC') hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV) # Colored objects retain more weight than the neutral table and arms. mask = ((hsv[:,:,1] > 150) & (hsv[:,:,2] > 70)).astype(np.float32) color = im.astype(np.float32) / 255. features.append(cv2.resize(color, (16,16), interpolation=cv2.INTER_AREA).ravel()) features.append(3 * cv2.resize(color * mask[:,:,None], (24,24), interpolation=cv2.INTER_AREA).ravel()) return np.concatenate(features).astype(np.float32) class Controller: SPEED=1.6 ARM_GAIN=.15 HAND_GAIN=2. def __init__(self): with np.load(Path(__file__).with_name('memory.npz'), allow_pickle=False) as data: self.bank = {key: data[key] for key in data.files} self.inverse = np.load(Path(__file__).with_name('feedback.npz'))['inverse'] self.scale = np.tile(np.r_[np.full(7,.20), np.full(16,.35)],2).astype(np.float32) self.reset_episode() def reset_episode(self): self.t = 0 self.last_phase = None self.last_step = None self.diagnostics = {'status':'reset'} def act(self, obs): self.t += 1 step = int(obs.get('step_id', self.t)) obs = obs.get('observation', obs) if self.last_step is not None and step < self.last_step: self.reset_episode() self.last_step = step # One native step was already consumed by the runner-owned hold. if step >= int(np.ceil(1559/self.SPEED))+41: self.diagnostics = {'status':'horizon_stop', 'success_claim':False} return None try: p = obs['proprioception'] q = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in QPATHS]) v = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in VPATHS]) if q.shape != (46,) or v.shape != (46,) or not np.isfinite(q).all() or not np.isfinite(v).all(): raise ValueError('Malformed proprioception') f = visual_features(obs['rgb']) except (KeyError, ValueError, TypeError, cv2.error): self.diagnostics = {'status':'missing_or_invalid_observation'} return np.zeros(44,dtype=np.float32) if (step-1)*self.SPEED >= 1559: a = np.zeros(44,dtype=np.float32) a[2] = a[24] = .04 self.diagnostics = {'status':'retract_for_visibility'} return a b = self.bank qcost = np.mean(((b['q']-q)/self.scale)**2,axis=1) vcost = .025*np.mean(np.minimum((b['v']-v)**2,25),axis=1) base = qcost + vcost # Timing resolves stationary/contact phase ambiguity seen in episode 1. expected = min((step - 1)*self.SPEED, 1558) base += .5*((b['phase']-expected)/8.)**2 # Soft continuity inferred from previous observed state, never demo ID or seed. if self.last_phase is not None: base += .08*np.minimum(((b['phase']-self.last_phase)/100.)**2,4.) ids = np.argpartition(base,min(127,len(base)-1))[:128] visual = np.mean((b['features'][ids]-f)**2,axis=1) score = base[ids] + 12*visual j = int(ids[np.argmin(score)]) if qcost[j] > 4.0 or float(visual[np.argmin(score)]) > .15: self.diagnostics = {'status':'outside_demo_support', 'qcost':float(qcost[j])} return np.zeros(44,dtype=np.float32) self.last_phase = float(b['phase'][j]) self.diagnostics = {'status':'retrieved_feedback','reference_phase':self.last_phase,'reference_demo':int(b['demo'][j]),'qcost':float(qcost[j]),'success_claim':False} # Jointly retrieve both robots to preserve the demonstrated coordination. # Parts are [robot 0: 6 arm + 16 hand, robot 1: 6 arm + 16 hand]. a = self.SPEED * b['actions'][j].copy() errors = b['q'][j] - q for r in range(2): correction = self.ARM_GAIN * errors[r*23:r*23+7] @ self.inverse[min(int(expected)//4,390),r] correction = np.clip(correction,-.03,.03) a[np.array([0,1,2,4])+r*22] += correction hand_a = np.array([0,1,2,3,12,14,15]) + r*22+6 hand_q = np.array([11,7,15,19,10,18,22]) + r*23 a[hand_a] += np.clip(self.HAND_GAIN*errors[hand_q],-.1,.1) self.diagnostics['pose_error'] = float(np.linalg.norm(errors)) return np.clip(a,-1.,1.).astype(np.float32) _policy = None def reset_episode(): global _policy if _policy is not None: _policy.reset_episode() def act(obs): global _policy if _policy is None: _policy = Controller() return _policy.act(obs)