"""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')