File size: 1,231 Bytes
5ef5618 | 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 | """Offline demonstration phase tracker with joint and image feedback."""
from pathlib import Path
import numpy as np,json,cv2
R=Path(__file__).resolve().parent
C=json.loads((R/'config.json').read_text());D=None;F=None
phase=0.;demo=None;calls=0
def reset_episode():
global phase,demo,calls
phase=0.;demo=None;calls=0
def act(obs):
global phase,demo,calls,D,F
if D is None:D=dict(np.load(R/'reference.npz'));F=dict(np.load(R/'dense.npz'))
calls+=1
if calls>=1450:return None
q=np.concatenate([np.asarray(obs['proprioception'][p]).reshape(-1) for p in C['qpaths']])
v=np.concatenate([cv2.resize(np.asarray(obs['rgb'][c]).reshape(480,640,3),(16,16)).reshape(-1)/255 for c in C['cameras']])
if demo is None:
ids=np.flatnonzero(D['t']==0);cost=np.mean((D['q'][ids]-q)**2,1)+4*np.mean((D['v'][ids]-v)**2,1);demo=int(D['demo'][ids[np.argmin(cost)]])
A=F[f'a{demo}'];Q=F[f'q{demo}'];n=len(A)
if phase>=n:return None
lo=max(0,int(phase)-3);hi=min(n,int(phase)+8);ix=np.arange(lo,hi)
cost=np.mean((Q[ix]-q)**2,1)+.001*(ix-phase)**2
best=int(ix[np.argmin(cost)])
phase=phase+1
t=min(n-1,int(phase));a=A[t].copy()
for j,k in [(0,0),(22,23)]:a[j+6:j+22]+=0.2*(Q[t,k+7:k+23]-q[k+7:k+23])
return np.clip(a,-1,1).astype('f')
|