Download astra/DualArmPlaceRopeOverRod-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 2.65 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPlaceRopeOverRod-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmPlaceRopeOverRod-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPlaceRopeOverRod-v1/controller.py
2.65 kB
| """D20 observation-conditioned nonparametric controller with bounded phase advance.""" | |
| from pathlib import Path | |
| import importlib.util | |
| import numpy as np | |
| _s=importlib.util.spec_from_file_location('_d20_features',Path(__file__).with_name('features.py')) | |
| _f=importlib.util.module_from_spec(_s);_s.loader.exec_module(_f) | |
| _bank=None | |
| _phase=None | |
| _calls=0 | |
| _bad=0 | |
| _last_step=0 | |
| def reset_episode(): | |
| global _phase,_calls,_bad,_last_step | |
| _phase=None;_calls=0;_bad=0;_last_step=0 | |
| def act(obs): | |
| global _bank,_phase,_calls,_bad,_last_step | |
| step=int(obs.get('step_id',_calls+1)) | |
| if step>=680 or _calls>=679: return None | |
| if _phase is not None and _phase>=.995: return np.zeros(44,dtype=np.float32) | |
| if step<_last_step: reset_episode() | |
| _last_step=step;_calls+=1 | |
| if _bank is None: | |
| with np.load(Path(__file__).with_name('bank.npz'),allow_pickle=False) as z: _bank={k:z[k] for k in z.files} | |
| b=_bank | |
| try: | |
| q=_f.proprio(obs['proprioception'],'qpos');v=_f.proprio(obs['proprioception'],'qvel');im=_f.visual(obs['rgb']) | |
| except (KeyError,ValueError,TypeError): | |
| _bad+=1 | |
| return None if _bad>=20 else np.zeros(44,dtype=np.float32) | |
| _bad=0 | |
| # Equal robot weighting; fingers are less reliable pose indicators under contact. | |
| scales=np.tile(np.r_[np.full(7,.22),np.full(16,.5)],2) | |
| dq=np.mean(((b['q']-q)/scales)**2,axis=1) | |
| dv=np.mean(np.minimum(((b['v']-v)/2)**2,9),axis=1) | |
| eligible=np.arange(len(dq)) | |
| if _phase is not None: | |
| eligible=eligible[(b['phase']>=max(0,_phase-.018)) & (b['phase']<=_phase+.009)] | |
| if len(eligible)<7: eligible=np.arange(len(dq)) | |
| shortlist=eligible[np.argsort((dq+.05*dv)[eligible])[:240]] | |
| score=dq[shortlist]+.05*dv[shortlist]+12*np.mean((b['img'][shortlist]-im)**2,axis=1) | |
| # Soft hysteresis disambiguates similar poses without selecting a fixed demo. | |
| if _phase is not None: | |
| dp=b['phase'][shortlist]-_phase | |
| score+=2*np.maximum(abs(dp)-.08,0)**2 | |
| ix=shortlist[np.argsort(score)[:7]] | |
| dist=np.sort(score)[:7] | |
| if dist[0]>12: return np.zeros(44,dtype=np.float32) | |
| w=np.exp(-(dist-dist.min())/.08);w/=w.sum() | |
| new_phase=float(w@b['phase'][ix]) | |
| _phase=new_phase if _phase is None else float(np.clip(new_phase,_phase+.0015,_phase+.0022)) | |
| a=w@b['a'][ix] | |
| # Hand action labels retain their recorded order; no unverified joint remapping. | |
| target=w@b['q'][ix] | |
| for j,offset in [(0,0),(22,23)]: | |
| # No direct finger correction: qpos and action ordering are unverified. | |
| a[j+3:j+6]=0 # every authorized label has zero rotation | |
| return np.clip(a,-1,1).astype(np.float32) | |