Download astra/DualArmOpenPen-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 4.68 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmOpenPen-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmOpenPen-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmOpenPen-v1/controller.py
4.68 kB
| """D20 observation-conditioned retrieval with bounded terminal-state recovery.""" | |
| from pathlib import Path | |
| import numpy as np | |
| import cv2 | |
| ROBOTS = ('panda_leaphand-1', 'panda_leaphand_left-0') | |
| CAMERAS = ('front_camera', 'top_camera') | |
| _bank = None | |
| _calls = 0 | |
| _still = 0 | |
| _pull = 0 | |
| _axis = np.array([0.,1.],dtype=np.float32) | |
| def visual_features(image): | |
| image = np.asarray(image) | |
| if image.ndim == 4 and image.shape[0] == 1: | |
| image = image[0] | |
| if image.ndim != 3 or image.shape[2] != 3 or image.dtype != np.uint8: | |
| raise ValueError('RGB must be uint8 HWC or 1HWC') | |
| x = image.astype(np.float32) / 255. | |
| # Color cue is a soft blue dominance map, not a claim of object identity. | |
| blue = np.maximum(0., x[:,:,2] - np.maximum(x[:,:,0], x[:,:,1])) | |
| return np.concatenate((cv2.resize(x, (24,24), interpolation=cv2.INTER_AREA).ravel(), | |
| cv2.resize(blue, (32,32), interpolation=cv2.INTER_AREA).ravel())).astype(np.float32) | |
| def reset_episode(): | |
| global _calls, _still, _pull, _axis | |
| _calls = 0 | |
| _still = 0 | |
| _pull = 0 | |
| _axis = np.array([0.,1.],dtype=np.float32) | |
| def _load(): | |
| global _bank | |
| if _bank is None: | |
| with np.load(Path(__file__).with_name('reference_bank.npz'), allow_pickle=False) as z: | |
| _bank = {k:z[k] for k in z.files} | |
| return _bank | |
| def act(obs): | |
| """Return contract-ordered 44-vector; None is budget exhaustion, not success.""" | |
| global _calls, _still, _pull, _axis | |
| step = int(obs.get('step_id', _calls + 1)) | |
| if step >= 400 or _calls >= 399: | |
| return None | |
| _calls += 1 | |
| try: | |
| prop = obs['proprioception'] | |
| q = np.concatenate([np.asarray(prop[f'agent/{r}/qpos'], dtype=np.float32).reshape(-1) for r in ROBOTS]) | |
| v = np.concatenate([np.asarray(prop[f'agent/{r}/qvel'], dtype=np.float32).reshape(-1) for r in ROBOTS]) | |
| if q.shape != (46,) or v.shape != (46,) or not np.isfinite(q).all() or not np.isfinite(v).all(): | |
| return np.zeros(44, dtype=np.float32) | |
| visuals = [visual_features(obs['rgb'][c]) for c in CAMERAS] | |
| except (KeyError, ValueError, TypeError): | |
| return np.zeros(44, dtype=np.float32) | |
| if _pull == 0: | |
| im = np.asarray(obs['rgb']['top_camera']).squeeze().astype(np.float32) | |
| ys,xs = np.where((im[:,:,2]-im[:,:,0]>25)&(im[:,:,2]-im[:,:,1]>15)) | |
| if len(xs)>20: | |
| xy=np.column_stack((xs,ys)); vals,vecs=np.linalg.eigh(np.cov(xy.T)); direction=vecs[:,-1] | |
| if direction[1]>0: direction=-direction | |
| _axis=np.array([direction[0],-direction[1]],dtype=np.float32) | |
| bank = _load() | |
| scale = np.tile(np.r_[np.full(7,.15),np.full(16,.3)],2) | |
| d = np.mean(((bank['q']-q)/scale)**2,axis=1) | |
| d += .005 * np.mean(((bank['v']-v)/.5)**2,axis=1) | |
| for i,f in enumerate(visuals): | |
| diff = bank[f'image_{i}']-f | |
| d += .6*np.mean((diff[:,:1728]/.15)**2,axis=1) | |
| d += 2.*np.mean((diff[:,1728:]/.04)**2,axis=1) | |
| if 'pose' not in bank: bank['pose']=blue_pose(bank['image_1']) | |
| d += .5*np.mean(((bank['pose']-blue_pose(visuals[1]))/[.1,.1,.3,.15])**2,axis=1) | |
| # Re-select from all observations at every step. No demo ID/time/seed lookup. | |
| idx = np.argpartition(d, 4)[:5] | |
| idx = idx[np.argsort(d[idx])] | |
| # Avoid blending contradictory modes of coordinated bimanual action. | |
| aa = bank['actions'][idx] | |
| compatible = np.linalg.norm(aa-aa[0],axis=1) < .2 | |
| idx = idx[compatible] | |
| weights = np.exp(-(d[idx]-d[idx[0]])/.08) | |
| action = np.sum(bank['actions'][idx]*weights[:,None],axis=0)/weights.sum() | |
| if not np.isfinite(action).all(): | |
| return np.zeros(44,dtype=np.float32) | |
| # Faster approach remains re-localized from current observations every step. | |
| action *= 2.0 | |
| arm_ids = [0,1,2,22,23,24] | |
| closed = min(np.mean(q[7:23]),np.mean(q[30:46])) > .55 | |
| _still = _still + 1 if closed and np.linalg.norm(action[arm_ids]) < .02 else 0 | |
| if _still >= 5 and _pull == 0: | |
| _pull = 1 | |
| if _pull: | |
| action[arm_ids] = 0. | |
| if _pull <= 35: | |
| action[:2] = .12 * _axis | |
| action[22:24] = -.12 * _axis | |
| _pull += 1 | |
| if _pull > 41: | |
| return None | |
| return np.clip(action,-1.,1.).astype(np.float32) | |
| def blue_pose(f): | |
| f=np.atleast_2d(f) | |
| yy,xx=np.mgrid[:32,:32];xy=np.column_stack((xx.ravel()/31,yy.ravel()/31)) | |
| w=np.maximum(f[:,1728:]-.015,0);w=w/(w.sum(-1,keepdims=True)+1e-8) | |
| c=w@xy;dx=xy[None,:,:]-c[:,None,:] | |
| cov=np.einsum('ni,nij,nik->njk',w,dx,dx); vals,vecs=np.linalg.eigh(cov) | |
| v=vecs[:,:,-1];v*=np.where(v[:,1:2]<0,-1,1) | |
| return np.column_stack((c,v[:,0],np.sqrt(np.maximum(0,vals[:,-1])))) | |