Download astra/DualArmPushObject-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 2.44 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPushObject-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmPushObject-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPushObject-v1/controller.py
2.44 kB
| """D20 linear trajectory interpolation with measured-joint pose feedback. | |
| Only fixed geometry, color localization and linear algebra; no neural model. | |
| """ | |
| 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('_task_'+name,_ROOT/(name+'.py'));m=importlib.util.module_from_spec(s);s.loader.exec_module(m);return m | |
| _loc=module('localization');_kin=module('kinematics') | |
| _db=None;_layout=None;_t=0;_last=None;_diagnostics={} | |
| GAIN=3.0 | |
| PUSH_EXTRA=12 | |
| PRESS_DEPTH=0.0 | |
| HOLD_TUBE=False | |
| def reset_episode(): | |
| global _layout,_t,_last,_diagnostics | |
| _layout=None;_t=0;_last=None;_diagnostics={} | |
| def act(obs): | |
| global _db,_layout,_t,_last,_diagnostics | |
| step=int(obs['step_id']) | |
| if step>=400:return None | |
| if _last is not None and step<=_last:reset_episode() | |
| _last=step | |
| if _db is None: | |
| with np.load(_ROOT/'trajectory.npz') as z:_db={k:z[k] for k in z.files} | |
| if _layout is None: | |
| f=_loc.locate(obs['rgb'])[4:10] | |
| _layout=np.r_[1,np.clip((f-_db['mean'])/.02,-3,3)] | |
| t=_t | |
| if t>=351+PUSH_EXTRA:return None | |
| ref=min(t,332) if t<333+PUSH_EXTRA else t-PUSH_EXTRA | |
| a=(_layout@_db['action'][ref]).copy() | |
| target=(_layout@_db['position'][ref]).reshape(2,3) | |
| yawtarget=_layout@_db['yaw'][ref] | |
| if 333<=t<333+PUSH_EXTRA: | |
| a[:6]=0;a[7:13]=0;a[8]=.04 | |
| target[1,0]+=(t-332)*.0022 | |
| elif t>=333+PUSH_EXTRA: | |
| target[1,0]+=PUSH_EXTRA*.0022 | |
| if HOLD_TUBE and ref>=207: | |
| a[:6]=0;a[6]=-1 | |
| target[0]=(_layout@_db["position"][206]).reshape(2,3)[0] | |
| yawtarget[0]=(_layout@_db["yaw"][206])[0] | |
| for r in range(2): | |
| depth=PRESS_DEPTH*max(0,min(1,(ref-20)/5,(65-ref)/7)) | |
| if r==1:depth=max(depth,PRESS_DEPTH*max(0,min(1,(ref-95)/5,(139-ref)/7))) | |
| target[r,2]-=depth | |
| errors=[] | |
| for r in range(2): | |
| q=np.asarray(obs['proprioception'][f'agent/panda-{r}/qpos']).reshape(-1) | |
| T=_kin.pose(q);err=target[r]-T[:3,3];errors.append(float(np.linalg.norm(err))) | |
| correction=np.array([err[1],-err[0],err[2]]) if r==0 else np.array([-err[1],err[0],err[2]]) | |
| a[r*7:r*7+3]+=np.clip(GAIN*correction,-.06,.06) | |
| R=T[:3,:3]@_db['initial_rotation'][r].T;yaw=np.arctan2(R[1,0],R[0,0]);err_yaw=(yawtarget[r]-yaw+np.pi)%(2*np.pi)-np.pi | |
| a[r*7+5]+=np.clip(-.6*err_yaw,-.04,.04) | |
| a[[6,13]]=np.where(a[[6,13]]>0,1.,-1.) | |
| _t+=1;_diagnostics={'reference_frame':ref,'phase_estimate':t,'pose_errors_m':errors,'reason':'pose_feedback'} | |
| return np.clip(a,-1,1).astype(np.float32) | |