Download astra/DualArmPressButton-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 3.65 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPressButton-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmPressButton-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPressButton-v1/controller.py
3.65 kB
| """Unrehearsed D20 observation-conditioned action interpolation; no neural model.""" | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| ROBOTS = ('panda_leaphand-1', 'panda_leaphand_left-0') | |
| CAMERAS = ('front_camera', 'top_camera') | |
| _memory = None | |
| def reset_episode(): | |
| """No trajectory index or scene identity is retained between calls/episodes.""" | |
| global _initial_target | |
| _initial_target = None | |
| def image_features(rgb): | |
| appearance, targets, visible = [], [], [] | |
| for cam in CAMERAS: | |
| im = np.asarray(rgb[cam]) | |
| if im.ndim == 4 and im.shape[0] == 1: | |
| im = im[0] | |
| if im.ndim != 3 or im.shape[2] != 3 or im.dtype != np.uint8: | |
| raise ValueError('Expected uint8 HWC or 1HWC RGB') | |
| appearance.extend((cv2.resize(im, (32, 24), interpolation=cv2.INTER_AREA).astype(np.float32)/255).ravel()) | |
| h, w = im.shape[:2] | |
| hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV) | |
| mask = cv2.inRange(hsv, np.array([95,70,45]), np.array([135,255,255])) | |
| # Spatial ordering is image-relative, not a robot/world axis conversion. | |
| for half in range(2): | |
| region = mask.copy() | |
| if cam == 'front_camera': | |
| region[:int(.30*h)] = 0 | |
| region[int(.82*h):] = 0 | |
| region[:, :w//2 if half else 0] = 0 | |
| if half == 0: region[:, w//2:] = 0 | |
| else: | |
| if half == 0: region[h//2:] = 0 | |
| else: region[:h//2] = 0 | |
| count, labels, stats, centers = cv2.connectedComponentsWithStats(region) | |
| ids = [i for i in range(1,count) if stats[i,cv2.CC_STAT_AREA] >= 3] | |
| if ids: | |
| i = max(ids, key=lambda j: stats[j,cv2.CC_STAT_AREA]) | |
| targets.extend(centers[i]/[w,h]); visible.extend([1,1]) | |
| else: | |
| targets.extend([0,0]); visible.extend([0,0]) | |
| return (np.asarray(appearance,np.float32), np.asarray(targets,np.float32), np.asarray(visible,np.float32)) | |
| def _load(): | |
| global _memory | |
| if _memory is None: | |
| with np.load(Path(__file__).with_name('demo_memory.npz'), allow_pickle=False) as f: | |
| _memory = {k:f[k] for k in f.files} | |
| return _memory | |
| _initial_target = None | |
| _regdata = None | |
| def act(obs): | |
| global _initial_target, _regdata | |
| if int(obs.get('step_id',1)) >= 200: return None | |
| out = np.zeros(44,np.float32) | |
| try: | |
| q = np.concatenate([np.asarray(obs['proprioception']['agent/'+r+'/qpos']).reshape(-1) for r in ROBOTS]) | |
| _, target, valid = image_features(obs['rgb']) | |
| except (KeyError,ValueError,TypeError): return out | |
| if _initial_target is None: | |
| if valid.sum()!=8: return out | |
| _initial_target = target.copy() | |
| m = _load() | |
| if _regdata is None: | |
| initial = m['target'][m['frame_index']==0] | |
| _regdata = initial[m['demo_index']] | |
| for arm,ts in [(0,[2,3,4,5]),(1,[0,1,6,7])]: | |
| qs=slice(arm*23,arm*23+7) | |
| # Locally affine non-neural imitation from measured joints and fixed target localization. | |
| dx=np.concatenate([(m['q'][:,qs]-q[qs])/.15,(_regdata[:,ts]-_initial_target[ts])/.025],axis=1) | |
| dist=np.mean(dx[:,:7]**2,axis=1)+np.mean(dx[:,7:]**2,axis=1)*2 | |
| ids=np.argpartition(dist,119)[:120] | |
| x=np.column_stack([np.ones(len(ids)),dx[ids]]) | |
| w=np.exp(-(dist[ids]-dist[ids].min())/max(.1,float(np.median(dist[ids])))) | |
| reg=np.eye(x.shape[1])*.015;reg[0,0]=.00001 | |
| y=m['action'][ids,arm*22:arm*22+3] | |
| coef=np.linalg.solve(x.T@(w[:,None]*x)+reg,x.T@(w[:,None]*y)) | |
| out[arm*22:arm*22+3]=np.clip(coef[0],-.06,.06) | |
| return out | |