File size: 2,950 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
61
62
63
64
65
66
67
68
69
70
71
72
73
"""D20 action interpolation with E20-tested wrist lateral alignment.
No environment imports, kinematics, hidden state, seed lookup or neural weights.
"""
from pathlib import Path
import importlib.util
import numpy as np

_spec=importlib.util.spec_from_file_location('_d20_features',Path(__file__).with_name('features.py'))
_features=importlib.util.module_from_spec(_spec)
_spec.loader.exec_module(_features)
_library=None
_cache={}

def reset_episode():
    global _cache
    _cache={}

def _hold(q):
    a=np.zeros(14,np.float32)
    for i in range(2):
        g=2*(float(q[i][7])+0.01)/0.05-1
        if not np.isfinite(g) or not -1<=g<=1:
            return None  # Never clip an invalid hold into another command.
        a[i*7+6]=g
    return a

def act(obs):
    global _library
    step=int(obs['step_id'])
    if step < 1 or step >= 250: return None
    if _library is None:
        with np.load(Path(__file__).with_name('library.npz'),allow_pickle=False) as f:
            _library={k:f[k] for k in ('q','v','targets','wrist','actions')}
    p=obs['proprioception']
    try:
        q=[np.asarray(p[f'agent/panda-{i}/qpos'],dtype=np.float32).reshape(-1) for i in range(2)]
        v=[np.asarray(p[f'agent/panda-{i}/qvel'],dtype=np.float32).reshape(-1) for i in range(2)]
        if any(len(x)!=9 or not np.isfinite(x).all() for x in q+v):return None
        targets,wrist=_features.visual(obs['rgb'],_cache)
    except (KeyError,ValueError,TypeError):return None
    if not np.isfinite(targets).all():return _hold(q)
    db=_library
    # Baseline demonstration distances, plus the E20 wrist alignment below.
    dt=np.mean(((db['targets']-targets)/0.04)**2,axis=1)
    result=[]
    for i in range(2):
        dq=np.mean(((db['q'][:,i]-q[i][:7])/0.18)**2,axis=1)
        dv=np.mean(((db['v'][:,i]-v[i][:7])/1.0)**2,axis=1)
        w=db['wrist'][:,i]; live=wrist[i]
        both=(w[:,3]>.5)&(live[3]>.5)
        dw=np.where(both,np.mean(((w[:,:3]-live[:3])/np.array([.12,.12,.15]))**2,axis=1),0)
        dw+=2.0*(w[:,3]-live[3])**2
        distance=dq+dt+0.15*dv+0.5*dw
        # Refuse to extrapolate far outside observed joint/target support.
        if np.min(dq+dt)>30:return _hold(q)
        idx=np.argpartition(distance,15)[:16]
        weight=np.exp(-(distance[idx]-distance[idx].min())/0.6)
        weight/=weight.sum()
        result.extend(weight@db['actions'][idx,i*7:(i+1)*7])
    a=np.asarray(result,np.float32)
    if a.shape!=(14,) or not np.isfinite(a).all():return None
    # Legal demo finite differences show positive root-y commands move the
    # button right in the wrist image. Correct lateral alignment before descent.
    for i in range(2):
        u,v,area,present=wrist[i]
        if present>.5 and area>.02 and .15<v<.8:
            error=float(u)-.5
            a[i*7+1]+=np.clip(-.2*error,-.08,.08)
            if abs(error)>.15:
                a[i*7]=0.
                a[i*7+2]=0.
    return np.clip(a,-1,1)