Download astra/ClutterGrasp-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 6.11 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/ClutterGrasp-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/ClutterGrasp-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/ClutterGrasp-v1/controller.py
6.11 kB
| """Unrehearsed D20 draft: nonparametric RGB/proprio action retrieval. | |
| No environment imports, learned neural weights, seed lookup, or timed replay. | |
| """ | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| from localizer import Localizer | |
| CAMERAS = ('front_camera', 'left_camera', 'robot_camera', 'top_camera') | |
| # Normalized crops selected by visual inspection; extrinsics remain unknown. | |
| CROPS = ((.18,.32,.65,.90), (.28,.25,.95,.95), (0.,0.,1.,1.), (.10,.40,.95,.98)) | |
| def image_features(rgb): | |
| out = [] | |
| for name, (x0,y0,x1,y1) in zip(CAMERAS,CROPS): | |
| im = np.asarray(rgb[name]) | |
| if im.ndim == 4 and im.shape[0] == 1: | |
| im = im[0] | |
| if im.ndim != 3 or im.shape[2] != 3 or min(im.shape[:2]) < 16 or im.dtype != np.uint8: | |
| raise ValueError('Expected uint8 RGB HWC or 1HWC') | |
| h,w = im.shape[:2] | |
| crop = im[int(y0*h):int(y1*h),int(x0*w):int(x1*w)] | |
| small = cv2.resize(crop,(12,12),interpolation=cv2.INTER_AREA).astype(np.float32)/255. | |
| # Red excess helps distinguish the chips package; it is not an object detector. | |
| red = np.maximum(small[:,:,0] - np.maximum(small[:,:,1],small[:,:,2]),0.) | |
| out.append(np.concatenate([small,red[:,:,None]],axis=2).reshape(-1)) | |
| return np.stack(out) | |
| class Controller: | |
| def __init__(self): | |
| with np.load(Path(__file__).with_name('reference_bank.npz'),allow_pickle=False) as z: | |
| self.bank = {k:z[k] for k in z.files} | |
| self.localizer = Localizer() | |
| self.reset_episode() | |
| def reset_episode(self): | |
| self.target_hint = None | |
| self.scene_penalty = np.zeros(20,dtype=np.float32) | |
| self.calls = 0 | |
| self.last_index = None | |
| self.visits = np.zeros(len(self.bank['actions']),dtype=np.int32) | |
| self.status = 'reset; untested draft' | |
| def act(self, obs): | |
| # The charged initial hold is owned by the runner. No warmup is inserted. | |
| try: | |
| self.calls += 1 | |
| if 'observation' in obs: | |
| outer = obs | |
| obs = dict(outer['observation']) | |
| obs['step_id'] = outer.get('step_id', self.calls) | |
| step = int(obs.get('step_id', self.calls)) | |
| if step >= 650: | |
| self.status = 'native horizon reached; no success claim' | |
| return None | |
| if step <= 21: | |
| self.status = 'settling clutter; legal zero deltas' | |
| return np.zeros(22,dtype=np.float32) | |
| if step < 1: | |
| raise ValueError('step_id must start at one after runner hold') | |
| p = obs['proprioception'] | |
| q = np.asarray(p['agent/qpos'],dtype=np.float32).reshape(-1) | |
| v = np.asarray(p['agent/qvel'],dtype=np.float32).reshape(-1) | |
| if q.shape != (23,) or v.shape != (23,) or not np.isfinite(q).all() or not np.isfinite(v).all(): | |
| raise ValueError('Invalid proprioception') | |
| f = image_features(obs['rgb']) | |
| except (KeyError,ValueError,TypeError,OverflowError): | |
| self.status = 'invalid legal observation; zero deltas' | |
| return np.zeros(22,dtype=np.float32) | |
| b = self.bank | |
| if self.target_hint is None and step <= 80: | |
| found = self.localizer.locate(obs['rgb']['left_camera']) | |
| if found is not None and found['matches'] >= 5: | |
| self.target_hint = found | |
| lb = self.localizer.bank | |
| axes = np.array([lb[f'axis_{i}'] for i in range(20)]) | |
| delta = (lb['centers']-np.array(found['center']))/100. | |
| self.scene_penalty = .5*np.sum(delta*delta,axis=1) + .7*(1.-np.abs(axes@np.array(found['axis']))) | |
| qerr = ((b['qpos']-q)/b['qscale'])**2 | |
| arm = np.mean(qerr[:,:7],axis=1) | |
| hand = np.mean(qerr[:,7:],axis=1) | |
| vel = np.mean(np.minimum(((b['qvel']-v)/b['vscale'])**2,9.),axis=1) | |
| vd = np.mean((b['features']-f)**2,axis=2) | |
| # Wrist camera has rendering artifacts and receives lower weight. | |
| visual = vd @ np.array([.30,.30,.10,.30],dtype=np.float32) | |
| red_dist = np.mean((b['features'][:,:,3::4]-f[:,3::4])**2,axis=(1,2)) | |
| cost = 12.*visual + 30.*red_dist + .25*arm + .18*hand + .015*vel | |
| cost += self.scene_penalty[b['demo']] | |
| base_cost = cost.copy() | |
| if self.last_index is not None: | |
| j = self.last_index | |
| same = b['demo'] == b['demo'][j] | |
| distance = np.abs(b['frame']-b['frame'][j]) | |
| cost += np.where(same,.015*np.minimum(distance/40.,4.),.12) | |
| cost += .001*np.minimum(self.visits,80) | |
| cost += np.where(np.isin(b['demo'],[0,3,7,12,19]),100.,0.) | |
| j = int(np.argmin(cost)) | |
| # Deliberately uncalibrated support checks; do not improvise large motions. | |
| arm_rms = float(np.sqrt(np.mean((b['qpos'][j,:7]-q[:7])**2))) | |
| if visual[j] > .12 or arm_rms > .65 or base_cost[j] > 2.5: | |
| self.last_index = None | |
| self.status = 'outside reference coverage; zero deltas' | |
| return np.zeros(22,dtype=np.float32) | |
| # Only blend nearby samples with compatible arm commands and hand pose. | |
| eligible = np.flatnonzero((np.linalg.norm(b['actions'][:,:6]-b['actions'][j,:6],axis=1)<.12) | |
| & (np.mean((b['qpos'][:,7:]-b['qpos'][j,7:])**2,axis=1)<.04)) | |
| ids = np.array([j]) | |
| weight = np.exp(-(cost[ids]-np.min(cost[ids]))/.025) | |
| action = np.sum(b['actions'][ids]*weight[:,None],axis=0)/weight.sum() | |
| self.visits[j] += 1 | |
| self.last_index = j | |
| self.status = 'observation-conditioned retrieval; completion unknown' | |
| # Single contract action part: arm translation, arm rotation, 16 dex deltas. | |
| return np.clip(action,-1.,1.).astype(np.float32) | |
| _controller = None | |
| def reset_episode(): | |
| global _controller | |
| if _controller is None: | |
| _controller = Controller() | |
| else: | |
| _controller.reset_episode() | |
| def act(obs): | |
| global _controller | |
| if _controller is None: | |
| reset_episode() | |
| return _controller.act(obs) | |