File size: 2,647 Bytes
c158cda
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""D20 observation-conditioned nonparametric controller with bounded phase advance."""
from pathlib import Path
import importlib.util
import numpy as np
_s=importlib.util.spec_from_file_location('_d20_features',Path(__file__).with_name('features.py'))
_f=importlib.util.module_from_spec(_s);_s.loader.exec_module(_f)
_bank=None
_phase=None
_calls=0
_bad=0
_last_step=0

def reset_episode():
    global _phase,_calls,_bad,_last_step
    _phase=None;_calls=0;_bad=0;_last_step=0

def act(obs):
    global _bank,_phase,_calls,_bad,_last_step
    step=int(obs.get('step_id',_calls+1))
    if step>=680 or _calls>=679: return None
    if _phase is not None and _phase>=.995: return np.zeros(44,dtype=np.float32)
    if step<_last_step: reset_episode()
    _last_step=step;_calls+=1
    if _bank is None:
        with np.load(Path(__file__).with_name('bank.npz'),allow_pickle=False) as z: _bank={k:z[k] for k in z.files}
    b=_bank
    try:
        q=_f.proprio(obs['proprioception'],'qpos');v=_f.proprio(obs['proprioception'],'qvel');im=_f.visual(obs['rgb'])
    except (KeyError,ValueError,TypeError):
        _bad+=1
        return None if _bad>=20 else np.zeros(44,dtype=np.float32)
    _bad=0
    # Equal robot weighting; fingers are less reliable pose indicators under contact.
    scales=np.tile(np.r_[np.full(7,.22),np.full(16,.5)],2)
    dq=np.mean(((b['q']-q)/scales)**2,axis=1)
    dv=np.mean(np.minimum(((b['v']-v)/2)**2,9),axis=1)
    eligible=np.arange(len(dq))
    if _phase is not None:
        eligible=eligible[(b['phase']>=max(0,_phase-.018)) & (b['phase']<=_phase+.009)]
    if len(eligible)<7: eligible=np.arange(len(dq))
    shortlist=eligible[np.argsort((dq+.05*dv)[eligible])[:240]]
    score=dq[shortlist]+.05*dv[shortlist]+12*np.mean((b['img'][shortlist]-im)**2,axis=1)
    # Soft hysteresis disambiguates similar poses without selecting a fixed demo.
    if _phase is not None:
        dp=b['phase'][shortlist]-_phase
        score+=2*np.maximum(abs(dp)-.08,0)**2
    ix=shortlist[np.argsort(score)[:7]]
    dist=np.sort(score)[:7]
    if dist[0]>12: return np.zeros(44,dtype=np.float32)
    w=np.exp(-(dist-dist.min())/.08);w/=w.sum()
    new_phase=float(w@b['phase'][ix])
    _phase=new_phase if _phase is None else float(np.clip(new_phase,_phase+.0015,_phase+.0022))
    a=w@b['a'][ix]
    # Hand action labels retain their recorded order; no unverified joint remapping.
    target=w@b['q'][ix]
    for j,offset in [(0,0),(22,23)]:
        # No direct finger correction: qpos and action ordering are unverified.
        a[j+3:j+6]=0 # every authorized label has zero rotation
    return np.clip(a,-1,1).astype(np.float32)