Download astra/DualArmPouringTwoCups-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 8.42 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPouringTwoCups-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmPouringTwoCups-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmPouringTwoCups-v1/controller.py
8.42 kB
| """D20 nonparametric observation-conditioned controller under E20 exploration.""" | |
| from pathlib import Path | |
| import json | |
| import cv2 | |
| import numpy as np | |
| ROOT = Path(__file__).resolve().parent | |
| CONFIG = json.loads((ROOT / 'config.json').read_text()) | |
| CAMS = ['front_camera', 'top_camera'] | |
| def visual_features(rgb): | |
| blocks=[] | |
| for camera in CAMS: | |
| 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 HWC RGB') | |
| small=cv2.resize(im,(24,24),interpolation=cv2.INTER_AREA).astype(np.float32)/255. | |
| # Color occupancy preserves cup location/tilt separately from background. | |
| r,g,b=[im[:,:,i].astype(np.float32) for i in range(3)] | |
| masks=[(b>r*1.25)&(b>g*1.05)&(b>70), (r>b*1.5)&(g>b*1.2)&(r>100)] | |
| blocks.append(small.ravel()) | |
| blocks.extend(cv2.resize(x.astype(np.float32),(24,24),interpolation=cv2.INTER_AREA).ravel() for x in masks) | |
| return np.concatenate(blocks) | |
| class Controller: | |
| def __init__(self): | |
| with np.load(ROOT/'reference_bank.npz',allow_pickle=False) as z: | |
| self.bank={k:z[k] for k in z.files} | |
| self.feedback=np.load(ROOT/'feedback_bank.npz')['maps'] if (ROOT/'feedback_bank.npz').exists() else None | |
| self.dense=np.load(ROOT/'trajectory_bank.npz') if (ROOT/'trajectory_bank.npz').exists() else None | |
| self.reset_episode() | |
| def reset_episode(self): | |
| self.cursors=np.zeros(20,dtype=np.float32) | |
| self.previous_step=0 | |
| self.previous_phase_speed=1.0 | |
| self.orange_first=None | |
| self.locked=None | |
| self.rotation_level=0.0 | |
| self.last_action=np.zeros(44,dtype=np.float32) | |
| self.diagnostic={} | |
| def act(self,obs): | |
| step=int(obs.get('step_id',self.previous_step+1)) | |
| if 'observation' in obs: obs=obs['observation'] | |
| if step>=CONFIG.get('development_stop',1000): return None | |
| if step>=CONFIG['native_horizon']: return None | |
| if step<1: raise ValueError('Runner-owned hold must precede act') | |
| if step<=self.previous_step: self.reset_episode() | |
| try: | |
| q=np.concatenate([np.asarray(obs['proprioception'][p],dtype=np.float32).reshape(-1) for p in CONFIG['proprio_paths']]) | |
| if q.shape!=(92,) or not np.isfinite(q).all(): raise ValueError('Invalid proprioception') | |
| v=visual_features(obs['rgb']) | |
| except (KeyError,ValueError,TypeError): | |
| self.diagnostic={'reason':'missing or invalid legal observation'} | |
| return np.zeros(44,dtype=np.float32) | |
| if self.orange_first is None: | |
| im=cv2.resize(np.asarray(obs['rgb']['top_camera']).squeeze(),(240,320)) | |
| roi=im[172:236,145:205].astype(np.float32) | |
| r,g,b=roi[:,:,0],roi[:,:,1],roi[:,:,2] | |
| self.orange_first=bool(np.sum((b>1.2*r)&(b>1.05*g)&(b>45))>100) | |
| bank=self.bank; elapsed=max(1,step-self.previous_step) | |
| candidates=[]; scores=[] | |
| phase_before_action=float(np.max(self.cursors)) | |
| second_pour=bool(phase_before_action>=CONFIG.get('second_pour_phase',400)) | |
| active_source_orange=(not self.orange_first) if second_pour else self.orange_first | |
| speed_key='orange_phase_speed' if active_source_orange else 'blue_phase_speed' | |
| speed=CONFIG.get(speed_key,CONFIG.get('phase_speed',1.0)) if step>40 else 1.0 | |
| orange_return_phase=CONFIG.get('orange_return_phase_second',650) if second_pour else CONFIG.get('orange_return_phase_first',275) | |
| if step>40 and active_source_orange and phase_before_action>=orange_return_phase: | |
| speed=CONFIG.get('orange_return_speed',speed) | |
| if step>40 and CONFIG.get('slow_rotation',False) and self.rotation_level>0.015: speed=min(speed,CONFIG.get('rotation_speed',1.5)) | |
| for demo in range(20): | |
| refs=CONFIG.get('preferred_refs') | |
| if refs is not None and demo!=refs[1 if self.orange_first else 0]: continue | |
| if self.locked is not None and demo!=self.locked: continue | |
| if (demo in [1,2,6,7,11,13,14,16,17,19]) != self.orange_first: continue | |
| advance_speed=self.previous_phase_speed if CONFIG.get('fixed_phase',False) else speed | |
| expected=self.cursors[demo]+(elapsed*advance_speed if self.previous_step else 0) | |
| ids=np.flatnonzero((bank['demo']==demo)&(bank['frame']>=max(0,expected-12))&(bank['frame']<=expected+20)) | |
| if not len(ids): continue | |
| dq=(bank['proprio'][ids]-q)/bank['scale'] | |
| # Equalized joint groups, light velocity influence, visual cup emphasis. | |
| qcost=np.mean(dq*dq,axis=1) | |
| vcost=np.mean((bank['visual'][ids]-v)**2,axis=1) | |
| cost=qcost+12*vcost+0.025*((bank['frame'][ids]-expected)/12)**2 | |
| best=int(np.argmin(cost)) | |
| if CONFIG.get('fixed_phase',False): | |
| best=int(np.argmin(np.abs(bank['frame'][ids]-expected))) | |
| self.cursors[demo]=expected | |
| else: | |
| self.cursors[demo]=max(self.cursors[demo]+0.9*elapsed*speed,bank['frame'][ids[best]]) | |
| candidates.append(ids[best]);scores.append(cost[best]) | |
| if not candidates: return None | |
| candidates=np.asarray(candidates); scores=np.asarray(scores) | |
| order=np.argsort(scores); anchor=candidates[order[0]] | |
| if CONFIG.get('lock_reference',False) and step>=40 and self.locked is None: self.locked=int(bank['demo'][anchor]) | |
| # Blend only nearby action directions; avoid averaging opposite pouring roles. | |
| compatible=np.mean((bank['action'][candidates]-bank['action'][anchor])**2,axis=1)<0.04 | |
| chosen=order[compatible[order]][:3] | |
| weights=np.exp(-(scores[chosen]-scores[order[0]])/0.08); weights/=weights.sum() | |
| action=np.sum(bank['action'][candidates[chosen]]*weights[:,None],axis=0) | |
| target_q=bank['proprio'][anchor] | |
| if self.dense is not None and CONFIG.get('dense_phase',False): | |
| demo=int(bank['demo'][anchor]); phase=float(self.cursors[demo]) | |
| aa=self.dense[f'a{demo}']; qq=self.dense[f'q{demo}'] | |
| lo=min(int(phase),len(aa)-1); frac=phase-lo | |
| target_q=qq[lo]*(1-min(frac,1))+qq[min(lo+1,len(qq)-1)]*min(frac,1) | |
| end=min(phase+speed,float(len(aa))) | |
| action=np.zeros(44,dtype=np.float32) | |
| weight=0.0 | |
| for t in range(lo,min(int(np.ceil(end)),len(aa))): | |
| w=max(0.0,min(end,t+1)-max(phase,t)) | |
| action+=aa[t]*w; weight+=w | |
| if weight>0: action/=weight | |
| self.rotation_level=max(float(np.linalg.norm(action[3:6])),float(np.linalg.norm(action[25:28]))) | |
| if step>40 and CONFIG.get('slow_rotation',False) and self.rotation_level>0.015: speed=min(speed,CONFIG.get('rotation_speed',1.5)) | |
| for arm,off,ao in [(0,0,0),(1,46,22)]: | |
| action[ao:ao+6]*=speed | |
| feedback_gain=CONFIG.get('orange_feedback_gain' if active_source_orange else 'blue_feedback_gain',CONFIG.get('feedback_gain',0)) | |
| if self.feedback is not None and feedback_gain>0 and step>40: | |
| error=target_q[off:off+7]-q[off:off+7] | |
| correction=error@self.feedback[arm,anchor]*feedback_gain | |
| action[ao:ao+6]+=np.clip(correction,-0.12,0.12) | |
| if CONFIG.get('hand_feedback_gain',0)>0: | |
| hand_error=target_q[off+7:off+23]-q[off+7:off+23] | |
| action[ao+6:ao+22]+=CONFIG['hand_feedback_gain']*hand_error/0.1 | |
| if scores[order[0]]>8: | |
| action=np.zeros(44,dtype=np.float32) | |
| self.previous_step=step | |
| self.previous_phase_speed=speed | |
| self.last_action=np.clip(action,CONFIG['action_low'],CONFIG['action_high']).astype(np.float32) | |
| self.diagnostic={'match_cost':float(scores[order[0]]),'reference_demo':int(bank['demo'][anchor]),'reference_frame':int(bank['frame'][anchor]),'orange_first':self.orange_first,'status':'unverified observation match'} | |
| # No success detector is claimed: let native horizon bound the attempt. | |
| return self.last_action.copy() | |
| _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) | |