File size: 3,647 Bytes
e25638c
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
74
75
76
77
78
79
80
81
82
83
84
85
86
"""Unrehearsed D20 observation-conditioned action interpolation; no neural model."""
from pathlib import Path
import numpy as np
import cv2

ROBOTS = ('panda_leaphand-1', 'panda_leaphand_left-0')
CAMERAS = ('front_camera', 'top_camera')
_memory = None

def reset_episode():
    """No trajectory index or scene identity is retained between calls/episodes."""
    global _initial_target
    _initial_target = None

def image_features(rgb):
    appearance, targets, visible = [], [], []
    for cam in CAMERAS:
        im = np.asarray(rgb[cam])
        if im.ndim == 4 and im.shape[0] == 1:
            im = im[0]
        if im.ndim != 3 or im.shape[2] != 3 or im.dtype != np.uint8:
            raise ValueError('Expected uint8 HWC or 1HWC RGB')
        appearance.extend((cv2.resize(im, (32, 24), interpolation=cv2.INTER_AREA).astype(np.float32)/255).ravel())
        h, w = im.shape[:2]
        hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV)
        mask = cv2.inRange(hsv, np.array([95,70,45]), np.array([135,255,255]))
        # Spatial ordering is image-relative, not a robot/world axis conversion.
        for half in range(2):
            region = mask.copy()
            if cam == 'front_camera':
                region[:int(.30*h)] = 0
                region[int(.82*h):] = 0
                region[:, :w//2 if half else 0] = 0
                if half == 0: region[:, w//2:] = 0
            else:
                if half == 0: region[h//2:] = 0
                else: region[:h//2] = 0
            count, labels, stats, centers = cv2.connectedComponentsWithStats(region)
            ids = [i for i in range(1,count) if stats[i,cv2.CC_STAT_AREA] >= 3]
            if ids:
                i = max(ids, key=lambda j: stats[j,cv2.CC_STAT_AREA])
                targets.extend(centers[i]/[w,h]); visible.extend([1,1])
            else:
                targets.extend([0,0]); visible.extend([0,0])
    return (np.asarray(appearance,np.float32), np.asarray(targets,np.float32), np.asarray(visible,np.float32))

def _load():
    global _memory
    if _memory is None:
        with np.load(Path(__file__).with_name('demo_memory.npz'), allow_pickle=False) as f:
            _memory = {k:f[k] for k in f.files}
    return _memory


_initial_target = None
_regdata = None

def act(obs):
    global _initial_target, _regdata
    if int(obs.get('step_id',1)) >= 200: return None
    out = np.zeros(44,np.float32)
    try:
        q = np.concatenate([np.asarray(obs['proprioception']['agent/'+r+'/qpos']).reshape(-1) for r in ROBOTS])
        _, target, valid = image_features(obs['rgb'])
    except (KeyError,ValueError,TypeError): return out
    if _initial_target is None:
        if valid.sum()!=8: return out
        _initial_target = target.copy()
    m = _load()
    if _regdata is None:
        initial = m['target'][m['frame_index']==0]
        _regdata = initial[m['demo_index']]
    for arm,ts in [(0,[2,3,4,5]),(1,[0,1,6,7])]:
        qs=slice(arm*23,arm*23+7)
        # Locally affine non-neural imitation from measured joints and fixed target localization.
        dx=np.concatenate([(m['q'][:,qs]-q[qs])/.15,(_regdata[:,ts]-_initial_target[ts])/.025],axis=1)
        dist=np.mean(dx[:,:7]**2,axis=1)+np.mean(dx[:,7:]**2,axis=1)*2
        ids=np.argpartition(dist,119)[:120]
        x=np.column_stack([np.ones(len(ids)),dx[ids]])
        w=np.exp(-(dist[ids]-dist[ids].min())/max(.1,float(np.median(dist[ids]))))
        reg=np.eye(x.shape[1])*.015;reg[0,0]=.00001
        y=m['action'][ids,arm*22:arm*22+3]
        coef=np.linalg.solve(x.T@(w[:,None]*x)+reg,x.T@(w[:,None]*y))
        out[arm*22:arm*22+3]=np.clip(coef[0],-.06,.06)
    return out