Download astra/DualArmFlipTable-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 6.86 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmFlipTable-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmFlipTable-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmFlipTable-v1/controller.py
6.86 kB
| """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) | |