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)
|