File size: 5,295 Bytes
d7a31d7 | 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 | """D20 nonparametric feedback policy with bounded phase progression. No environment imports."""
from pathlib import Path
import numpy as np
import cv2
CAMERAS = ('top_camera', 'front_camera')
QPATHS = tuple(f'agent/panda_leaphand-{r}/qpos' for r in range(2))
VPATHS = tuple(f'agent/panda_leaphand-{r}/qvel' for r in range(2))
def visual_features(rgb):
features = []
for camera in CAMERAS:
im = np.asarray(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 RGB HWC or 1HWC')
hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV)
# Colored objects retain more weight than the neutral table and arms.
mask = ((hsv[:,:,1] > 150) & (hsv[:,:,2] > 70)).astype(np.float32)
color = im.astype(np.float32) / 255.
features.append(cv2.resize(color, (16,16), interpolation=cv2.INTER_AREA).ravel())
features.append(3 * cv2.resize(color * mask[:,:,None], (24,24), interpolation=cv2.INTER_AREA).ravel())
return np.concatenate(features).astype(np.float32)
class Controller:
SPEED=1.6
ARM_GAIN=.15
HAND_GAIN=2.
def __init__(self):
with np.load(Path(__file__).with_name('memory.npz'), allow_pickle=False) as data:
self.bank = {key: data[key] for key in data.files}
self.inverse = np.load(Path(__file__).with_name('feedback.npz'))['inverse']
self.scale = np.tile(np.r_[np.full(7,.20), np.full(16,.35)],2).astype(np.float32)
self.reset_episode()
def reset_episode(self):
self.t = 0
self.last_phase = None
self.last_step = None
self.diagnostics = {'status':'reset'}
def act(self, obs):
self.t += 1
step = int(obs.get('step_id', self.t))
obs = obs.get('observation', obs)
if self.last_step is not None and step < self.last_step:
self.reset_episode()
self.last_step = step
# One native step was already consumed by the runner-owned hold.
if step >= int(np.ceil(1559/self.SPEED))+41:
self.diagnostics = {'status':'horizon_stop', 'success_claim':False}
return None
try:
p = obs['proprioception']
q = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in QPATHS])
v = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in VPATHS])
if q.shape != (46,) or v.shape != (46,) or not np.isfinite(q).all() or not np.isfinite(v).all():
raise ValueError('Malformed proprioception')
f = visual_features(obs['rgb'])
except (KeyError, ValueError, TypeError, cv2.error):
self.diagnostics = {'status':'missing_or_invalid_observation'}
return np.zeros(44,dtype=np.float32)
if (step-1)*self.SPEED >= 1559:
a = np.zeros(44,dtype=np.float32)
a[2] = a[24] = .04
self.diagnostics = {'status':'retract_for_visibility'}
return a
b = self.bank
qcost = np.mean(((b['q']-q)/self.scale)**2,axis=1)
vcost = .025*np.mean(np.minimum((b['v']-v)**2,25),axis=1)
base = qcost + vcost
# Timing resolves stationary/contact phase ambiguity seen in episode 1.
expected = min((step - 1)*self.SPEED, 1558)
base += .5*((b['phase']-expected)/8.)**2
# Soft continuity inferred from previous observed state, never demo ID or seed.
if self.last_phase is not None:
base += .08*np.minimum(((b['phase']-self.last_phase)/100.)**2,4.)
ids = np.argpartition(base,min(127,len(base)-1))[:128]
visual = np.mean((b['features'][ids]-f)**2,axis=1)
score = base[ids] + 12*visual
j = int(ids[np.argmin(score)])
if qcost[j] > 4.0 or float(visual[np.argmin(score)]) > .15:
self.diagnostics = {'status':'outside_demo_support', 'qcost':float(qcost[j])}
return np.zeros(44,dtype=np.float32)
self.last_phase = float(b['phase'][j])
self.diagnostics = {'status':'retrieved_feedback','reference_phase':self.last_phase,'reference_demo':int(b['demo'][j]),'qcost':float(qcost[j]),'success_claim':False}
# Jointly retrieve both robots to preserve the demonstrated coordination.
# Parts are [robot 0: 6 arm + 16 hand, robot 1: 6 arm + 16 hand].
a = self.SPEED * b['actions'][j].copy()
errors = b['q'][j] - q
for r in range(2):
correction = self.ARM_GAIN * errors[r*23:r*23+7] @ self.inverse[min(int(expected)//4,390),r]
correction = np.clip(correction,-.03,.03)
a[np.array([0,1,2,4])+r*22] += correction
hand_a = np.array([0,1,2,3,12,14,15]) + r*22+6
hand_q = np.array([11,7,15,19,10,18,22]) + r*23
a[hand_a] += np.clip(self.HAND_GAIN*errors[hand_q],-.1,.1)
self.diagnostics['pose_error'] = float(np.linalg.norm(errors))
return np.clip(a,-1.,1.).astype(np.float32)
_policy = None
def reset_episode():
global _policy
if _policy is not None:
_policy.reset_episode()
def act(obs):
global _policy
if _policy is None:
_policy = Controller()
return _policy.act(obs)
|