File size: 4,177 Bytes
044c27e
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""D20 observation-conditioned retrieval controller. No simulator or weights."""
from pathlib import Path
import numpy as np
import cv2

CAMERAS = ('top_camera', 'front_camera')

def image_feature(rgb):
    parts = []
    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 min(im.shape[:2]) < 2:
            raise ValueError('Expected RGB HWC or 1HWC')
        if not np.isfinite(im).all():
            raise ValueError('Nonfinite RGB')
        parts.append(cv2.resize(im.astype(np.float32), (16, 16),
                                interpolation=cv2.INTER_AREA).ravel() / 255.)
    return np.concatenate(parts)

_bank = None
_phase = None
_last_step = None
_diagnostics = {}

def reset_episode():
    global _phase, _last_step, _diagnostics
    _phase = None
    _last_step = None
    _diagnostics = {}

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

def act(obs):
    """Return 6 root-frame arm deltas then 16 dex deltas, in one 22-vector.

    All retrieval is recomputed from live legal observations. Temporal continuity
    is a soft phase prior, never a selected-demo playback cursor. None means
    horizon exhaustion or unusable input, and does not assert task completion.
    """
    global _phase, _last_step, _diagnostics
    if 'observation' in obs:
        envelope = obs
        obs = dict(envelope['observation'])
        obs['step_id'] = envelope.get('step_id', 1 if _last_step is None else _last_step + 1)
    step = int(obs.get('step_id', 1 if _last_step is None else _last_step + 1))
    if step >= 500:
        return None  # Runner already charged one native hold step.
    if _last_step is not None and step < _last_step:
        reset_episode()
    try:
        p = obs['proprioception']
        q = np.asarray(p['agent/qpos'], dtype=np.float32).reshape(-1)
        v = np.asarray(p['agent/qvel'], dtype=np.float32).reshape(-1)
        if q.size != 23 or v.size != 23 or not np.isfinite(q).all() or not np.isfinite(v).all():
            return None
        visual = image_feature(obs['rgb'])
    except (KeyError, ValueError, TypeError):
        return None
    bank = _load()
    # Coarse proprioceptive candidates; current RGB reranks every invocation.
    dq = (bank['q'] - q) / bank['q_scale']
    dv = (bank['v'] - v) / bank['v_scale']
    cost = np.mean(dq[:, :7] ** 2, axis=1) + .6 * np.mean(dq[:, 7:] ** 2, axis=1)
    cost += .08 * np.mean(np.minimum(dv ** 2, 25), axis=1)
    if _phase is not None:
        # A bounded one-step expectation resolves nearly stationary grasp stages.
        cost += .35 * (bank['phase'] - _phase) ** 2
    pool = np.argpartition(cost, min(95, len(cost)-1))[:96]
    image_error = np.mean((bank['visual'][pool] - visual) ** 2, axis=1)
    score = cost[pool] + 80. * image_error
    order = np.argsort(score)
    anchor = pool[order[0]]
    # Do not average actions from opposite sides of a grasp transition.
    compatible = pool[np.floor(bank['phase'][pool]) == np.floor(bank['phase'][anchor])]
    vis = np.mean((bank['visual'][compatible] - visual) ** 2, axis=1)
    total = cost[compatible] + 80. * vis
    chosen = np.argsort(total)[:5]
    ids = compatible[chosen]
    weights = np.exp(-(total[chosen] - total[chosen[0]]) / .08)
    weights /= weights.sum()
    action = weights @ bank['actions'][ids]
    estimate = float(weights @ bank['phase'][ids])
    increment = float(weights @ bank['phase_rate'][ids])
    if _phase is None:
        _phase = estimate + increment
    else:
        _phase = float(np.clip(.8*_phase + .2*estimate + increment, 0, 7.99))
    _last_step = step
    _diagnostics = {'phase': _phase, 'reference_distance': float(total[chosen[0]]),
                    'visual_error': float(image_error[order[0]])}
    if 5.0 <= _phase < 6.0:
        action[6:] *= .5
    # Exact action-part order: one width-22 part, no mimic gripper component.
    return np.clip(action, -1., 1.).astype(np.float32)