Download astra/DrapeCloth-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 6.45 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DrapeCloth-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DrapeCloth-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DrapeCloth-v1/controller.py
6.45 kB
| """Task-local nonparametric RGB/proprioception feedback policy. No fitted weights.""" | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| CAMERAS = ('front_camera', 'left_camera', 'robot_camera', 'top_camera') | |
| def visual_feature(image): | |
| im = np.asarray(image) | |
| 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') | |
| small = cv2.resize(im, (16, 12), interpolation=cv2.INTER_AREA).astype(np.float32) / 255. | |
| # Orange cloth cue, deliberately separate from the background-heavy thumbnail. | |
| hsv = cv2.cvtColor(im, cv2.COLOR_RGB2HSV) | |
| mask = ((hsv[..., 0] >= 3) & (hsv[..., 0] <= 24) & | |
| (hsv[..., 1] > 110) & (hsv[..., 2] > 150)).astype(np.float32) | |
| grid = cv2.resize(mask, (16, 12), interpolation=cv2.INTER_AREA) | |
| return np.concatenate((small.ravel()/np.sqrt(small.size)*2, | |
| grid.ravel()/np.sqrt(grid.size)*3)).astype(np.float32) | |
| class Controller: | |
| def __init__(self): | |
| with np.load(Path(__file__).with_name('demo_bank.npz'), allow_pickle=False) as b: | |
| self.bank = {k: b[k] for k in b.files} | |
| self.visual_flat = self.bank['visual'].reshape(len(self.bank['visual']), -1) | |
| self.visual_norm = np.einsum('ij,ij->i', self.visual_flat, self.visual_flat) | |
| self.reset_episode() | |
| def reset_episode(self): | |
| self.end_count = 0 | |
| self.invalid_count = 0 | |
| self.last_diagnostic = {} | |
| self.calls = 0 | |
| self.bowl_centers = [] | |
| self.servo_steps = 0 | |
| def act(self, obs): | |
| if int(obs.get('step_id', 1)) >= 800: | |
| return None # one runner-owned hold + at most 799 policy actions | |
| 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.shape != (23,) or v.shape != (23,) or not np.isfinite(q).all() or not np.isfinite(v).all(): | |
| raise ValueError('Invalid proprioception') | |
| visual = np.stack([visual_feature(obs['rgb'][c]) for c in CAMERAS]) | |
| except (KeyError, ValueError, TypeError): | |
| self.invalid_count += 1 | |
| return None if self.invalid_count >= 10 else np.zeros(22, dtype=np.float32) | |
| self.calls += 1 | |
| b = self.bank | |
| dq = (b['qpos'] - q) / b['qscale'] | |
| dv = (b['qvel'] - v) / .5 | |
| qdist = np.mean(dq[:, :7]**2, axis=1) + .4*np.mean(dq[:, 7:]**2, axis=1) | |
| vdist = .03*np.mean(dv**2, axis=1) | |
| visual_dist = np.maximum(0., (self.visual_norm + np.sum(visual*visual) - | |
| 2*np.einsum('ij,j->i', self.visual_flat, visual.ravel(), optimize=False))/len(CAMERAS)) | |
| distance = qdist + vdist + 4*visual_dist | |
| best = int(np.argmin(distance)) | |
| endpoint_close = bool(b['terminal'][best]) and qdist[best] < .15 and visual_dist[best] < .08 | |
| if b['terminal'][best] and not endpoint_close: | |
| distance[b['terminal']] = np.inf | |
| best = int(np.argmin(distance)) | |
| frame = int(b['frame_index'][best]) | |
| if 35 <= frame <= 125 and self.calls % 10 == 0: | |
| top = np.asarray(obs['rgb']['top_camera']).squeeze() | |
| gray = cv2.GaussianBlur(cv2.cvtColor(top, cv2.COLOR_RGB2GRAY), (5,5), 1) | |
| circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 70, | |
| param1=100, param2=30, minRadius=45, maxRadius=75) | |
| if circles is not None: | |
| valid = [c[:2] for c in circles[0] if 150<c[0]<380 and 240<c[1]<440 and 50<c[2]<69] | |
| if len(valid) == 1: | |
| self.bowl_centers.append(valid[0]) | |
| self.last_diagnostic = {'distance': float(distance[best]), 'reference_row': best, | |
| 'visual_distance': float(visual_dist[best]), | |
| 'pose_distance': float(qdist[best]), | |
| 'demo': int(b['demo_index'][best]), | |
| 'frame': int(b['frame_index'][best])} | |
| # Unvalidated conservative out-of-support gate; no blind replay fallback. | |
| if qdist[best] > 9 or visual_dist[best] > 1.5: | |
| self.invalid_count += 1 | |
| return None if self.invalid_count >= 10 else np.zeros(22, dtype=np.float32) | |
| self.invalid_count = 0 | |
| terminal = bool(b['terminal'][best]) and qdist[best] < .15 and visual_dist[best] < .08 | |
| self.end_count = self.end_count + 1 if terminal else 0 | |
| if self.end_count >= 6: | |
| return None # endpoint resemblance, NOT a success assertion | |
| if terminal: | |
| return np.zeros(22, dtype=np.float32) | |
| # Blend only nearby samples with the same coarse action direction. | |
| a0 = b['actions'][best] | |
| indices = np.argsort(distance)[:12] | |
| compatible = (~b['terminal'][indices]) & (np.linalg.norm(b['actions'][indices,:3]-a0[:3],axis=1)<.08) | |
| indices = indices[compatible][:5] | |
| if len(indices) == 0: | |
| indices = np.array([best]) | |
| weights = np.exp(-(distance[indices]-distance[best])/.06) | |
| action = np.average(b['actions'][indices], axis=0, weights=weights) | |
| if frame >= 210 and len(self.bowl_centers) >= 3: | |
| top = np.asarray(obs['rgb']['top_camera']).squeeze() | |
| hsv = cv2.cvtColor(top, cv2.COLOR_RGB2HSV) | |
| mask = (hsv[:,:,0]>=3)&(hsv[:,:,0]<=24)&(hsv[:,:,1]>110)&(hsv[:,:,2]>150) | |
| yy, xx = np.where(mask) | |
| if len(xx) >= 30 and np.mean(yy) < 500: | |
| center = np.array([np.mean(xx),np.mean(yy)]) | |
| target = np.median(self.bowl_centers, axis=0) | |
| error = target-center | |
| action[:2] = np.clip(error*np.array([.012,-.012]), -.1, .1) | |
| if np.linalg.norm(error) > 35: | |
| action[2] = max(0.,action[2]) | |
| self.servo_steps += 1 | |
| self.last_diagnostic.update(bowl=target.tolist(),cloth=center.tolist(),servo_error=float(np.linalg.norm(error))) | |
| return np.clip(action, -1., 1.).astype(np.float32) | |
| _policy = None | |
| def reset_episode(): | |
| global _policy | |
| if _policy is None: | |
| _policy = Controller() | |
| else: | |
| _policy.reset_episode() | |
| def act(obs): | |
| global _policy | |
| if _policy is None: | |
| reset_episode() | |
| return _policy.act(obs) | |