Download astra/DualArmClothHanger-v1/controller.py from Linslab/ManiLadder_Models: direct link, hf CLI and curl.
- Browser
- Download file 5.97 kB
-
https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmClothHanger-v1/controller.py
- Command line
-
hf download hf://Linslab/ManiLadder_Models/astra/DualArmClothHanger-v1/controller.py
-
curl -L -o controller.py https://huggingface.co/Linslab/ManiLadder_Models/resolve/main/astra/DualArmClothHanger-v1/controller.py
5.97 kB
| """D20 geometric trajectory tracking with legal-image reference selection.""" | |
| from pathlib import Path | |
| import importlib.util | |
| import numpy as np | |
| import cv2 | |
| _ROOT=Path(__file__).resolve().parent | |
| for _name in ('features','kinematics','visibility','active_calibration'): | |
| _spec=importlib.util.spec_from_file_location('_local_'+_name,_ROOT/(_name+'.py')) | |
| _mod=importlib.util.module_from_spec(_spec);_spec.loader.exec_module(_mod) | |
| globals()['_'+_name]=_mod | |
| _memory=None;_poses=None;_initial=None;_left1=None;_hanger=None | |
| _phase=0.;_last_step=0;_demo=None;_bad=0;_hanger=None;_vision=None;_hanger_offset=0.;_vest_offset=0.;_lost_checks=0;_misplaced_checks=0;_stop_reason=None;_probe=None;_depth_offset=0.;_calibration_report={} | |
| GAIN=10. | |
| DEPTH_OFFSET=0. | |
| def reset_episode(): | |
| global _phase,_last_step,_demo,_bad,_hanger,_hanger_offset,_vest_offset,_lost_checks,_misplaced_checks,_stop_reason,_probe,_depth_offset,_calibration_report | |
| _phase=0.;_last_step=0;_demo=None;_bad=0;_hanger=None;_hanger_offset=0.;_vest_offset=0.;_lost_checks=0;_misplaced_checks=0;_stop_reason=None;_probe=None;_depth_offset=0.;_calibration_report={} | |
| def act(obs): | |
| global _memory,_poses,_phase,_last_step,_demo,_bad,_initial,_left1,_hanger,_vision,_hanger_offset,_vest_offset,_lost_checks,_misplaced_checks,_stop_reason,_probe,_depth_offset,_calibration_report | |
| step=int(obs['step_id']) | |
| if step>=1500:return None | |
| if step<=_last_step:raise ValueError('Nonincreasing step') | |
| _last_step=step | |
| if _memory is None: | |
| with np.load(_ROOT/'memory.npz') as d:_memory={k:d[k] for k in d.files} | |
| with np.load(_ROOT/'poses.npz') as d:_poses={k:d[k] for k in d.files} | |
| with np.load(_ROOT/'initial.npz') as d:_initial={k:d[k] for k in d.files} | |
| with np.load(_ROOT/'left1.npz') as d:_left1={k:d[k] for k in d.files} | |
| with np.load(_ROOT/'vision_refs.npz') as d:_vision={k:d[k] for k in d.files} | |
| q,v=_features.proprio_feature(obs['proprioception']) | |
| if _demo is None: | |
| im=_features.image_feature(obs['rgb']);idx=np.flatnonzero(_memory['frame']==0) | |
| cost=np.mean((_memory['image'][idx]-im)**2,axis=1) | |
| _demo=int(_memory['demo'][idx[np.argmin(cost)]]) | |
| left=cv2.resize(np.asarray(obs['rgb']['left_camera']).squeeze(),(160,120)).astype(np.float32) | |
| hc=[np.mean((left[:12,90:98]-_left1[str(k)][:12,90:98].astype(np.float32))**2) for k in range(20)] | |
| _hanger=int(np.argmin(hc)) | |
| def cap(im): | |
| gray=im[:24,180:196].astype(np.float64).mean(2) | |
| ink=np.clip((110-gray)/70,0,1).sum(1);w=np.maximum(ink-2.6,0) | |
| return float((w*np.arange(24)).sum()/max(w.sum(),1e-8)) | |
| cy=np.array([cap(_vision[f'left_camera_{k}']) for k in range(20)]) | |
| yy=np.array([_kinematics.fk(_poses[f'qpos_{k}'][51,9:16])[1,3] for k in range(20)]) | |
| coeff=np.polyfit(cy,yy,1) | |
| live=cv2.resize(np.asarray(obs['rgb']['left_camera']).squeeze(),(320,240),interpolation=cv2.INTER_AREA) | |
| estimate=float(np.clip(np.polyval(coeff,cap(live)),yy.min(),yy.max())) | |
| _hanger=int(np.argmin(abs(yy-estimate))) | |
| _hanger_offset=float(estimate-yy[_hanger]) | |
| def vest_center(im): | |
| r,g,b=im.astype(np.float32).transpose(2,0,1) | |
| mask=((r>180)&(g>130)&(b>100)&(r-g>15)&(r-g<80)&(g-b>5)).astype(np.uint8) | |
| n,labels,stats,centers=cv2.connectedComponentsWithStats(mask) | |
| if n<=1:return np.nan | |
| i=1+int(np.argmax(stats[1:,cv2.CC_STAT_AREA])) | |
| return float(centers[i,0]) if stats[i,cv2.CC_STAT_AREA]>=500 else np.nan | |
| centers=np.array([vest_center(_vision[f'robot_camera_0_{k}']) for k in range(20)]) | |
| anchors=np.array([int(np.flatnonzero(_poses[f'a_{k}'][:,3]>.09)[0]) for k in range(20)]) | |
| positions=np.array([_kinematics.fk(_poses[f'qpos_{k}'][anchors[k],:7])[1,3] for k in range(20)]) | |
| live_vest=cv2.resize(np.asarray(obs['rgb']['robot_camera_0']).squeeze(),(320,240),interpolation=cv2.INTER_AREA) | |
| cx=vest_center(live_vest);valid=np.isfinite(centers) | |
| if np.isfinite(cx) and valid.sum()>=3: | |
| calibration=np.polyfit(centers[valid],positions[valid],1) | |
| _vest_offset=float(np.clip(calibration[0]*(cx-centers[_demo]),-.015,.015)) | |
| if _probe is None:_probe=_active_calibration.Probe(_ROOT,_poses,_demo,_kinematics.fk) | |
| probe_axis=_probe.observe(step,obs,q) | |
| if _probe.accepted:_depth_offset=_probe.offset_x;_vest_offset=_probe.offset_y | |
| _calibration_report=_probe.diagnostic.copy() | |
| actions=_poses[f'a_{_demo}'];refs=_poses[f'qpos_{_demo}'] | |
| roll_start=int(np.flatnonzero(actions[:,3]>.09)[0]) | |
| rot_end=int(np.flatnonzero(np.any(np.abs(actions[:,3:6])>.001,axis=1))[-1]) | |
| t=step-1;_phase=t/len(actions) | |
| release=int(np.flatnonzero(np.diff(actions[:,6])>0)[0]+1) | |
| if rot_end+60<=t<=release+20 and step%5==0: | |
| _lost_checks=_lost_checks+1 if _visibility.cloth_area(obs['rgb'])<20 else 0 | |
| if _lost_checks>=5: | |
| _stop_reason='persistent_cloth_loss';return None | |
| if release-5<=t<=release+25 and step%5==0: | |
| _misplaced_checks=_misplaced_checks+1 if _visibility.carried_position_error(obs['rgb'],_demo)>25 else 0 | |
| if _misplaced_checks>=3: | |
| _stop_reason='cloth_not_carried_to_center';return None | |
| if t>=len(actions)+40:return None | |
| ti=min(t,len(actions)-1) | |
| a=actions[ti].copy() if t<len(actions) else np.zeros(14,dtype=np.float32) | |
| if t>=len(actions):a[[6,13]]=actions[-1,[6,13]] | |
| target=refs[min(t,len(refs)-1)].copy() | |
| if t<220: | |
| target[9:]=_poses[f'qpos_{_hanger}'][t,9:] | |
| a[7:]=_poses[f'a_{_hanger}'][t,7:] | |
| maxerr=0. | |
| for j in range(2): | |
| now=_kinematics.fk(q[j*9:j*9+7]);ref=_kinematics.fk(target[j*9:j*9+7]) | |
| if j==0: | |
| weight=min(float(np.clip((t-roll_start+40)/20.,0,1)),float(np.clip((rot_end+100-t)/60.,0,1))) | |
| ref[1,3]+=_vest_offset*weight | |
| ref[0,3]+=_depth_offset*weight | |
| if j==1 and 3<=t<120: | |
| ref[1,3]+=_hanger_offset*min(1.,max(0.,(120-t)/35.)) | |
| err=ref[:3,3]-now[:3,3];maxerr=max(maxerr,float(np.linalg.norm(err))) | |
| a[j*7:j*7+3]+=np.clip(GAIN*err,-.3,.3) | |
| rot=_kinematics.rotation_error(ref[:3,:3],now[:3,:3]) | |
| a[j*7+3:j*7+6]+=np.clip(-2.*rot,-.15,.15) | |
| if maxerr>.15:_bad+=1 | |
| else:_bad=0 | |
| if _bad>30:return None | |
| if probe_axis is not None:a[probe_axis]+=.1 | |
| return np.clip(a,-1,1).astype(np.float32) | |