+ PUj+ :Rt^RIt^RIt^RIt^RIt^RIt^RIt^RIt]PP^]PP]PP]PP] 4444^RIHtHt^RIHt^RIHtHtHt^RIHt^RIHt^RIHtHt]PP]PP] 44t]PP?]R4t R t!R t"R t#]$R 8Xd ]#!4R#R#) azRecord a localization dataset from the scene + wrist cameras. World frame = the calib board's ArUco (GridBoard) frame. Everything is solved by PnP on the boards in the scene image (no MuJoCo needed): intrinsics : scene-camera K, solved once from the first --calib-frames views (saved in meta). scene_cam_pose : the scene camera's pose in the world frame, per frame (OpenCV convention, z-forward). If the calib board isn't visible, the last known pose is reused. umi_pose : the UMI gripper's ArUco pose (UMI_V2 board) in the world frame, per frame (reuses last known when not visible; a visibility flag is stored). wrist rgb : the wrist camera frame (downscaled, aspect preserved). gripper : the jaw servo tick + calibrated radians. Images are saved downscaled (default 500px wide) at --fps (default 4). Layout: datasets// { meta.json, scene/NNNNNN.jpg, wrist/NNNNNN.jpg, state/NNNNNN.npz } Usage: python record_dataset.py [--scene-index 1] [--wrist-index 0] [--fps 4] [--scene-width 500] [--wrist-width 500] [--calib-frames 10] [--overwrite] [--port ...] N)Bus ticks_to_rad) calib_path) detect_boardsestimate_focal solve_pose) CALIB_BOARD)UMI_V2) open_namedload_intrinsicsdatasetsc\\VP^,V,VP^,, 44p\P!WV34#))introundshapecv2resize)framewidthhs&& ar_view/record_dataset.py downscaler*s> E%++a.5(5;;q>9 :;A ::eQZ ((c \P!V\\VP^,V,VP^,, 44V34#))rrrrr)rrs&&r to_heightr/s< ::ec% A(:U[[^(K"LMqQ RRrcXaB\P!4pVPR4VPR\^R7VPR\^R7VPR\RR7VPR\RR7VPR\RR7VPR \^ R7VPR R R 7VPR R R 7VPRRR7VP 4p\ P!\\R444p\RXP44R4pVf\P!R4VwrE\P P#\$VP&4oBVB3RlR_4wrgp\P P)V4'dS\P*!V4'd7VP,'g%\P!RVP& R24WgV3Fkp \P.!V RR7\P*!V 4F6p \P0!\P P#W44K8 Km \3RVP44wrp \3RVP64wrpV eV f\P!R4Vf\P!R4V P8^,V P8^,pp\;RV RV R V R!V 24\;R"VP< R#24.p\?V4VP<8EdV PA4wppV'dVfK<\C\DPF!V\DPH4\J.4p R$V 9dVPMV R$,4VPN'dK\DPP!VR%\?V4 R&VP< 2R`\DPRR'Ra^4\DPT!R(\WVR)44\DPX!^4EK \[VVV3R'\]VV4,R*7wpppVP^\ V4, pVPa4pV^;;,V,uu&V^;;,V,uu&\;R+VRb,R, R-V R V R.VR/ R02 4VP8^,VP8^,pp\cR4pVeV\ VR1,4, pVR2,Pa4pV^;;,V,uu&V^;;,V,uu&VPd\ V4, p VPa4p!V!^;;,V ,uu&V!^;;,V ,uu&\;R3VRb,R, R-V R V 24MR;pp!\;R44VPf'd\iVPf4M \i4p"V"PkV4'g\P!R5V R624V"PmVR74R8p#\oVP&VPpYER9R:R;\oV#!\J4V#!\r4R<7\oVV.VP^VPu4VPu4\ V4R=R>7\oVV.VPdVeVPu4MRV!eV!Pu4MRVeR?MRR@7\vPx!RA4RB7 p$\ Pz!V$\\P P#SBRC4RD4^RE7\;RFVP& RGVPp RHSB RI24\|P~!^4p%\|P~!^4p&\|P~!^4p'RJVPp, p(^p)\DP^_.p*\vP!4p+V PA4wp,p-VPA4wp.p/V,'dV-fEM\C\DPF!V-\DPH4\J\r.4p0R$V09p1V1'd8\.V0R$,OVN5!wp2p\|PPV24p%T%p3RKV09p4V4'd"\.V0RK,OVN5!wp5pV3V5,p&T&p6R7p7V.'d}V/eyVeu\C\DPF!V/\DPH4\J.4p8R$V89d:\.V8R$,OVN5!wp9p\|PPV94p'Rp7T'p:V"PV4p;V;e \V;V4M \ RL4p\DPP!T>RRT) RSV1'dRTMRU RVV4'dRTMRU RWV; 2Rd\DPRRXRe^4V.'daV/e]\V/RQ4p?\DPP!T?RYV7'dRTMRU 2Rd\DPRRXRe^4\|P!V>V?.4p@MT>p@\DPT!R(X@4\DPX!^4^,\RZ48XdMF\vP!4V+, pAVAV(8gEK\vP!V(XA, 4EKV$PV)\vPx!RA4R\7\ Pz!V$\\P P#SBRC4RD4^RE7V P4VP4\DP!4V"P4\;R]V) R^SB 24R# \d\P!R4E Li;i \d\;R[4ELi;i T$PT)\vPx!RA4R\7\ Pz!T$\\P P#SBRC4RD4^RE7T P4TP4\DP!4T"P4\;R]T) R^SB 24i;i)ftaskz --scene-index)typedefaultz --wrist-indexz--fpsg@z --scene-widthiz --wrist-widthz--calib-framesz --overwrite store_true)actionz --no-previewz--portN)r gripperz9No calib/gripper.json. Calibrate the gripper servo first.c3"TF4wrVR,R9gK\V4\VR,43xK6 R#5i)jointsignN)jaw gripper_jaw)r).0sidcs& r main..Fs79vsW:!77+S3qy>*s>%>z/No jaw/gripper_jaw servo in calib/gripper.json.c3d<"TF%p\PPSV4xK' R#5i)N)ospathjoin)r)dddirs& rr,r-Ns%T8S1dA..8Ss-0scenewristz Dataset 'z(' exists. Use --overwrite or a new name.T)exist_okz%Scene camera didn't open / no frames.zWrist camera didn't open.zscene cam idx z @ xz wrist cam idx zRecording the first z: frames for scene intrinsics (point at the calib board)...calibzscene intrinsics /g?record_dataseti)f_guessz scene f=z.0fzpx @ z , reproj z.2fpxrKz wrist intrinsics loaded: f=u` wrist intrinsics NOT found (run calibrate_camera.py wrist) — wrist pose off, RGB still savedzGripper servo z not responding.Fc\\VR,^,4\VR,R,4.\VR,4VR,VR,R7#)idsrmarker_msep_m)r?rr@rA)dictrlist)specs&r board_metamain..board_metasNT%[^,c$u+b/.BC4PTU\P]K^!*-T']D Dropencv_z_forwardzzero (pinhole)z)calib board GridBoard frame (ids 109-132))r8umizlive (this run)) native_wh save_widthK_nativeK_saveintrinsics_reproj_pxintrinsics_sourcezsaved intrinsics/wrist.json)rJrKrLrMrOz%Y-%m-%d %H:%M:%S) rfps jaw_servo_idjaw_signcamera_convention distortion world_frameboardsr4r5startedz meta.jsonw)indentz Recording 'z' at z fps -> z+ 'q' in the preview (or Ctrl-C) to stop. g?umi_v2nan06dz.jpgz.npz) scene_cam_poseumi_posewrist_cam_pose scene_visible umi_visible wrist_visible gripper_tick gripper_radtizREC #z scene:Ynz umi:z grip:gffffff?z wrist board:qz stopped (Ctrl-C).) num_framesendedz Saved z frames to )r4r5state) )r)rrrB)rl)rror)RargparseArgumentParser add_argumentrfloat parse_argsjsonloadopenrFileNotFoundErrorsysexitnextitemsr/r0r1DATASETSrisdirlistdir overwritemakedirsremover scene_index wrist_indexrprint calib_frameslenreadrrcvtColorCOLOR_BGR2GRAYrappend no_previewputTextFONT_HERSHEY_SIMPLEXimshowrwaitKeyrmax scene_widthcopyr wrist_widthportrpingtorquerCrPr tolisttimestrftimedumpnpeyeIMWRITE_JPEG_QUALITY perf_counterrlinalginvread_posrimwritesavezrhstackordsleepKeyboardInterruptupdatereleasedestroyAllWindowsclose)Capagcalr'jaw_idrRsdirwdirstdirr2fscapsidxsfrwcapwidxwfrnatWnatHviewsokrr=_rmssrMnatWwnatHwwintrswK_wristsws K_wrist_savebusrFmeta last_scenelast_umi last_wristperiodijpgt0okssframeokwwframedets scene_visTcb_cam scene_poseumi_visTumi_camr^ wrist_viswdTcb_wcam wrist_posetickradnamedsdwdispdtr3sC @rmainr3s  "BOOFOOO#qO9OOO#qO9OOG%O5OOO#sO;OOO#sO;OO$3O;OOM,O7OON @C { BCF 77<<!&& )DT8STD ww}}Trzz$//  9QVVH$LMN%  A%AA IIbggll1( )! !!--8OD !--8OD |s{ 89 | ,-1syy|$D N4&D64&0A$ HI  00j kl E e*q~~ %IIK EU]  #,,uc.@.@AK= Q a< LL7 $||| KK!23u:,a?OPRZ00#}a I JJ'5#)> ? KKNutTlC#dD/* **//#WQZ2%5ZwqzR7GzmmeEl*||~ |A#'=|ARU?U -gcl3-?uUG1UGTU!%%, pq#aff+SUC 88F   >&)9:;JJvuD QVVV"4AQG*["9z&?QRdD\amm$%HHJv}}05c N_aeU^ 9@9LW^^%5RVqI M!&&quugXdV;h ijJvvayHJ 155[F A  # #R (CE/""$B))+KC))+KC&. fc6H6H!IKY_K`aD4I':g:: YY]]73 #J$&G(<$x.D(CH4""$r)BF{ 6B;' q 6I(J K $RWW\\$ rs(  277??277??277??83L#MNO)AA#/ wwrwwx01 77<<j )) Sr/j zFr