Download astra/DualArmOpenFridge-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 10.8 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmOpenFridge-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmOpenFridge-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmOpenFridge-v1/controller.py
10.8 kB
| """D20 observation-conditioned exemplar controller with bounded grasp recovery; no learned network.""" | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| CAMERAS = ('front_camera', 'left_camera', 'robot_camera_0', 'robot_camera_1', 'top_camera') | |
| PATHS = tuple(f'agent/mobile_panda-{r}/{kind}' for r in (0, 1) for kind in ('qpos', 'qvel')) | |
| # Order in the contract: robot 0 [base(3), arm(6), gripper], then robot 1. | |
| BASE = np.array([0, 1, 2, 10, 11, 12]) | |
| NORMALIZED = np.array([3,4,5,6,7,8,9,13,14,15,16,17,18,19]) | |
| _bank = None | |
| _scene_ready = False | |
| _object_votes = [] | |
| _object_mode = None | |
| _object_centers = [] | |
| _object_adjusted = False | |
| _scene_target = None | |
| _grasp_settle = 0 | |
| _grasp_target = None | |
| _grasp_retry = False | |
| _progress = 0.0 | |
| _terminal_count = 0 | |
| _last_step = 0 | |
| _last_action = None | |
| def visual_feature(rgb): | |
| parts = [] | |
| for camera in CAMERAS: | |
| a = np.asarray(rgb[camera]) | |
| if a.ndim == 4 and a.shape[0] == 1: | |
| a = a[0] | |
| if a.ndim != 3 or a.shape[2] != 3 or a.dtype != np.uint8: | |
| raise ValueError('Expected uint8 HWC or 1HWC RGB') | |
| parts.append(cv2.resize(a, (16, 12), interpolation=cv2.INTER_AREA).reshape(-1)) | |
| return np.concatenate(parts) | |
| def reset_episode(): | |
| global _progress, _terminal_count, _last_step, _last_action, _bank, _scene_ready, _object_votes, _object_mode | |
| global _grasp_settle, _grasp_target, _grasp_retry | |
| _grasp_target, _grasp_retry = None, False | |
| _grasp_settle = 0 | |
| global _object_centers, _object_adjusted, _scene_target | |
| _object_centers, _object_adjusted, _scene_target = [], False, None | |
| _bank, _scene_ready = None, False | |
| _object_votes, _object_mode = [], None | |
| _progress, _terminal_count, _last_step, _last_action = 0.0, 0, 0, None | |
| def _load(): | |
| global _bank | |
| if _bank is None: | |
| with np.load(Path(__file__).with_name('exemplars.npz'), allow_pickle=False) as f: | |
| _bank = {k: f[k] for k in f.files} | |
| return _bank | |
| def localize_scene(obs, b): | |
| global _scene_ready, _scene_target | |
| if _scene_ready: return b | |
| a = np.asarray(obs['rgb']['top_camera']).squeeze() | |
| a = cv2.resize(a,(240,320),interpolation=cv2.INTER_AREA) | |
| mask = (np.min(a,axis=2)>225).astype('uint8'); mask[130:]=0 | |
| n, labels, stats, cent = cv2.connectedComponentsWithStats(mask) | |
| if n < 2: return b | |
| i = 1+np.argmax(stats[1:,4]); x,y,w,h,area=stats[i] | |
| if area<500 or w<20: return b | |
| g=np.load(Path(__file__).with_name('geometry.npz')) | |
| target=np.array([x+w/2,y+h,1])@g['W'] | |
| _scene_target=target.copy() | |
| offsets=target-g['targets'][b['demo'].astype(int)] | |
| ph=b['phase'] | |
| f0=np.minimum(ph/.5,1)*np.clip((4-ph)/.45,0,1) | |
| f1=np.clip((ph-1.42)/.4,0,1) | |
| for j in (0,1): | |
| b['q'][:,j] += offsets[:,j]*f0 | |
| b['a'][:,j] += offsets[:,j]*f0 | |
| b['q'][:,12+j] += offsets[:,j]*f1 | |
| b['a'][:,10+j] += offsets[:,j]*f1 | |
| _scene_ready=True | |
| return b | |
| def detect_object(obs): | |
| global _object_mode, _object_votes | |
| if not (1.98 < _progress < 2.5): return | |
| a=cv2.resize(np.asarray(obs['rgb']['front_camera']).squeeze(),(320,240),interpolation=cv2.INTER_AREA).astype(float) | |
| r,g,bl=a.transpose(2,0,1) | |
| mask=(bl>r+3)&(bl>g+2)&(r<60)&(g<60)&(bl<100) | |
| mask[:60]=False;mask[132:]=False;mask[:,:110]=False;mask[:,185:]=False | |
| y,x=np.where(mask) | |
| if len(y)>=5: | |
| _object_centers.append([float(np.median(x)),float(np.median(y))]) | |
| _object_votes.append(int(np.median(y)<110)) | |
| if len(_object_votes)>=3: _object_mode=int(np.mean(_object_votes[-15:])>=.5) | |
| def refine_object(b): | |
| global _object_adjusted | |
| if _object_adjusted or _object_mode is None or _scene_target is None or _progress<2.25: return | |
| og=np.load(Path(__file__).with_name('object_geometry.npz')) | |
| center=np.median(_object_centers[-15:],axis=0) | |
| target=np.r_[center,_scene_target,1]@og['weights'][_object_mode] | |
| demos=np.flatnonzero(og['mode']==_object_mode) | |
| # Restrict interpolation to the observed within-shelf pose envelope. | |
| target[3:]=np.clip(target[3:],og['targets'][demos,3:].min(0),og['targets'][demos,3:].max(0)) | |
| target[:3]=np.clip(target[:3],b['base_low'][:3],b['base_high'][:3]) | |
| for di in demos: | |
| ids=np.flatnonzero(b['demo']==di) | |
| grasp=ids[np.flatnonzero(b['phase'][ids]>=3)[0]] | |
| offset=target-b['q'][grasp,:10] | |
| f=np.clip((b['phase'][ids]-2.08)/.62,0,1)*np.clip((3.6-b['phase'][ids])/.4,0,1) | |
| b['q'][ids,:10]+=f[:,None]*offset | |
| b['a'][ids,:3]+=f[:,None]*offset[:3] | |
| _object_adjusted=True | |
| def _hold(q): | |
| a = np.zeros(20, dtype=np.float32) | |
| for r in (0, 1): | |
| a[10*r:10*r+3] = q[12*r:12*r+3] | |
| a[10*r+9] = np.clip(2*(q[12*r+10]+0.01)/0.05-1, -1, 1) | |
| return a | |
| def act(obs): | |
| """Return a 20-vector or None; step 1 follows the runner-owned hold. | |
| Base bounds are absent in the supplied contract. Base outputs lie within | |
| the observed demonstration envelope, except measured-position fallback | |
| holds. Neither constitutes verification of unknown native limits. | |
| """ | |
| global _progress, _terminal_count, _last_step, _last_action, _grasp_settle, _grasp_target, _grasp_retry | |
| step = int(obs.get('step_id', 1)) | |
| if step < _last_step: | |
| reset_episode() | |
| if step == _last_step and _last_action is not None: | |
| return _last_action.copy() | |
| # 650 includes runner's initial hold: at most 649 controller actions. | |
| if step >= 650: | |
| return None | |
| try: | |
| p = obs['proprioception'] | |
| arrays = [np.asarray(p[path], dtype=np.float32).reshape(-1) for path in PATHS] | |
| if any(a.size != 12 or not np.isfinite(a).all() for a in arrays): | |
| return None | |
| q = np.concatenate((arrays[0], arrays[2])) | |
| v = np.concatenate((arrays[1], arrays[3])) | |
| except (KeyError, TypeError, ValueError): | |
| return None | |
| b = _load() | |
| try: b = localize_scene(obs,b) | |
| except (KeyError, ValueError, cv2.error): pass | |
| hold = _hold(q) | |
| try: | |
| image = visual_feature(obs['rgb']).astype(np.float32)/255.0 | |
| except (KeyError, TypeError, ValueError, cv2.error): | |
| _last_step, _last_action = step, hold | |
| return hold.copy() | |
| try: detect_object(obs) | |
| except (KeyError,ValueError,cv2.error): pass | |
| refine_object(b) | |
| # Progress is inferred from current observations across all 20 demos; | |
| # it is never set from an episode ID, hidden seed, or step schedule. | |
| eligible = np.flatnonzero((b['phase'] >= _progress-0.015) & | |
| (b['phase'] <= _progress+0.45)) | |
| if _object_mode is not None and _progress>2.08: | |
| og=np.load(Path(__file__).with_name('object_geometry.npz')) | |
| eligible=eligible[og['mode'][b['demo'][eligible].astype(int)]==_object_mode] | |
| dq = (b['q'][eligible]-q)/b['qscale'] | |
| qcost = np.mean(np.minimum(dq*dq, 100), axis=1) | |
| dv = (b['v'][eligible]-v)/b['vscale'] | |
| vcost = np.mean(np.minimum(dv*dv, 25), axis=1) | |
| preliminary = qcost + 0.03*vcost | |
| k = min(96, len(eligible)) | |
| short = np.argpartition(preliminary, k-1)[:k] | |
| ids = eligible[short] | |
| visual = np.mean((b['rgb'][ids].astype(np.float32)/255.0-image)**2, axis=1) | |
| continuity = 0.12*(b['phase'][ids]-(_progress+0.025))**2 | |
| cost = preliminary[short] + 12*visual + continuity | |
| best_local = int(np.argmin(cost)); best = ids[best_local] | |
| # Conservative, uncalibrated rejection; does not certify safety/success. | |
| if qcost[short[best_local]] > 2.5 or visual[best_local] > 0.09: | |
| _terminal_count = 0 | |
| _last_step, _last_action = step, hold | |
| return hold.copy() | |
| _progress = min(4.99, max(_progress + 0.004, float(b['phase'][best]))) | |
| # Never blend conflicting gripper commands or widely separated phases. | |
| compatible = (np.all(b['a'][ids][:,[9,19]] == b['a'][best,[9,19]], axis=1) & | |
| (np.abs(b['phase'][ids]-_progress) < 0.045)) | |
| selected = np.flatnonzero(compatible) | |
| selected = selected[np.argsort(cost[selected])[:4]] | |
| weights = np.exp(-(cost[selected]-cost[selected].min())/0.035) | |
| weights /= weights.sum() | |
| # Small demonstrated lookahead avoids stationary nearest-neighbor fixed points. | |
| future = np.minimum(ids[selected]+2, len(b['a'])-1) | |
| future = np.where(b['demo'][future] == b['demo'][ids[selected]], future, ids[selected]) | |
| a = weights @ b['a'][future] | |
| a[[9,19]] = b['a'][best,[9,19]] | |
| if 2.05 < _progress < 3.5: | |
| pm=np.load(Path(__file__).with_name('pose_maps.npz')) | |
| mi=int(np.argmin(abs(pm['centers']-_progress))) | |
| qref=weights @ b['q'][ids[selected]][:,3:10] | |
| correction=(qref-q[3:10])@pm['maps'][mi] | |
| a[3:9] += np.clip(correction,[-.04,-.04,-.04,-.02,-.02,-.02],[.04,.04,.04,.02,.02,.02]) | |
| # Advance demonstrated support-base placement while the handle is opening. | |
| if 1.35 < _progress < 1.99: | |
| support=np.flatnonzero((b['demo']==b['demo'][best])&(b['phase']>=min(1.99,_progress+.18))) | |
| if len(support): a[10:13]=b['a'][support[0],10:13] | |
| # Fixed grasp-pose servo, followed by closure dwell and one empty-grasp retry. | |
| if 2.96 < _progress < 3.18 and _grasp_settle < 50 and _object_mode is not None: | |
| if _grasp_target is None: | |
| og=np.load(Path(__file__).with_name('object_geometry.npz')) | |
| gi=np.flatnonzero((b['phase']>=3.03)&(b['phase']<=3.06)&(og['mode'][b['demo'].astype(int)]==_object_mode)) | |
| err=np.mean(((b['q'][gi,:10]-q[:10])/b['qscale'][:10])**2,axis=1) | |
| _grasp_target=b['q'][gi[np.argmin(err)]].copy() | |
| ref=_grasp_target | |
| pm=np.load(Path(__file__).with_name('pose_maps.npz')) | |
| mi=int(np.argmin(abs(pm['centers']-3))) | |
| a[:3]=ref[:3] | |
| a[3:9]=np.clip(2*(ref[3:10]-q[3:10])@pm['maps'][mi],-.1,.1) | |
| a[9]=1 if _grasp_settle<35 else -1 | |
| _progress=2.97 | |
| _grasp_settle+=1 | |
| if _grasp_settle==50: | |
| if q[10]<.004 and not _grasp_retry: | |
| _grasp_retry=True | |
| _grasp_target[0]-=.025 | |
| _grasp_settle=15 | |
| else: _progress=3.06 | |
| # Base targets remain absolute. Rate limit distance from measured base. | |
| current_base = q[[0,1,2,12,13,14]] | |
| a[BASE] = current_base + np.clip(a[BASE]-current_base, | |
| -b['base_step_cap'], b['base_step_cap']) | |
| a[BASE] = np.clip(a[BASE], b['base_low'], b['base_high']) | |
| a[NORMALIZED] = np.clip(a[NORMALIZED], -1, 1) | |
| # Terminal similarity is only a stop heuristic, never a success signal. | |
| if _progress > 4.85 and a[9] > 0 and visual[best_local] < 0.012 and qcost[short[best_local]] < 0.15: | |
| _terminal_count += 1 | |
| else: | |
| _terminal_count = 0 | |
| if _terminal_count >= 8: | |
| return None | |
| _last_step, _last_action = step, a.astype(np.float32) | |
| return _last_action.copy() | |