Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
5.3 kB
"""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)