File size: 6,688 Bytes
746ed81
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
"""D20 observation-conditioned lookup with bounded image feedback.
No simulator, kinematics package, neural weights, seed, or timed demo replay.
"""
from pathlib import Path
import cv2
import numpy as np

CAMERAS = ('top_camera', 'front_camera')
_BANK = None
_last_action = None
_unsupported = 0
_target_y = .5
last_diagnostics = {}


def reset_episode():
    global _last_action, last_diagnostics, _unsupported, _target_y
    _last_action = None
    _unsupported = 0
    _target_y = .5
    last_diagnostics = {'status': 'reset'}


def features(obs):
    """Fixed hand-designed metric, shared verbatim by compiler and runtime."""
    parts = []
    blue_counts = []
    for camera in CAMERAS:
        im = np.asarray(obs['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 HWC or 1HWC RGB')
        r, g, b = [im[..., i].astype(np.float32) for i in range(3)]
        masks = [(b > 65) & (b > r * 1.6 + 15) & (b > g * 1.4 + 15),
                 (r > 65) & (r > b * 1.5 + 15) & (r > g * 1.4 + 15)]
        blue_counts.append(int(masks[0].sum()))
        # Spatial masks preserve visible box shape and target location. Missing
        # red pixels remain a visibility feature; target may be occluded.
        for mask in masks:
            small = cv2.resize(mask.astype(np.float32), (16, 16), interpolation=cv2.INTER_AREA)
            parts.append(small.ravel() * (8.0 / 16.0))
            yy, xx = np.nonzero(mask)
            if len(xx) >= 8:
                x, y = xx / im.shape[1], yy / im.shape[0]
                geom = [x.mean(), y.mean(), x.std(), y.std(),
                        np.mean((x-x.mean())*(y-y.mean())), len(xx)/mask.size, 1.0]
            else:
                geom = [0.] * 7
            parts.append(np.asarray(geom, np.float32) * np.array([12,12,6,6,20,10,1],np.float32))
        small_rgb = cv2.resize(im, (16,16), interpolation=cv2.INTER_AREA).astype(np.float32)/255
        parts.append(small_rgb.ravel() * (1.0 / np.sqrt(768)))
    for kind, scale in [('qpos', .20), ('qvel', 1.0)]:
        q = []
        for robot in range(2):
            v = np.asarray(obs['proprioception'][f'agent/panda-{robot}/{kind}'],dtype=np.float32).reshape(-1)
            if v.size != 9 or not np.isfinite(v).all():
                raise ValueError('Expected nine finite proprioceptive values per robot')
            q.extend(v[:7])
        parts.append(np.array(q,np.float32)/(scale*np.sqrt(14)))
    f = np.concatenate(parts).astype(np.float32)
    if not np.isfinite(f).all():
        raise ValueError('Nonfinite features')
    return f, min(blue_counts) >= 8


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


def box_pose(im):
 r,g,b=[im[...,i].astype(float) for i in range(3)]
 mask=((b>65)&(b>1.6*r+15)&(b>1.4*g+15)).astype(np.uint8)
 contours,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
 if not contours:return None
 contour=max(contours,key=cv2.contourArea)
 (x,y),(w,h),angle=cv2.minAreaRect(contour)
 pts=cv2.boxPoints(((x,y),(w,h),angle));edges=np.roll(pts,-1,axis=0)-pts;v=edges[np.argmax(np.linalg.norm(edges,axis=1))]
 if v[1]<0:v=-v
 tilt=np.arctan2(v[0],v[1])
 return dict(x=float(x/im.shape[1]),y=float(y/im.shape[0]),tilt=float(tilt),area=float(cv2.contourArea(contour)/mask.size))


def act(obs):
    global _last_action, last_diagnostics, _unsupported, _target_y
    # Native horizon includes the runner-owned reset hold. No extra hold here.
    if int(obs.get('step_id', 1)) >= 700:
        last_diagnostics = {'status': 'native_horizon'}
        return None
    try:
        f, visible = features(obs)
    except (KeyError, ValueError, TypeError):
        last_diagnostics = {'status': 'invalid_observation'}
        return None
    if _last_action is None:
        im=np.asarray(obs['rgb']['top_camera']).squeeze().astype(float)
        r,g,b=im[...,0],im[...,1],im[...,2]
        yy,xx=np.nonzero((r>65)&(r>1.5*b+15)&(r>1.4*g+15))
        if len(yy)>100: _target_y=float((yy.min()+yy.max())/(2*im.shape[0]))
    bank = _load()
    distance = np.sum((bank['features']-f)**2,axis=1)
    idx = np.array([np.where(bank['demo_index']==di)[0][np.argmin(distance[bank['demo_index']==di])] for di in range(20)])
    idx = idx[np.argsort(distance[idx])][:5]
    best = idx[0]
    # Reject gross extrapolation. This threshold is heuristic, not validated.
    if not visible or distance[best] > 16.0:
        action = np.zeros(14,np.float32); action[[6,13]] = -1
        last_diagnostics = {'status': 'unsupported_hold', 'squared_distance': float(distance[best])}
    else:
        # Avoid averaging opposing contact/repositioning actions. Both robot
        # commands always come from the same paired observation/action record.
        compatible = np.max(np.abs(bank['actions'][idx]-bank['actions'][best]),axis=1) <= .12
        idx = idx[compatible]
        weights = np.exp(-(distance[idx]-distance[best])/.35)
        action = np.average(bank['actions'][idx], axis=0, weights=weights)
        action = np.clip(action,-1,1).astype(np.float32)
        last_diagnostics = {'status': 'lookup', 'squared_distance': float(distance[best]),
                            'reference_demo': int(bank['demo_index'][best]),
                            'reference_frame': int(bank['frame_index'][best])}
    pose = box_pose(np.asarray(obs['rgb']['top_camera']).squeeze())
    if (last_diagnostics['status']=='lookup' and pose is not None
        and .54 < pose['x'] < .83 and pose['area'] > .045
        and abs(pose['tilt']) < .65
        and min(action[0], action[7]) > .035
        and min(action[2], action[9]) > -.05):
        tilt = pose['tilt']
        correction = np.clip(.18 * np.sign(tilt)*max(abs(tilt)-.06,0),-.06,.06)
        action[0] = np.clip(action[0] + correction, 0, .15)
        action[7] = np.clip(action[7] - correction, 0, .15)
        last_diagnostics['yaw_correction'] = float(correction)
        last_diagnostics['box_tilt'] = float(tilt)
        lateral=float(np.clip(.4*(pose['y']-_target_y),-.035,.035))
        action[1] = np.clip(action[1]+lateral,-.15,.15)
        action[8] = np.clip(action[8]+lateral,-.15,.15)
        last_diagnostics['lateral_correction'] = lateral
    _unsupported = _unsupported+1 if last_diagnostics['status']=='unsupported_hold' else 0
    if _unsupported >= 40:
        last_diagnostics['status']='unsupported_abort'
        return None
    _last_action = action.copy()
    return action