File size: 6,688 Bytes
746ed81 | 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 133 134 135 136 137 138 139 140 141 142 143 144 145 146 | """D20 observation-conditioned lookup with bounded image feedback.
No simulator, kinematics package, neural weights, seed, or timed demo replay.
"""
from pathlib import Path
import cv2
import numpy as np
CAMERAS = ('top_camera', 'front_camera')
_BANK = None
_last_action = None
_unsupported = 0
_target_y = .5
last_diagnostics = {}
def reset_episode():
global _last_action, last_diagnostics, _unsupported, _target_y
_last_action = None
_unsupported = 0
_target_y = .5
last_diagnostics = {'status': 'reset'}
def features(obs):
"""Fixed hand-designed metric, shared verbatim by compiler and runtime."""
parts = []
blue_counts = []
for camera in CAMERAS:
im = np.asarray(obs['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 HWC or 1HWC RGB')
r, g, b = [im[..., i].astype(np.float32) for i in range(3)]
masks = [(b > 65) & (b > r * 1.6 + 15) & (b > g * 1.4 + 15),
(r > 65) & (r > b * 1.5 + 15) & (r > g * 1.4 + 15)]
blue_counts.append(int(masks[0].sum()))
# Spatial masks preserve visible box shape and target location. Missing
# red pixels remain a visibility feature; target may be occluded.
for mask in masks:
small = cv2.resize(mask.astype(np.float32), (16, 16), interpolation=cv2.INTER_AREA)
parts.append(small.ravel() * (8.0 / 16.0))
yy, xx = np.nonzero(mask)
if len(xx) >= 8:
x, y = xx / im.shape[1], yy / im.shape[0]
geom = [x.mean(), y.mean(), x.std(), y.std(),
np.mean((x-x.mean())*(y-y.mean())), len(xx)/mask.size, 1.0]
else:
geom = [0.] * 7
parts.append(np.asarray(geom, np.float32) * np.array([12,12,6,6,20,10,1],np.float32))
small_rgb = cv2.resize(im, (16,16), interpolation=cv2.INTER_AREA).astype(np.float32)/255
parts.append(small_rgb.ravel() * (1.0 / np.sqrt(768)))
for kind, scale in [('qpos', .20), ('qvel', 1.0)]:
q = []
for robot in range(2):
v = np.asarray(obs['proprioception'][f'agent/panda-{robot}/{kind}'],dtype=np.float32).reshape(-1)
if v.size != 9 or not np.isfinite(v).all():
raise ValueError('Expected nine finite proprioceptive values per robot')
q.extend(v[:7])
parts.append(np.array(q,np.float32)/(scale*np.sqrt(14)))
f = np.concatenate(parts).astype(np.float32)
if not np.isfinite(f).all():
raise ValueError('Nonfinite features')
return f, min(blue_counts) >= 8
def _load():
global _BANK
if _BANK is None:
with np.load(Path(__file__).with_name('demo_bank.npz'), allow_pickle=False) as d:
_BANK = {k: d[k] for k in d.files}
return _BANK
def box_pose(im):
r,g,b=[im[...,i].astype(float) for i in range(3)]
mask=((b>65)&(b>1.6*r+15)&(b>1.4*g+15)).astype(np.uint8)
contours,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
if not contours:return None
contour=max(contours,key=cv2.contourArea)
(x,y),(w,h),angle=cv2.minAreaRect(contour)
pts=cv2.boxPoints(((x,y),(w,h),angle));edges=np.roll(pts,-1,axis=0)-pts;v=edges[np.argmax(np.linalg.norm(edges,axis=1))]
if v[1]<0:v=-v
tilt=np.arctan2(v[0],v[1])
return dict(x=float(x/im.shape[1]),y=float(y/im.shape[0]),tilt=float(tilt),area=float(cv2.contourArea(contour)/mask.size))
def act(obs):
global _last_action, last_diagnostics, _unsupported, _target_y
# Native horizon includes the runner-owned reset hold. No extra hold here.
if int(obs.get('step_id', 1)) >= 700:
last_diagnostics = {'status': 'native_horizon'}
return None
try:
f, visible = features(obs)
except (KeyError, ValueError, TypeError):
last_diagnostics = {'status': 'invalid_observation'}
return None
if _last_action is None:
im=np.asarray(obs['rgb']['top_camera']).squeeze().astype(float)
r,g,b=im[...,0],im[...,1],im[...,2]
yy,xx=np.nonzero((r>65)&(r>1.5*b+15)&(r>1.4*g+15))
if len(yy)>100: _target_y=float((yy.min()+yy.max())/(2*im.shape[0]))
bank = _load()
distance = np.sum((bank['features']-f)**2,axis=1)
idx = np.array([np.where(bank['demo_index']==di)[0][np.argmin(distance[bank['demo_index']==di])] for di in range(20)])
idx = idx[np.argsort(distance[idx])][:5]
best = idx[0]
# Reject gross extrapolation. This threshold is heuristic, not validated.
if not visible or distance[best] > 16.0:
action = np.zeros(14,np.float32); action[[6,13]] = -1
last_diagnostics = {'status': 'unsupported_hold', 'squared_distance': float(distance[best])}
else:
# Avoid averaging opposing contact/repositioning actions. Both robot
# commands always come from the same paired observation/action record.
compatible = np.max(np.abs(bank['actions'][idx]-bank['actions'][best]),axis=1) <= .12
idx = idx[compatible]
weights = np.exp(-(distance[idx]-distance[best])/.35)
action = np.average(bank['actions'][idx], axis=0, weights=weights)
action = np.clip(action,-1,1).astype(np.float32)
last_diagnostics = {'status': 'lookup', 'squared_distance': float(distance[best]),
'reference_demo': int(bank['demo_index'][best]),
'reference_frame': int(bank['frame_index'][best])}
pose = box_pose(np.asarray(obs['rgb']['top_camera']).squeeze())
if (last_diagnostics['status']=='lookup' and pose is not None
and .54 < pose['x'] < .83 and pose['area'] > .045
and abs(pose['tilt']) < .65
and min(action[0], action[7]) > .035
and min(action[2], action[9]) > -.05):
tilt = pose['tilt']
correction = np.clip(.18 * np.sign(tilt)*max(abs(tilt)-.06,0),-.06,.06)
action[0] = np.clip(action[0] + correction, 0, .15)
action[7] = np.clip(action[7] - correction, 0, .15)
last_diagnostics['yaw_correction'] = float(correction)
last_diagnostics['box_tilt'] = float(tilt)
lateral=float(np.clip(.4*(pose['y']-_target_y),-.035,.035))
action[1] = np.clip(action[1]+lateral,-.15,.15)
action[8] = np.clip(action[8]+lateral,-.15,.15)
last_diagnostics['lateral_correction'] = lateral
_unsupported = _unsupported+1 if last_diagnostics['status']=='unsupported_hold' else 0
if _unsupported >= 40:
last_diagnostics['status']='unsupported_abort'
return None
_last_action = action.copy()
return action
|