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