"""Task-local offline reference controller with image feedback and grip gates.""" from pathlib import Path import importlib.util import numpy as np ROOT=Path(__file__).resolve().parent def module(name): s=importlib.util.spec_from_file_location('_'+name,ROOT/(name+'.py'));m=importlib.util.module_from_spec(s);s.loader.exec_module(m);return m features=module('features');geometry=module('geometry') _memory=None _targets=None _wrists=None _initial=None def reset_episode(): global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count _frame=1;_selected=None;_ids=None;_last_grip=np.ones(2,np.float32);_last_kind=np.zeros(2,np.int8);_wrist_previous=None;_gate_count=0 def load(): global _memory,_targets,_wrists,_initial if _memory is None: with np.load(ROOT/'memory.npz',allow_pickle=False) as f:_memory={k:f[k] for k in f.files} with np.load(ROOT/'targets.npz',allow_pickle=False) as f:_targets={k:f[k] for k in f.files} _wrists=np.load(ROOT/'wrists.npz',allow_pickle=False)['wrist'] _initial=np.load(ROOT/'initial_geometry.npz',allow_pickle=False)['pot'] def hold(q=None): a=np.zeros(14,np.float32);a[[6,13]]=_last_grip if q is None else np.clip(2*(q[[7,16]]+.01)/.05-1,-1,1);return a def act(obs): global _frame,_selected,_ids,_last_grip,_last_kind,_wrist_previous,_gate_count if 'observation' in obs: outer=obs;obs=dict(outer['observation']);obs['step_id']=outer.get('step_id',obs.get('step_id',1)) if int(obs.get('step_id',1))>=1000:return None try:q,v=features.proprio(obs['proprioception']) except (KeyError,TypeError,ValueError):return hold() try: rgb=obs['rgb'] if any(np.std(features.rgb_array(rgb[c]))<2 for c in features.CAMERAS):return hold(q) im=features.visual(rgb) except (KeyError,TypeError,ValueError):return hold(q) load();m=_memory;pot=geometry.pot_geometry(rgb['top_camera']);cubes=geometry.cube_positions(rgb['top_camera']) if _selected is None: ids=np.flatnonzero(m['frame']==0);delta=_initial-pot prior=200*delta[:,0]**2+350*delta[:,1]**2+.75*np.sum(delta[:,2:4]**2,axis=1) if not pot[4]:prior=np.zeros(len(_initial)) score=35*np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1)+prior[m['demo'][ids]] _selected=int(m['demo'][ids[np.argmin(score)]]);_ids=np.flatnonzero(m['demo']==_selected) if _frame>int(m['frame'][_ids[-1]])+25:return None # Keep demonstrated order; observation matching refines only a narrow local window. ids=_ids[np.abs(m['frame'][_ids]-min(_frame,m['frame'][_ids[-1]]))<=2] scales=np.tile([.35]*7+[.025,.025],2) qd=np.mean(((m['q'][ids]-q)/scales)**2,axis=1) vd=np.mean((m['image'][ids].astype(np.float32)-im)**2,axis=1) score=qd+20*vd+.08*(m['frame'][ids]-_frame)**2 best=int(ids[np.argmin(score)]);a=m['action'][best].copy();kind=_targets['kind'][best].copy() live=geometry.track_wrists(rgb['top_camera'],_wrist_previous) if _wrist_previous is None:_wrist_previous=np.array([[240,55],[240,582]],np.float32) correction=np.zeros((2,2),np.float32);gates=np.zeros(2,bool) for r in range(2): closing=_last_grip[r]>0 and a[6+7*r]<0 opening=_last_grip[r]<0 and a[6+7*r]>0 active_kind=int(kind[r]) if (closing or opening) and active_kind==0:active_kind=int(_last_kind[r]) if live[r,2]: _wrist_previous[r]=live[r,:2] if _wrists[best,r,2]: correction[r]=.0045*(_wrists[best,r,:2]-live[r,:2])*np.array([1,-1]) correction[r]+=geometry.target_offset(active_kind,r,pot,cubes,_targets['pot'][best],_targets['cubes'][best]) # Small horizontal correction before a contact-changing command. if closing and active_kind in (1,2,3) and np.linalg.norm(correction[r])>.036:gates[r]=True if opening and int(_last_kind[r])==4 and pot[4] and cubes[r,2]: delta=(pot[:2]-cubes[r,:2])*np.array([480,640]) held=True if live[r,2] and _wrists[best,r,2] and _targets['cubes'][best,r,2]: rel=cubes[r,:2]*[480,640]-live[r,:2] refrel=_targets['cubes'][best,r,:2]*[480,640]-_wrists[best,r,:2] held=np.linalg.norm(rel-refrel)<35 if held and np.linalg.norm(delta)>35: gates[r]=True;correction[r]=.0045*delta*np.array([1,-1]) a[7*r:7*r+2]+=np.clip(correction[r],-.06,.06) if gates.any() and _gate_count<30: # Pause the reference clock and other motion; no repeated descent while aligning. a=hold();a[[6,13]]=_last_grip for r in range(2): if gates[r]:a[7*r:7*r+2]=np.clip(correction[r],-.06,.06) _gate_count+=1 else: _gate_count=0;_frame+=1;_last_kind=kind a=np.clip(np.nan_to_num(a),-1,1).astype(np.float32);_last_grip=a[[6,13]].copy() return a reset_episode()