File size: 5,295 Bytes
d7a31d7
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
"""D20 nonparametric feedback policy with bounded phase progression. No environment imports."""
from pathlib import Path
import numpy as np
import cv2

CAMERAS = ('top_camera', 'front_camera')
QPATHS = tuple(f'agent/panda_leaphand-{r}/qpos' for r in range(2))
VPATHS = tuple(f'agent/panda_leaphand-{r}/qvel' for r in range(2))

def visual_features(rgb):
    features = []
    for camera in CAMERAS:
        im = np.asarray(rgb[camera])
        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 RGB HWC or 1HWC')
        hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV)
        # Colored objects retain more weight than the neutral table and arms.
        mask = ((hsv[:,:,1] > 150) & (hsv[:,:,2] > 70)).astype(np.float32)
        color = im.astype(np.float32) / 255.
        features.append(cv2.resize(color, (16,16), interpolation=cv2.INTER_AREA).ravel())
        features.append(3 * cv2.resize(color * mask[:,:,None], (24,24), interpolation=cv2.INTER_AREA).ravel())
    return np.concatenate(features).astype(np.float32)

class Controller:
    SPEED=1.6
    ARM_GAIN=.15
    HAND_GAIN=2.
    def __init__(self):
        with np.load(Path(__file__).with_name('memory.npz'), allow_pickle=False) as data:
            self.bank = {key: data[key] for key in data.files}
        self.inverse = np.load(Path(__file__).with_name('feedback.npz'))['inverse']
        self.scale = np.tile(np.r_[np.full(7,.20), np.full(16,.35)],2).astype(np.float32)
        self.reset_episode()

    def reset_episode(self):
        self.t = 0
        self.last_phase = None
        self.last_step = None
        self.diagnostics = {'status':'reset'}

    def act(self, obs):
        self.t += 1
        step = int(obs.get('step_id', self.t))
        obs = obs.get('observation', obs)
        if self.last_step is not None and step < self.last_step:
            self.reset_episode()
        self.last_step = step
        # One native step was already consumed by the runner-owned hold.
        if step >= int(np.ceil(1559/self.SPEED))+41:
            self.diagnostics = {'status':'horizon_stop', 'success_claim':False}
            return None
        try:
            p = obs['proprioception']
            q = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in QPATHS])
            v = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in VPATHS])
            if q.shape != (46,) or v.shape != (46,) or not np.isfinite(q).all() or not np.isfinite(v).all():
                raise ValueError('Malformed proprioception')
            f = visual_features(obs['rgb'])
        except (KeyError, ValueError, TypeError, cv2.error):
            self.diagnostics = {'status':'missing_or_invalid_observation'}
            return np.zeros(44,dtype=np.float32)
        if (step-1)*self.SPEED >= 1559:
            a = np.zeros(44,dtype=np.float32)
            a[2] = a[24] = .04
            self.diagnostics = {'status':'retract_for_visibility'}
            return a
        b = self.bank
        qcost = np.mean(((b['q']-q)/self.scale)**2,axis=1)
        vcost = .025*np.mean(np.minimum((b['v']-v)**2,25),axis=1)
        base = qcost + vcost
        # Timing resolves stationary/contact phase ambiguity seen in episode 1.
        expected = min((step - 1)*self.SPEED, 1558)
        base += .5*((b['phase']-expected)/8.)**2
        # Soft continuity inferred from previous observed state, never demo ID or seed.
        if self.last_phase is not None:
            base += .08*np.minimum(((b['phase']-self.last_phase)/100.)**2,4.)
        ids = np.argpartition(base,min(127,len(base)-1))[:128]
        visual = np.mean((b['features'][ids]-f)**2,axis=1)
        score = base[ids] + 12*visual
        j = int(ids[np.argmin(score)])
        if qcost[j] > 4.0 or float(visual[np.argmin(score)]) > .15:
            self.diagnostics = {'status':'outside_demo_support', 'qcost':float(qcost[j])}
            return np.zeros(44,dtype=np.float32)
        self.last_phase = float(b['phase'][j])
        self.diagnostics = {'status':'retrieved_feedback','reference_phase':self.last_phase,'reference_demo':int(b['demo'][j]),'qcost':float(qcost[j]),'success_claim':False}
        # Jointly retrieve both robots to preserve the demonstrated coordination.
        # Parts are [robot 0: 6 arm + 16 hand, robot 1: 6 arm + 16 hand].
        a = self.SPEED * b['actions'][j].copy()
        errors = b['q'][j] - q
        for r in range(2):
            correction = self.ARM_GAIN * errors[r*23:r*23+7] @ self.inverse[min(int(expected)//4,390),r]
            correction = np.clip(correction,-.03,.03)
            a[np.array([0,1,2,4])+r*22] += correction
            hand_a = np.array([0,1,2,3,12,14,15]) + r*22+6
            hand_q = np.array([11,7,15,19,10,18,22]) + r*23
            a[hand_a] += np.clip(self.HAND_GAIN*errors[hand_q],-.1,.1)
        self.diagnostics['pose_error'] = float(np.linalg.norm(errors))
        return np.clip(a,-1.,1.).astype(np.float32)

_policy = None

def reset_episode():
    global _policy
    if _policy is not None:
        _policy.reset_episode()

def act(obs):
    global _policy
    if _policy is None:
        _policy = Controller()
    return _policy.act(obs)