Download astra/ClothWipe-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 4.18 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/ClothWipe-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/ClothWipe-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/ClothWipe-v1/controller.py
4.18 kB
| """D20 observation-conditioned retrieval controller. No simulator or weights.""" | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| CAMERAS = ('top_camera', 'front_camera') | |
| def image_feature(rgb): | |
| parts = [] | |
| for camera in CAMERAS: | |
| im = np.asarray(rgb[camera]) | |
| 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]) < 2: | |
| raise ValueError('Expected RGB HWC or 1HWC') | |
| if not np.isfinite(im).all(): | |
| raise ValueError('Nonfinite RGB') | |
| parts.append(cv2.resize(im.astype(np.float32), (16, 16), | |
| interpolation=cv2.INTER_AREA).ravel() / 255.) | |
| return np.concatenate(parts) | |
| _bank = None | |
| _phase = None | |
| _last_step = None | |
| _diagnostics = {} | |
| def reset_episode(): | |
| global _phase, _last_step, _diagnostics | |
| _phase = None | |
| _last_step = None | |
| _diagnostics = {} | |
| def _load(): | |
| global _bank | |
| if _bank is None: | |
| with np.load(Path(__file__).with_name('reference_bank.npz'), allow_pickle=False) as f: | |
| _bank = {k: f[k] for k in f.files} | |
| return _bank | |
| def act(obs): | |
| """Return 6 root-frame arm deltas then 16 dex deltas, in one 22-vector. | |
| All retrieval is recomputed from live legal observations. Temporal continuity | |
| is a soft phase prior, never a selected-demo playback cursor. None means | |
| horizon exhaustion or unusable input, and does not assert task completion. | |
| """ | |
| global _phase, _last_step, _diagnostics | |
| if 'observation' in obs: | |
| envelope = obs | |
| obs = dict(envelope['observation']) | |
| obs['step_id'] = envelope.get('step_id', 1 if _last_step is None else _last_step + 1) | |
| step = int(obs.get('step_id', 1 if _last_step is None else _last_step + 1)) | |
| if step >= 500: | |
| return None # Runner already charged one native hold step. | |
| if _last_step is not None and step < _last_step: | |
| reset_episode() | |
| try: | |
| 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.size != 23 or v.size != 23 or not np.isfinite(q).all() or not np.isfinite(v).all(): | |
| return None | |
| visual = image_feature(obs['rgb']) | |
| except (KeyError, ValueError, TypeError): | |
| return None | |
| bank = _load() | |
| # Coarse proprioceptive candidates; current RGB reranks every invocation. | |
| dq = (bank['q'] - q) / bank['q_scale'] | |
| dv = (bank['v'] - v) / bank['v_scale'] | |
| cost = np.mean(dq[:, :7] ** 2, axis=1) + .6 * np.mean(dq[:, 7:] ** 2, axis=1) | |
| cost += .08 * np.mean(np.minimum(dv ** 2, 25), axis=1) | |
| if _phase is not None: | |
| # A bounded one-step expectation resolves nearly stationary grasp stages. | |
| cost += .35 * (bank['phase'] - _phase) ** 2 | |
| pool = np.argpartition(cost, min(95, len(cost)-1))[:96] | |
| image_error = np.mean((bank['visual'][pool] - visual) ** 2, axis=1) | |
| score = cost[pool] + 80. * image_error | |
| order = np.argsort(score) | |
| anchor = pool[order[0]] | |
| # Do not average actions from opposite sides of a grasp transition. | |
| compatible = pool[np.floor(bank['phase'][pool]) == np.floor(bank['phase'][anchor])] | |
| vis = np.mean((bank['visual'][compatible] - visual) ** 2, axis=1) | |
| total = cost[compatible] + 80. * vis | |
| chosen = np.argsort(total)[:5] | |
| ids = compatible[chosen] | |
| weights = np.exp(-(total[chosen] - total[chosen[0]]) / .08) | |
| weights /= weights.sum() | |
| action = weights @ bank['actions'][ids] | |
| estimate = float(weights @ bank['phase'][ids]) | |
| increment = float(weights @ bank['phase_rate'][ids]) | |
| if _phase is None: | |
| _phase = estimate + increment | |
| else: | |
| _phase = float(np.clip(.8*_phase + .2*estimate + increment, 0, 7.99)) | |
| _last_step = step | |
| _diagnostics = {'phase': _phase, 'reference_distance': float(total[chosen[0]]), | |
| 'visual_error': float(image_error[order[0]])} | |
| if 5.0 <= _phase < 6.0: | |
| action[6:] *= .5 | |
| # Exact action-part order: one width-22 part, no mimic gripper component. | |
| return np.clip(action, -1., 1.).astype(np.float32) | |