File size: 8,421 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
147
"""D20 nonparametric observation-conditioned controller under E20 exploration."""
from pathlib import Path
import json
import cv2
import numpy as np

ROOT = Path(__file__).resolve().parent
CONFIG = json.loads((ROOT / 'config.json').read_text())
CAMS = ['front_camera', 'top_camera']

def visual_features(rgb):
    blocks=[]
    for camera in CAMS:
        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 HWC RGB')
        small=cv2.resize(im,(24,24),interpolation=cv2.INTER_AREA).astype(np.float32)/255.
        # Color occupancy preserves cup location/tilt separately from background.
        r,g,b=[im[:,:,i].astype(np.float32) for i in range(3)]
        masks=[(b>r*1.25)&(b>g*1.05)&(b>70), (r>b*1.5)&(g>b*1.2)&(r>100)]
        blocks.append(small.ravel())
        blocks.extend(cv2.resize(x.astype(np.float32),(24,24),interpolation=cv2.INTER_AREA).ravel() for x in masks)
    return np.concatenate(blocks)

class Controller:
    def __init__(self):
        with np.load(ROOT/'reference_bank.npz',allow_pickle=False) as z:
            self.bank={k:z[k] for k in z.files}
        self.feedback=np.load(ROOT/'feedback_bank.npz')['maps'] if (ROOT/'feedback_bank.npz').exists() else None
        self.dense=np.load(ROOT/'trajectory_bank.npz') if (ROOT/'trajectory_bank.npz').exists() else None
        self.reset_episode()
    def reset_episode(self):
        self.cursors=np.zeros(20,dtype=np.float32)
        self.previous_step=0
        self.previous_phase_speed=1.0
        self.orange_first=None
        self.locked=None
        self.rotation_level=0.0
        self.last_action=np.zeros(44,dtype=np.float32)
        self.diagnostic={}
    def act(self,obs):
        step=int(obs.get('step_id',self.previous_step+1))
        if 'observation' in obs: obs=obs['observation']
        if step>=CONFIG.get('development_stop',1000): return None
        if step>=CONFIG['native_horizon']: return None
        if step<1: raise ValueError('Runner-owned hold must precede act')
        if step<=self.previous_step: self.reset_episode()
        try:
            q=np.concatenate([np.asarray(obs['proprioception'][p],dtype=np.float32).reshape(-1) for p in CONFIG['proprio_paths']])
            if q.shape!=(92,) or not np.isfinite(q).all(): raise ValueError('Invalid proprioception')
            v=visual_features(obs['rgb'])
        except (KeyError,ValueError,TypeError):
            self.diagnostic={'reason':'missing or invalid legal observation'}
            return np.zeros(44,dtype=np.float32)
        if self.orange_first is None:
            im=cv2.resize(np.asarray(obs['rgb']['top_camera']).squeeze(),(240,320))
            roi=im[172:236,145:205].astype(np.float32)
            r,g,b=roi[:,:,0],roi[:,:,1],roi[:,:,2]
            self.orange_first=bool(np.sum((b>1.2*r)&(b>1.05*g)&(b>45))>100)
        bank=self.bank; elapsed=max(1,step-self.previous_step)
        candidates=[]; scores=[]
        phase_before_action=float(np.max(self.cursors))
        second_pour=bool(phase_before_action>=CONFIG.get('second_pour_phase',400))
        active_source_orange=(not self.orange_first) if second_pour else self.orange_first
        speed_key='orange_phase_speed' if active_source_orange else 'blue_phase_speed'
        speed=CONFIG.get(speed_key,CONFIG.get('phase_speed',1.0)) if step>40 else 1.0
        orange_return_phase=CONFIG.get('orange_return_phase_second',650) if second_pour else CONFIG.get('orange_return_phase_first',275)
        if step>40 and active_source_orange and phase_before_action>=orange_return_phase:
            speed=CONFIG.get('orange_return_speed',speed)
        if step>40 and CONFIG.get('slow_rotation',False) and self.rotation_level>0.015: speed=min(speed,CONFIG.get('rotation_speed',1.5))
        for demo in range(20):
            refs=CONFIG.get('preferred_refs')
            if refs is not None and demo!=refs[1 if self.orange_first else 0]: continue
            if self.locked is not None and demo!=self.locked: continue
            if (demo in [1,2,6,7,11,13,14,16,17,19]) != self.orange_first: continue
            advance_speed=self.previous_phase_speed if CONFIG.get('fixed_phase',False) else speed
            expected=self.cursors[demo]+(elapsed*advance_speed if self.previous_step else 0)
            ids=np.flatnonzero((bank['demo']==demo)&(bank['frame']>=max(0,expected-12))&(bank['frame']<=expected+20))
            if not len(ids): continue
            dq=(bank['proprio'][ids]-q)/bank['scale']
            # Equalized joint groups, light velocity influence, visual cup emphasis.
            qcost=np.mean(dq*dq,axis=1)
            vcost=np.mean((bank['visual'][ids]-v)**2,axis=1)
            cost=qcost+12*vcost+0.025*((bank['frame'][ids]-expected)/12)**2
            best=int(np.argmin(cost))
            if CONFIG.get('fixed_phase',False):
                best=int(np.argmin(np.abs(bank['frame'][ids]-expected)))
                self.cursors[demo]=expected
            else:
                self.cursors[demo]=max(self.cursors[demo]+0.9*elapsed*speed,bank['frame'][ids[best]])
            candidates.append(ids[best]);scores.append(cost[best])
        if not candidates: return None
        candidates=np.asarray(candidates); scores=np.asarray(scores)
        order=np.argsort(scores); anchor=candidates[order[0]]
        if CONFIG.get('lock_reference',False) and step>=40 and self.locked is None: self.locked=int(bank['demo'][anchor])
        # Blend only nearby action directions; avoid averaging opposite pouring roles.
        compatible=np.mean((bank['action'][candidates]-bank['action'][anchor])**2,axis=1)<0.04
        chosen=order[compatible[order]][:3]
        weights=np.exp(-(scores[chosen]-scores[order[0]])/0.08); weights/=weights.sum()
        action=np.sum(bank['action'][candidates[chosen]]*weights[:,None],axis=0)
        target_q=bank['proprio'][anchor]
        if self.dense is not None and CONFIG.get('dense_phase',False):
            demo=int(bank['demo'][anchor]); phase=float(self.cursors[demo])
            aa=self.dense[f'a{demo}']; qq=self.dense[f'q{demo}']
            lo=min(int(phase),len(aa)-1); frac=phase-lo
            target_q=qq[lo]*(1-min(frac,1))+qq[min(lo+1,len(qq)-1)]*min(frac,1)
            end=min(phase+speed,float(len(aa)))
            action=np.zeros(44,dtype=np.float32)
            weight=0.0
            for t in range(lo,min(int(np.ceil(end)),len(aa))):
                w=max(0.0,min(end,t+1)-max(phase,t))
                action+=aa[t]*w; weight+=w
            if weight>0: action/=weight
        self.rotation_level=max(float(np.linalg.norm(action[3:6])),float(np.linalg.norm(action[25:28])))
        if step>40 and CONFIG.get('slow_rotation',False) and self.rotation_level>0.015: speed=min(speed,CONFIG.get('rotation_speed',1.5))
        for arm,off,ao in [(0,0,0),(1,46,22)]:
            action[ao:ao+6]*=speed
            feedback_gain=CONFIG.get('orange_feedback_gain' if active_source_orange else 'blue_feedback_gain',CONFIG.get('feedback_gain',0))
            if self.feedback is not None and feedback_gain>0 and step>40:
                error=target_q[off:off+7]-q[off:off+7]
                correction=error@self.feedback[arm,anchor]*feedback_gain
                action[ao:ao+6]+=np.clip(correction,-0.12,0.12)
            if CONFIG.get('hand_feedback_gain',0)>0:
                hand_error=target_q[off+7:off+23]-q[off+7:off+23]
                action[ao+6:ao+22]+=CONFIG['hand_feedback_gain']*hand_error/0.1
        if scores[order[0]]>8:
            action=np.zeros(44,dtype=np.float32)
        self.previous_step=step
        self.previous_phase_speed=speed
        self.last_action=np.clip(action,CONFIG['action_low'],CONFIG['action_high']).astype(np.float32)
        self.diagnostic={'match_cost':float(scores[order[0]]),'reference_demo':int(bank['demo'][anchor]),'reference_frame':int(bank['frame'][anchor]),'orange_first':self.orange_first,'status':'unverified observation match'}
        # No success detector is claimed: let native horizon bound the attempt.
        return self.last_action.copy()

_controller=None

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

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