File size: 3,647 Bytes
e25638c | 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 | """Unrehearsed D20 observation-conditioned action interpolation; no neural model."""
from pathlib import Path
import numpy as np
import cv2
ROBOTS = ('panda_leaphand-1', 'panda_leaphand_left-0')
CAMERAS = ('front_camera', 'top_camera')
_memory = None
def reset_episode():
"""No trajectory index or scene identity is retained between calls/episodes."""
global _initial_target
_initial_target = None
def image_features(rgb):
appearance, targets, visible = [], [], []
for cam in CAMERAS:
im = np.asarray(rgb[cam])
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')
appearance.extend((cv2.resize(im, (32, 24), interpolation=cv2.INTER_AREA).astype(np.float32)/255).ravel())
h, w = im.shape[:2]
hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV)
mask = cv2.inRange(hsv, np.array([95,70,45]), np.array([135,255,255]))
# Spatial ordering is image-relative, not a robot/world axis conversion.
for half in range(2):
region = mask.copy()
if cam == 'front_camera':
region[:int(.30*h)] = 0
region[int(.82*h):] = 0
region[:, :w//2 if half else 0] = 0
if half == 0: region[:, w//2:] = 0
else:
if half == 0: region[h//2:] = 0
else: region[:h//2] = 0
count, labels, stats, centers = cv2.connectedComponentsWithStats(region)
ids = [i for i in range(1,count) if stats[i,cv2.CC_STAT_AREA] >= 3]
if ids:
i = max(ids, key=lambda j: stats[j,cv2.CC_STAT_AREA])
targets.extend(centers[i]/[w,h]); visible.extend([1,1])
else:
targets.extend([0,0]); visible.extend([0,0])
return (np.asarray(appearance,np.float32), np.asarray(targets,np.float32), np.asarray(visible,np.float32))
def _load():
global _memory
if _memory is None:
with np.load(Path(__file__).with_name('demo_memory.npz'), allow_pickle=False) as f:
_memory = {k:f[k] for k in f.files}
return _memory
_initial_target = None
_regdata = None
def act(obs):
global _initial_target, _regdata
if int(obs.get('step_id',1)) >= 200: return None
out = np.zeros(44,np.float32)
try:
q = np.concatenate([np.asarray(obs['proprioception']['agent/'+r+'/qpos']).reshape(-1) for r in ROBOTS])
_, target, valid = image_features(obs['rgb'])
except (KeyError,ValueError,TypeError): return out
if _initial_target is None:
if valid.sum()!=8: return out
_initial_target = target.copy()
m = _load()
if _regdata is None:
initial = m['target'][m['frame_index']==0]
_regdata = initial[m['demo_index']]
for arm,ts in [(0,[2,3,4,5]),(1,[0,1,6,7])]:
qs=slice(arm*23,arm*23+7)
# Locally affine non-neural imitation from measured joints and fixed target localization.
dx=np.concatenate([(m['q'][:,qs]-q[qs])/.15,(_regdata[:,ts]-_initial_target[ts])/.025],axis=1)
dist=np.mean(dx[:,:7]**2,axis=1)+np.mean(dx[:,7:]**2,axis=1)*2
ids=np.argpartition(dist,119)[:120]
x=np.column_stack([np.ones(len(ids)),dx[ids]])
w=np.exp(-(dist[ids]-dist[ids].min())/max(.1,float(np.median(dist[ids]))))
reg=np.eye(x.shape[1])*.015;reg[0,0]=.00001
y=m['action'][ids,arm*22:arm*22+3]
coef=np.linalg.solve(x.T@(w[:,None]*x)+reg,x.T@(w[:,None]*y))
out[arm*22:arm*22+3]=np.clip(coef[0],-.06,.06)
return out
|