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