Download astra/DualArmButtonPress-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 2.95 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmButtonPress-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmButtonPress-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmButtonPress-v1/controller.py
2.95 kB
| """D20 action interpolation with E20-tested wrist lateral alignment. | |
| No environment imports, kinematics, hidden state, seed lookup or neural weights. | |
| """ | |
| from pathlib import Path | |
| import importlib.util | |
| import numpy as np | |
| _spec=importlib.util.spec_from_file_location('_d20_features',Path(__file__).with_name('features.py')) | |
| _features=importlib.util.module_from_spec(_spec) | |
| _spec.loader.exec_module(_features) | |
| _library=None | |
| _cache={} | |
| def reset_episode(): | |
| global _cache | |
| _cache={} | |
| def _hold(q): | |
| a=np.zeros(14,np.float32) | |
| for i in range(2): | |
| g=2*(float(q[i][7])+0.01)/0.05-1 | |
| if not np.isfinite(g) or not -1<=g<=1: | |
| return None # Never clip an invalid hold into another command. | |
| a[i*7+6]=g | |
| return a | |
| def act(obs): | |
| global _library | |
| step=int(obs['step_id']) | |
| if step < 1 or step >= 250: return None | |
| if _library is None: | |
| with np.load(Path(__file__).with_name('library.npz'),allow_pickle=False) as f: | |
| _library={k:f[k] for k in ('q','v','targets','wrist','actions')} | |
| p=obs['proprioception'] | |
| try: | |
| q=[np.asarray(p[f'agent/panda-{i}/qpos'],dtype=np.float32).reshape(-1) for i in range(2)] | |
| v=[np.asarray(p[f'agent/panda-{i}/qvel'],dtype=np.float32).reshape(-1) for i in range(2)] | |
| if any(len(x)!=9 or not np.isfinite(x).all() for x in q+v):return None | |
| targets,wrist=_features.visual(obs['rgb'],_cache) | |
| except (KeyError,ValueError,TypeError):return None | |
| if not np.isfinite(targets).all():return _hold(q) | |
| db=_library | |
| # Baseline demonstration distances, plus the E20 wrist alignment below. | |
| dt=np.mean(((db['targets']-targets)/0.04)**2,axis=1) | |
| result=[] | |
| for i in range(2): | |
| dq=np.mean(((db['q'][:,i]-q[i][:7])/0.18)**2,axis=1) | |
| dv=np.mean(((db['v'][:,i]-v[i][:7])/1.0)**2,axis=1) | |
| w=db['wrist'][:,i]; live=wrist[i] | |
| both=(w[:,3]>.5)&(live[3]>.5) | |
| dw=np.where(both,np.mean(((w[:,:3]-live[:3])/np.array([.12,.12,.15]))**2,axis=1),0) | |
| dw+=2.0*(w[:,3]-live[3])**2 | |
| distance=dq+dt+0.15*dv+0.5*dw | |
| # Refuse to extrapolate far outside observed joint/target support. | |
| if np.min(dq+dt)>30:return _hold(q) | |
| idx=np.argpartition(distance,15)[:16] | |
| weight=np.exp(-(distance[idx]-distance[idx].min())/0.6) | |
| weight/=weight.sum() | |
| result.extend(weight@db['actions'][idx,i*7:(i+1)*7]) | |
| a=np.asarray(result,np.float32) | |
| if a.shape!=(14,) or not np.isfinite(a).all():return None | |
| # Legal demo finite differences show positive root-y commands move the | |
| # button right in the wrist image. Correct lateral alignment before descent. | |
| for i in range(2): | |
| u,v,area,present=wrist[i] | |
| if present>.5 and area>.02 and .15<v<.8: | |
| error=float(u)-.5 | |
| a[i*7+1]+=np.clip(-.2*error,-.08,.08) | |
| if abs(error)>.15: | |
| a[i*7]=0. | |
| a[i*7+2]=0. | |
| return np.clip(a,-1,1) | |