File size: 6,445 Bytes
20033c3 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 | """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<c[0]<380 and 240<c[1]<440 and 50<c[2]<69]
if len(valid) == 1:
self.bowl_centers.append(valid[0])
self.last_diagnostic = {'distance': float(distance[best]), 'reference_row': best,
'visual_distance': float(visual_dist[best]),
'pose_distance': float(qdist[best]),
'demo': int(b['demo_index'][best]),
'frame': int(b['frame_index'][best])}
# Unvalidated conservative out-of-support gate; no blind replay fallback.
if qdist[best] > 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)
|