Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
8.42 kB
"""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)