"""Task-local nonparametric RGB/proprioception feedback policy. No fitted weights.""" from pathlib import Path import numpy as np import cv2 CAMERAS = ('front_camera', 'left_camera', 'robot_camera', 'top_camera') def visual_feature(image): im = np.asarray(image) 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') small = cv2.resize(im, (16, 12), interpolation=cv2.INTER_AREA).astype(np.float32) / 255. # Orange cloth cue, deliberately separate from the background-heavy thumbnail. hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV) mask = ((hsv[..., 0] >= 3) & (hsv[..., 0] <= 24) & (hsv[..., 1] > 110) & (hsv[..., 2] > 150)).astype(np.float32) grid = cv2.resize(mask, (16, 12), interpolation=cv2.INTER_AREA) return np.concatenate((small.ravel()/np.sqrt(small.size)*2, grid.ravel()/np.sqrt(grid.size)*3)).astype(np.float32) class Controller: def __init__(self): with np.load(Path(__file__).with_name('demo_bank.npz'), allow_pickle=False) as b: self.bank = {k: b[k] for k in b.files} self.visual_flat = self.bank['visual'].reshape(len(self.bank['visual']), -1) self.visual_norm = np.einsum('ij,ij->i', self.visual_flat, self.visual_flat) self.reset_episode() def reset_episode(self): self.end_count = 0 self.invalid_count = 0 self.last_diagnostic = {} self.calls = 0 self.bowl_centers = [] self.servo_steps = 0 def act(self, obs): if int(obs.get('step_id', 1)) >= 800: return None # one runner-owned hold + at most 799 policy actions try: p = obs['proprioception'] q = np.asarray(p['agent/qpos'], dtype=np.float32).reshape(-1) v = np.asarray(p['agent/qvel'], dtype=np.float32).reshape(-1) if q.shape != (23,) or v.shape != (23,) or not np.isfinite(q).all() or not np.isfinite(v).all(): raise ValueError('Invalid proprioception') visual = np.stack([visual_feature(obs['rgb'][c]) for c in CAMERAS]) except (KeyError, ValueError, TypeError): self.invalid_count += 1 return None if self.invalid_count >= 10 else np.zeros(22, dtype=np.float32) self.calls += 1 b = self.bank dq = (b['qpos'] - q) / b['qscale'] dv = (b['qvel'] - v) / .5 qdist = np.mean(dq[:, :7]**2, axis=1) + .4*np.mean(dq[:, 7:]**2, axis=1) vdist = .03*np.mean(dv**2, axis=1) visual_dist = np.maximum(0., (self.visual_norm + np.sum(visual*visual) - 2*np.einsum('ij,j->i', self.visual_flat, visual.ravel(), optimize=False))/len(CAMERAS)) distance = qdist + vdist + 4*visual_dist best = int(np.argmin(distance)) endpoint_close = bool(b['terminal'][best]) and qdist[best] < .15 and visual_dist[best] < .08 if b['terminal'][best] and not endpoint_close: distance[b['terminal']] = np.inf best = int(np.argmin(distance)) frame = int(b['frame_index'][best]) if 35 <= frame <= 125 and self.calls % 10 == 0: top = np.asarray(obs['rgb']['top_camera']).squeeze() gray = cv2.GaussianBlur(cv2.cvtColor(top, cv2.COLOR_RGB2GRAY), (5,5), 1) circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 70, param1=100, param2=30, minRadius=45, maxRadius=75) if circles is not None: valid = [c[:2] for c in circles[0] if 150 9 or visual_dist[best] > 1.5: self.invalid_count += 1 return None if self.invalid_count >= 10 else np.zeros(22, dtype=np.float32) self.invalid_count = 0 terminal = bool(b['terminal'][best]) and qdist[best] < .15 and visual_dist[best] < .08 self.end_count = self.end_count + 1 if terminal else 0 if self.end_count >= 6: return None # endpoint resemblance, NOT a success assertion if terminal: return np.zeros(22, dtype=np.float32) # Blend only nearby samples with the same coarse action direction. a0 = b['actions'][best] indices = np.argsort(distance)[:12] compatible = (~b['terminal'][indices]) & (np.linalg.norm(b['actions'][indices,:3]-a0[:3],axis=1)<.08) indices = indices[compatible][:5] if len(indices) == 0: indices = np.array([best]) weights = np.exp(-(distance[indices]-distance[best])/.06) action = np.average(b['actions'][indices], axis=0, weights=weights) if frame >= 210 and len(self.bowl_centers) >= 3: top = np.asarray(obs['rgb']['top_camera']).squeeze() hsv = cv2.cvtColor(top, cv2.COLOR_RGB2HSV) mask = (hsv[:,:,0]>=3)&(hsv[:,:,0]<=24)&(hsv[:,:,1]>110)&(hsv[:,:,2]>150) yy, xx = np.where(mask) if len(xx) >= 30 and np.mean(yy) < 500: center = np.array([np.mean(xx),np.mean(yy)]) target = np.median(self.bowl_centers, axis=0) error = target-center action[:2] = np.clip(error*np.array([.012,-.012]), -.1, .1) if np.linalg.norm(error) > 35: action[2] = max(0.,action[2]) self.servo_steps += 1 self.last_diagnostic.update(bowl=target.tolist(),cloth=center.tolist(),servo_error=float(np.linalg.norm(error))) return np.clip(action, -1., 1.).astype(np.float32) _policy = None def reset_episode(): global _policy if _policy is None: _policy = Controller() else: _policy.reset_episode() def act(obs): global _policy if _policy is None: reset_episode() return _policy.act(obs)