Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
6.45 kB
"""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)