Download astra/DualArmMakeLine-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 5.3 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmMakeLine-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmMakeLine-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmMakeLine-v1/controller.py
5.3 kB
| """D20 nonparametric feedback policy with bounded phase progression. No environment imports.""" | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| CAMERAS = ('top_camera', 'front_camera') | |
| QPATHS = tuple(f'agent/panda_leaphand-{r}/qpos' for r in range(2)) | |
| VPATHS = tuple(f'agent/panda_leaphand-{r}/qvel' for r in range(2)) | |
| def visual_features(rgb): | |
| features = [] | |
| 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 im.dtype != np.uint8: | |
| raise ValueError('Expected uint8 RGB HWC or 1HWC') | |
| hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV) | |
| # Colored objects retain more weight than the neutral table and arms. | |
| mask = ((hsv[:,:,1] > 150) & (hsv[:,:,2] > 70)).astype(np.float32) | |
| color = im.astype(np.float32) / 255. | |
| features.append(cv2.resize(color, (16,16), interpolation=cv2.INTER_AREA).ravel()) | |
| features.append(3 * cv2.resize(color * mask[:,:,None], (24,24), interpolation=cv2.INTER_AREA).ravel()) | |
| return np.concatenate(features).astype(np.float32) | |
| class Controller: | |
| SPEED=1.6 | |
| ARM_GAIN=.15 | |
| HAND_GAIN=2. | |
| def __init__(self): | |
| with np.load(Path(__file__).with_name('memory.npz'), allow_pickle=False) as data: | |
| self.bank = {key: data[key] for key in data.files} | |
| self.inverse = np.load(Path(__file__).with_name('feedback.npz'))['inverse'] | |
| self.scale = np.tile(np.r_[np.full(7,.20), np.full(16,.35)],2).astype(np.float32) | |
| self.reset_episode() | |
| def reset_episode(self): | |
| self.t = 0 | |
| self.last_phase = None | |
| self.last_step = None | |
| self.diagnostics = {'status':'reset'} | |
| def act(self, obs): | |
| self.t += 1 | |
| step = int(obs.get('step_id', self.t)) | |
| obs = obs.get('observation', obs) | |
| if self.last_step is not None and step < self.last_step: | |
| self.reset_episode() | |
| self.last_step = step | |
| # One native step was already consumed by the runner-owned hold. | |
| if step >= int(np.ceil(1559/self.SPEED))+41: | |
| self.diagnostics = {'status':'horizon_stop', 'success_claim':False} | |
| return None | |
| try: | |
| p = obs['proprioception'] | |
| q = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in QPATHS]) | |
| v = np.concatenate([np.asarray(p[k],dtype=np.float32).reshape(-1) for k in VPATHS]) | |
| if q.shape != (46,) or v.shape != (46,) or not np.isfinite(q).all() or not np.isfinite(v).all(): | |
| raise ValueError('Malformed proprioception') | |
| f = visual_features(obs['rgb']) | |
| except (KeyError, ValueError, TypeError, cv2.error): | |
| self.diagnostics = {'status':'missing_or_invalid_observation'} | |
| return np.zeros(44,dtype=np.float32) | |
| if (step-1)*self.SPEED >= 1559: | |
| a = np.zeros(44,dtype=np.float32) | |
| a[2] = a[24] = .04 | |
| self.diagnostics = {'status':'retract_for_visibility'} | |
| return a | |
| b = self.bank | |
| qcost = np.mean(((b['q']-q)/self.scale)**2,axis=1) | |
| vcost = .025*np.mean(np.minimum((b['v']-v)**2,25),axis=1) | |
| base = qcost + vcost | |
| # Timing resolves stationary/contact phase ambiguity seen in episode 1. | |
| expected = min((step - 1)*self.SPEED, 1558) | |
| base += .5*((b['phase']-expected)/8.)**2 | |
| # Soft continuity inferred from previous observed state, never demo ID or seed. | |
| if self.last_phase is not None: | |
| base += .08*np.minimum(((b['phase']-self.last_phase)/100.)**2,4.) | |
| ids = np.argpartition(base,min(127,len(base)-1))[:128] | |
| visual = np.mean((b['features'][ids]-f)**2,axis=1) | |
| score = base[ids] + 12*visual | |
| j = int(ids[np.argmin(score)]) | |
| if qcost[j] > 4.0 or float(visual[np.argmin(score)]) > .15: | |
| self.diagnostics = {'status':'outside_demo_support', 'qcost':float(qcost[j])} | |
| return np.zeros(44,dtype=np.float32) | |
| self.last_phase = float(b['phase'][j]) | |
| self.diagnostics = {'status':'retrieved_feedback','reference_phase':self.last_phase,'reference_demo':int(b['demo'][j]),'qcost':float(qcost[j]),'success_claim':False} | |
| # Jointly retrieve both robots to preserve the demonstrated coordination. | |
| # Parts are [robot 0: 6 arm + 16 hand, robot 1: 6 arm + 16 hand]. | |
| a = self.SPEED * b['actions'][j].copy() | |
| errors = b['q'][j] - q | |
| for r in range(2): | |
| correction = self.ARM_GAIN * errors[r*23:r*23+7] @ self.inverse[min(int(expected)//4,390),r] | |
| correction = np.clip(correction,-.03,.03) | |
| a[np.array([0,1,2,4])+r*22] += correction | |
| hand_a = np.array([0,1,2,3,12,14,15]) + r*22+6 | |
| hand_q = np.array([11,7,15,19,10,18,22]) + r*23 | |
| a[hand_a] += np.clip(self.HAND_GAIN*errors[hand_q],-.1,.1) | |
| self.diagnostics['pose_error'] = float(np.linalg.norm(errors)) | |
| return np.clip(a,-1.,1.).astype(np.float32) | |
| _policy = None | |
| def reset_episode(): | |
| global _policy | |
| if _policy is not None: | |
| _policy.reset_episode() | |
| def act(obs): | |
| global _policy | |
| if _policy is None: | |
| _policy = Controller() | |
| return _policy.act(obs) | |