File size: 6,445 Bytes
20033c3
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""Task-local nonparametric RGB/proprioception feedback policy. No fitted weights."""
from pathlib import Path
import numpy as np
import cv2

CAMERAS = ('front_camera', 'left_camera', 'robot_camera', 'top_camera')

def visual_feature(image):
    im = np.asarray(image)
    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')
    small = cv2.resize(im, (16, 12), interpolation=cv2.INTER_AREA).astype(np.float32) / 255.
    # Orange cloth cue, deliberately separate from the background-heavy thumbnail.
    hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV)
    mask = ((hsv[..., 0] >= 3) & (hsv[..., 0] <= 24) &
            (hsv[..., 1] > 110) & (hsv[..., 2] > 150)).astype(np.float32)
    grid = cv2.resize(mask, (16, 12), interpolation=cv2.INTER_AREA)
    return np.concatenate((small.ravel()/np.sqrt(small.size)*2,
                           grid.ravel()/np.sqrt(grid.size)*3)).astype(np.float32)

class Controller:
    def __init__(self):
        with np.load(Path(__file__).with_name('demo_bank.npz'), allow_pickle=False) as b:
            self.bank = {k: b[k] for k in b.files}
        self.visual_flat = self.bank['visual'].reshape(len(self.bank['visual']), -1)
        self.visual_norm = np.einsum('ij,ij->i', self.visual_flat, self.visual_flat)
        self.reset_episode()

    def reset_episode(self):
        self.end_count = 0
        self.invalid_count = 0
        self.last_diagnostic = {}
        self.calls = 0
        self.bowl_centers = []
        self.servo_steps = 0

    def act(self, obs):
        if int(obs.get('step_id', 1)) >= 800:
            return None  # one runner-owned hold + at most 799 policy actions
        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.shape != (23,) or v.shape != (23,) or not np.isfinite(q).all() or not np.isfinite(v).all():
                raise ValueError('Invalid proprioception')
            visual = np.stack([visual_feature(obs['rgb'][c]) for c in CAMERAS])
        except (KeyError, ValueError, TypeError):
            self.invalid_count += 1
            return None if self.invalid_count >= 10 else np.zeros(22, dtype=np.float32)
        self.calls += 1
        b = self.bank
        dq = (b['qpos'] - q) / b['qscale']
        dv = (b['qvel'] - v) / .5
        qdist = np.mean(dq[:, :7]**2, axis=1) + .4*np.mean(dq[:, 7:]**2, axis=1)
        vdist = .03*np.mean(dv**2, axis=1)
        visual_dist = np.maximum(0., (self.visual_norm + np.sum(visual*visual) -
            2*np.einsum('ij,j->i', self.visual_flat, visual.ravel(), optimize=False))/len(CAMERAS))
        distance = qdist + vdist + 4*visual_dist
        best = int(np.argmin(distance))
        endpoint_close = bool(b['terminal'][best]) and qdist[best] < .15 and visual_dist[best] < .08
        if b['terminal'][best] and not endpoint_close:
            distance[b['terminal']] = np.inf
            best = int(np.argmin(distance))
        frame = int(b['frame_index'][best])
        if 35 <= frame <= 125 and self.calls % 10 == 0:
            top = np.asarray(obs['rgb']['top_camera']).squeeze()
            gray = cv2.GaussianBlur(cv2.cvtColor(top, cv2.COLOR_RGB2GRAY), (5,5), 1)
            circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 70,
                param1=100, param2=30, minRadius=45, maxRadius=75)
            if circles is not None:
                valid = [c[:2] for c in circles[0] if 150<c[0]<380 and 240<c[1]<440 and 50<c[2]<69]
                if len(valid) == 1:
                    self.bowl_centers.append(valid[0])

        self.last_diagnostic = {'distance': float(distance[best]), 'reference_row': best,
                                'visual_distance': float(visual_dist[best]),
                                'pose_distance': float(qdist[best]),
                                'demo': int(b['demo_index'][best]),
                                'frame': int(b['frame_index'][best])}
        # Unvalidated conservative out-of-support gate; no blind replay fallback.
        if qdist[best] > 9 or visual_dist[best] > 1.5:
            self.invalid_count += 1
            return None if self.invalid_count >= 10 else np.zeros(22, dtype=np.float32)
        self.invalid_count = 0
        terminal = bool(b['terminal'][best]) and qdist[best] < .15 and visual_dist[best] < .08
        self.end_count = self.end_count + 1 if terminal else 0
        if self.end_count >= 6:
            return None  # endpoint resemblance, NOT a success assertion
        if terminal:
            return np.zeros(22, dtype=np.float32)
        # Blend only nearby samples with the same coarse action direction.
        a0 = b['actions'][best]
        indices = np.argsort(distance)[:12]
        compatible = (~b['terminal'][indices]) & (np.linalg.norm(b['actions'][indices,:3]-a0[:3],axis=1)<.08)
        indices = indices[compatible][:5]
        if len(indices) == 0:
            indices = np.array([best])
        weights = np.exp(-(distance[indices]-distance[best])/.06)
        action = np.average(b['actions'][indices], axis=0, weights=weights)
        if frame >= 210 and len(self.bowl_centers) >= 3:
            top = np.asarray(obs['rgb']['top_camera']).squeeze()
            hsv = cv2.cvtColor(top, cv2.COLOR_RGB2HSV)
            mask = (hsv[:,:,0]>=3)&(hsv[:,:,0]<=24)&(hsv[:,:,1]>110)&(hsv[:,:,2]>150)
            yy, xx = np.where(mask)
            if len(xx) >= 30 and np.mean(yy) < 500:
                center = np.array([np.mean(xx),np.mean(yy)])
                target = np.median(self.bowl_centers, axis=0)
                error = target-center
                action[:2] = np.clip(error*np.array([.012,-.012]), -.1, .1)
                if np.linalg.norm(error) > 35:
                    action[2] = max(0.,action[2])
                self.servo_steps += 1
                self.last_diagnostic.update(bowl=target.tolist(),cloth=center.tolist(),servo_error=float(np.linalg.norm(error)))
        return np.clip(action, -1., 1.).astype(np.float32)

_policy = None

def reset_episode():
    global _policy
    if _policy is None:
        _policy = Controller()
    else:
        _policy.reset_episode()

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