"""Task-local demo-reference tracking with visible selection and bounded feedback.""" from pathlib import Path import json,numpy as np,cv2 from visual_features import marker_features,marker_distance ROOT=Path(__file__).parent CONFIG=json.loads((ROOT/'settings.json').read_text()) ROBOTS=('panda_leaphand-1','panda_leaphand_left-0') _STATE={};_BANK=None def _load(): global _BANK if _BANK is None: with np.load(ROOT/'reference_bank.npz') as f:b={k:f[k] for k in f.files} with np.load(ROOT/'numeric_demos.npz') as f:b['numeric']={k:f[k] for k in f.files} b['hand_map']=np.load(ROOT/'hand_map.npy');b['hand_sign']=np.load(ROOT/'hand_sign.npy') b['table_features']=np.load(ROOT/'table_features.npy') b['markers']=np.load(ROOT/'marker_bank.npy');b['feedback']=np.load(ROOT/'pose_feedback.npy') with np.load(ROOT/'geometry_feedback.npz') as f:b['geometry']={k:f[k] for k in f.files} b['indices']=[np.flatnonzero(b['demo_index']==i) for i in range(20)] b['flip']=[int(np.flatnonzero((np.linalg.norm(b['numeric']['a'+str(i)][:,3:6],axis=1)>.25)&(np.arange(len(b['numeric']['a'+str(i)]))>len(b['numeric']['a'+str(i)])*.65))[0]) for i in range(20)] b['gate']=[int(np.flatnonzero(b['numeric']['a'+str(i)][:400,23]<-.04)[-1]) for i in range(20)] _BANK=b return _BANK def reset_episode(): _STATE.clear();_STATE.update(last_step=0,t=-1.,was_recovering=False,demo=None,gate_done=False,recovery=0,bad=0,reason='reset',phase=0.) def image_features(rgb): return np.concatenate([cv2.resize(np.asarray(rgb[c]).squeeze(),(16,12),interpolation=cv2.INTER_AREA).reshape(-1).astype(np.float32)/255 for c in ['front_camera','left_camera']]) def scene_features(m): out=[] for st in [8,20]: p=m[st:st+4].reshape(2,2) if np.any(p<0):return None p=p[np.argsort(p[:,1 if st==20 else 0])];out.extend(p.mean(0));out.extend(p[1]-p[0]) return np.array(out) def observe_scene(m,reference,previous): result=np.asarray(previous if previous is not None else reference,np.float32).copy();visible=0 for cam,st in enumerate([8,20]): p=m[st:st+4].reshape(2,2);p=p[p[:,0]>=0];visible+=len(p);base=result[cam*4:cam*4+4] if len(p)==2: p=p[np.argsort(p[:,cam])];result[cam*4:cam*4+4]=np.r_[p.mean(0),p[1]-p[0]] elif len(p)==1: endpoints=np.stack([base[:2]-.5*base[2:],base[:2]+.5*base[2:]]) side=int(np.argmin(np.sum((endpoints-p[0])**2,axis=1))) result[cam*4:cam*4+2]=p[0]+(.5 if side==0 else -.5)*base[2:] return result,visible def leg_error(m): red=m[:8].reshape(4,2);blue=m[8:12].reshape(2,2) if np.any(red[:,0]<0) or np.any(blue[:,0]<0):return None upper=red[np.argsort(red[:,1])[:2]] return float(np.min(upper[:,0])-np.min(blue[:,0])) def act(obs): if not _STATE:reset_episode() step=int(obs['step_id']);elapsed=step-_STATE['last_step'] if step>=min(2400,CONFIG.get('stop_step',2400)):return None if elapsed<=0:raise ValueError('Non-increasing step') _STATE['last_step']=step;b=_load();p=obs['proprioception'] q=np.concatenate([np.asarray(p['agent/'+r+'/qpos']).reshape(-1) for r in ROBOTS]) if q.shape!=(46,) or not np.isfinite(q).all():return None im=image_features(obs['rgb']);marks=marker_features(obs['rgb']) if _STATE['demo'] is None or step<=9: ids=np.array([x[min(int((step-1)//8),len(x)-1)] for x in b['indices']]) ds=marker_distance(b['markers'][ids],marks)*CONFIG.get('marker_weight',.1)+np.mean(((b['image'][ids]-im)/.15)**2,1) _STATE['demo']=int(np.argmin(ds));_STATE['selection_distance']=float(ds.min()) did=_STATE['demo'];a=b['numeric']['a'+str(did)];qref=b['numeric']['q'+str(did)];n=len(a) if not _STATE['was_recovering']:_STATE['t']+=elapsed*CONFIG.get('speed',1.) t=int(round(_STATE['t']));gate=b['gate'][did];err=leg_error(marks);_STATE['leg_error']=err recovering=False if CONFIG.get('leg_gate',False) and not _STATE['gate_done'] and t>=gate: if err is not None and err<=CONFIG.get('leg_threshold',.025):_STATE['gate_done']=True elif _STATE['recovery']>=CONFIG.get('recovery_limit',80): _STATE['reason']='first-leg recovery exhausted; failed attempt';return None else: recovering=True;_STATE['recovery']+=elapsed;t=gate-20 if t>=n: _STATE['reason']='reference endpoint reached; not a success assertion';return None j=b['indices'][did][min(t//8,len(b['indices'][did])-1)] action=a[t].copy();error=qref[t]-q flipping=t>=b['flip'][did]-4 _STATE['flipping']=bool(flipping) _STATE['geometry_offset_norm']=0. geometry_joint_offset=np.zeros(14,np.float32) if CONFIG.get('geometry_gain',0.) and t>n*.02 and not flipping: reference=b['table_features'][j] current,visible=observe_scene(marks,reference,_STATE.get('table_geometry')) _STATE['table_geometry']=current.tolist();_STATE['geometry_visible_points']=int(visible) previous=np.asarray(_STATE.get('geometry_offset',[0.]*14),np.float32) offset=previous if current is not None and reference is not None: g=b['geometry'];k=int(np.argmin(abs(g['grid']-g['aligned_phase'][j]))) if g['valid'][k]>=8: target=CONFIG['geometry_gain']*((current-reference)@g['jac'][k]) target=np.clip(target,-CONFIG.get('geometry_limit',.1),CONFIG.get('geometry_limit',.1)) alpha=1.-.9**elapsed;offset=previous+alpha*(target-previous) _STATE['geometry_offset']=offset.tolist() geometry_joint_offset=offset _STATE['geometry_offset_norm']=float(np.linalg.norm(offset)) for r in range(2): qs=slice(r*23,r*23+7);aa=slice(r*22,r*22+6) gain=0. if (r==0 and flipping and CONFIG.get('disable_flip_feedback',True)) else CONFIG.get('pose_gain',.1) correction=gain*(error[qs]@b['feedback'][j,r]) correction[3:]=0. # Preserve demonstrated wrist orientation commands. correction[:3]+=gain*(geometry_joint_offset[r*7:r*7+7]@b['feedback'][j,r])[:3] action[aa]+=np.clip(correction,-CONFIG.get('pose_limit',.02),CONFIG.get('pose_limit',.02)) hs=slice(r*23+7,r*23+23);ha=slice(r*22+6,r*22+22) action[ha]+=np.clip(CONFIG.get('hand_gain',.1)*error[hs][b['hand_map'][r]]*b['hand_sign'][r]/.1,-CONFIG.get('hand_limit',.05),CONFIG.get('hand_limit',.05)) if recovering: action[:22]=0 # Continue the demonstrated pull, without pulling back toward an earlier pose. action[22:25]=a[max(gate-30,0),22:25] if recovering:_STATE['t']=float(gate) _STATE['was_recovering']=recovering _STATE['phase']=float(_STATE['t']/n);_STATE['frame']=t _STATE['arm_distance']=float(np.mean((error[:7]/b['q_scale'][:7])**2)) support_error=error.copy() if flipping and CONFIG.get('disable_flip_feedback',True):support_error[:7]=0 _STATE['distance']=float(np.mean((support_error/b['q_scale'])**2)) _STATE['bad']=_STATE['bad']+elapsed if _STATE['distance']>CONFIG.get('pose_abort',3.) else 0 if _STATE['bad']>40: _STATE['reason']='sustained pose deviation; failed attempt';return None _STATE['reason']='visible leg recovery' if recovering else 'reference tracking with joint feedback' return np.clip(action,-1,1).astype(np.float32)