Download astra/DualArmPushBox-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 6.69 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPushBox-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmPushBox-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPushBox-v1/controller.py
6.69 kB
| """D20 observation-conditioned lookup with bounded image feedback. | |
| No simulator, kinematics package, neural weights, seed, or timed demo replay. | |
| """ | |
| from pathlib import Path | |
| import cv2 | |
| import numpy as np | |
| CAMERAS = ('top_camera', 'front_camera') | |
| _BANK = None | |
| _last_action = None | |
| _unsupported = 0 | |
| _target_y = .5 | |
| last_diagnostics = {} | |
| def reset_episode(): | |
| global _last_action, last_diagnostics, _unsupported, _target_y | |
| _last_action = None | |
| _unsupported = 0 | |
| _target_y = .5 | |
| last_diagnostics = {'status': 'reset'} | |
| def features(obs): | |
| """Fixed hand-designed metric, shared verbatim by compiler and runtime.""" | |
| parts = [] | |
| blue_counts = [] | |
| for camera in CAMERAS: | |
| im = np.asarray(obs['rgb'][camera]) | |
| 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') | |
| r, g, b = [im[..., i].astype(np.float32) for i in range(3)] | |
| masks = [(b > 65) & (b > r * 1.6 + 15) & (b > g * 1.4 + 15), | |
| (r > 65) & (r > b * 1.5 + 15) & (r > g * 1.4 + 15)] | |
| blue_counts.append(int(masks[0].sum())) | |
| # Spatial masks preserve visible box shape and target location. Missing | |
| # red pixels remain a visibility feature; target may be occluded. | |
| for mask in masks: | |
| small = cv2.resize(mask.astype(np.float32), (16, 16), interpolation=cv2.INTER_AREA) | |
| parts.append(small.ravel() * (8.0 / 16.0)) | |
| yy, xx = np.nonzero(mask) | |
| if len(xx) >= 8: | |
| x, y = xx / im.shape[1], yy / im.shape[0] | |
| geom = [x.mean(), y.mean(), x.std(), y.std(), | |
| np.mean((x-x.mean())*(y-y.mean())), len(xx)/mask.size, 1.0] | |
| else: | |
| geom = [0.] * 7 | |
| parts.append(np.asarray(geom, np.float32) * np.array([12,12,6,6,20,10,1],np.float32)) | |
| small_rgb = cv2.resize(im, (16,16), interpolation=cv2.INTER_AREA).astype(np.float32)/255 | |
| parts.append(small_rgb.ravel() * (1.0 / np.sqrt(768))) | |
| for kind, scale in [('qpos', .20), ('qvel', 1.0)]: | |
| q = [] | |
| for robot in range(2): | |
| v = np.asarray(obs['proprioception'][f'agent/panda-{robot}/{kind}'],dtype=np.float32).reshape(-1) | |
| if v.size != 9 or not np.isfinite(v).all(): | |
| raise ValueError('Expected nine finite proprioceptive values per robot') | |
| q.extend(v[:7]) | |
| parts.append(np.array(q,np.float32)/(scale*np.sqrt(14))) | |
| f = np.concatenate(parts).astype(np.float32) | |
| if not np.isfinite(f).all(): | |
| raise ValueError('Nonfinite features') | |
| return f, min(blue_counts) >= 8 | |
| def _load(): | |
| global _BANK | |
| if _BANK is None: | |
| with np.load(Path(__file__).with_name('demo_bank.npz'), allow_pickle=False) as d: | |
| _BANK = {k: d[k] for k in d.files} | |
| return _BANK | |
| def box_pose(im): | |
| r,g,b=[im[...,i].astype(float) for i in range(3)] | |
| mask=((b>65)&(b>1.6*r+15)&(b>1.4*g+15)).astype(np.uint8) | |
| contours,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) | |
| if not contours:return None | |
| contour=max(contours,key=cv2.contourArea) | |
| (x,y),(w,h),angle=cv2.minAreaRect(contour) | |
| pts=cv2.boxPoints(((x,y),(w,h),angle));edges=np.roll(pts,-1,axis=0)-pts;v=edges[np.argmax(np.linalg.norm(edges,axis=1))] | |
| if v[1]<0:v=-v | |
| tilt=np.arctan2(v[0],v[1]) | |
| return dict(x=float(x/im.shape[1]),y=float(y/im.shape[0]),tilt=float(tilt),area=float(cv2.contourArea(contour)/mask.size)) | |
| def act(obs): | |
| global _last_action, last_diagnostics, _unsupported, _target_y | |
| # Native horizon includes the runner-owned reset hold. No extra hold here. | |
| if int(obs.get('step_id', 1)) >= 700: | |
| last_diagnostics = {'status': 'native_horizon'} | |
| return None | |
| try: | |
| f, visible = features(obs) | |
| except (KeyError, ValueError, TypeError): | |
| last_diagnostics = {'status': 'invalid_observation'} | |
| return None | |
| if _last_action is None: | |
| im=np.asarray(obs['rgb']['top_camera']).squeeze().astype(float) | |
| r,g,b=im[...,0],im[...,1],im[...,2] | |
| yy,xx=np.nonzero((r>65)&(r>1.5*b+15)&(r>1.4*g+15)) | |
| if len(yy)>100: _target_y=float((yy.min()+yy.max())/(2*im.shape[0])) | |
| bank = _load() | |
| distance = np.sum((bank['features']-f)**2,axis=1) | |
| idx = np.array([np.where(bank['demo_index']==di)[0][np.argmin(distance[bank['demo_index']==di])] for di in range(20)]) | |
| idx = idx[np.argsort(distance[idx])][:5] | |
| best = idx[0] | |
| # Reject gross extrapolation. This threshold is heuristic, not validated. | |
| if not visible or distance[best] > 16.0: | |
| action = np.zeros(14,np.float32); action[[6,13]] = -1 | |
| last_diagnostics = {'status': 'unsupported_hold', 'squared_distance': float(distance[best])} | |
| else: | |
| # Avoid averaging opposing contact/repositioning actions. Both robot | |
| # commands always come from the same paired observation/action record. | |
| compatible = np.max(np.abs(bank['actions'][idx]-bank['actions'][best]),axis=1) <= .12 | |
| idx = idx[compatible] | |
| weights = np.exp(-(distance[idx]-distance[best])/.35) | |
| action = np.average(bank['actions'][idx], axis=0, weights=weights) | |
| action = np.clip(action,-1,1).astype(np.float32) | |
| last_diagnostics = {'status': 'lookup', 'squared_distance': float(distance[best]), | |
| 'reference_demo': int(bank['demo_index'][best]), | |
| 'reference_frame': int(bank['frame_index'][best])} | |
| pose = box_pose(np.asarray(obs['rgb']['top_camera']).squeeze()) | |
| if (last_diagnostics['status']=='lookup' and pose is not None | |
| and .54 < pose['x'] < .83 and pose['area'] > .045 | |
| and abs(pose['tilt']) < .65 | |
| and min(action[0], action[7]) > .035 | |
| and min(action[2], action[9]) > -.05): | |
| tilt = pose['tilt'] | |
| correction = np.clip(.18 * np.sign(tilt)*max(abs(tilt)-.06,0),-.06,.06) | |
| action[0] = np.clip(action[0] + correction, 0, .15) | |
| action[7] = np.clip(action[7] - correction, 0, .15) | |
| last_diagnostics['yaw_correction'] = float(correction) | |
| last_diagnostics['box_tilt'] = float(tilt) | |
| lateral=float(np.clip(.4*(pose['y']-_target_y),-.035,.035)) | |
| action[1] = np.clip(action[1]+lateral,-.15,.15) | |
| action[8] = np.clip(action[8]+lateral,-.15,.15) | |
| last_diagnostics['lateral_correction'] = lateral | |
| _unsupported = _unsupported+1 if last_diagnostics['status']=='unsupported_hold' else 0 | |
| if _unsupported >= 40: | |
| last_diagnostics['status']='unsupported_abort' | |
| return None | |
| _last_action = action.copy() | |
| return action | |