+ UjbRt^RIHt^RIt^RIt^RIt^RIt^RIHt^RI t ^RI t ]PPRR4]!]4PP!4t]P$P'^]!]44^RIHt^RIHt^RIHt]R , t]R , R , t]!R 4tR RltR8tR9t R]3RRllt!Rt"Rt#RRlt$RRlt%R:t&R;t'] PP!]&R, ]'R, R.4t)RR R!llt-R?R"R#llt.R$R%lt/R&R'lt0R(R)lt1R*R+lt2R@R,R-llt3R.R/lt4RAR0R1llt5R/f_scene_umi.png — UMI gripper rendered from scene camera B. /f_scene_calib.png — scene calib board rendered from scene camera C. /f_wrist_calib.png — scene calib board rendered from wrist camera D. /f_wrist_umi.png — UMI gripper rendered from wrist camera, chained through a pre-calibrated wrist_cam→UMI transform. The wrist_cam→UMI transform (constant, since the wrist camera is rigidly attached to the UMI gripper) is recovered once from a chosen "calibration frame" where all three boards were detected, then cached to `wrist_umi_calibration.json`. Usage: MUJOCO_GL=osmesa python render_dataset_frame.py --dataset-root /data/cameron/puget_ar_datasets/testing_cube_in_bowl --frame 28 --calib-frame 184 --out /data/cameron/ar_renders/testing_cube_in_bowl ) annotationsN)Path MUJOCO_GLosmesa)Layer)UMI_V2) CALIB_BOARDzwrist_umi_calibration.jsoncalib_board_modelzcalib_board.xmlz7/data/cameron/cad_recovery/mj_umi_v2/umi_gripper_v2.xmlc(V^8dQhRRRRRRRR/#)K_save np.ndarrayH_renderintH_savereturnfloat)formats"G/data/cameron/claude_feetech_controller/ar_view/render_dataset_frame.py __annotate__r,s( > >j >C > > >c\VR,4pW8XdTpMW1V, ,pR\P!\P!VRV,, 44,#)zK_save is the intrinsics matched to the saved (500-wide) JPEGs. Since fovy is resolution-independent (fovy = 2 atan(H/(2 fy))), we can render at any size using the same fy scaled to that render height.@)r)rnpdegreesarctan)r rrfy_savefys&&& r _fovy_degr ,sR F4L!G  6) * BIIh#(&;<= ==r?c(V^8dQhRRRRRRRR/#)r real_bgrr sim_bgrweightrrr)rs"rrr?s)Z*e",rcVP^R7P\P4R, pVR8p\P!V4pW&V&VR,pVP\P4^V, ,VP\P4V,,p\P !V^^4P\P 4pVP\P 4^,p \P!V \P\P!\PR44p W8V ^8&V#)a)Composite the model onto the real image. Where the sim has drawn ANY pixel we blend heavily (default 90% sim) so the overlay is unmistakable; elsewhere the real image is preserved. A cyan silhouette outline is drawn along the sim mask so the align target is easy to see even when the sim is dark. axisgo@{Gz?).Nr+) maxastyperfloat32 zeros_likeclipuint8cv2 morphologyExMORPH_GRADIENTgetStructuringElement MORPH_RECT) r#r$r% outline_colorlummaskalphaaoutmask_u8edgess &&&& r_blendr??s ++1+  $ $RZZ 05 8C $JD MM# E$K iA //"** %Q /'..2Lq2P PC ''#q#  % %bhh /C{{288$+G   Wc&8&8 66s~~vN PE" N Jrc$^RIp\VPP4Fhp\ VPP V,4\ VP P48XgKM.ROVPPV&Kj R#)uMake every plane-type geom in the loaded model invisible (rgba alpha → 0) so the ambient floor/checker plane doesn't leak into the render — only real model meshes (like the calib board or the UMI gripper) survive on a black background.N)rArArA) mujocorangemodelngeomr geom_typemjtGeom mjGEOM_PLANE geom_rgba)layerrBgs& r _hide_floorsrLUs_ 5;;$$ % u{{$$Q' (C0K0K,L L'3EKK ! !! $&rc^RIp^RIHpV!VPVPW4VP VPVP 4VPPW434pVf,VPVPW44pWpPW43&VPVP VPR7VPpVPPVPPVPPVPP VPP"3Fp ^VP$V &K \&P(!VP+4\&P,4#)zLayer.render() but with skybox + floor + reflections stripped so the sim background is black and only the model's dynamic geoms (the mesh we care about) survive.N)set_cam)camera)rB render_corerNrDextcam mj_forwarddata renderersgetRenderer update_scenescene mjtRndFlag mjRND_SKYBOX mjRND_HAZEmjRND_REFLECTION mjRND_SHADOW mjRND_FOGflagsr2cvtColorrender COLOR_RGB2BGR) rJ T_camCV_wfovyHWrBrNrscnflgs &&&&& r _render_cleanrj_s# EKKy7 ekk5::. QF#Ay OOEKK ."#NN5::ellN3 ''C!!..!!,,!!22!!..!!++ -  # - << C$5$5 66rc$V^8dQhRRRRRR/#)r imgr textstrrr)rs"rrrvs! #*rc \P!VRVP^,^3RR4\P!WR\PRR^\P 4V#)rAg?)rArArArArA))rtrt)r2 rectangleshapeputTextFONT_HERSHEY_SIMPLEXLINE_AA)rlrms&&r_labelrzvsNMM#v ! b19bAKK7C$<$Nr+Nrq)rlinalginvarrayrrround)r|r}r~ T_world_camP_camzuvs&&& r_projectr|s))-- ,K "((GAJ GAJ#LM ME eAhADy2qy Bi!m B uRU| c%1,/ 22rrgc$V^8dQhRRRRRR/#)r umi_poser refrnrr)rs"rrrs!""j"s"*"rcVR8XdVR,P\4#\P!.\ORN4pW,R,#)uReturn the world-frame reference point for a UMI pose. 'corner' → cv2 GridBoard origin (buggy behaviour, kept for A/B comparison). 'center' → center of the physical UMI ArUco board (much closer to the gripper).cornerrrrr+)r-rrrUMI_BOARD_CENTER)rrlocals&& r_keypoint_worldrsH h%%e,, HH-'-- .E  b !!rc4V^8dQhRRRRRRRRRRR R R R/#) r base_bgrr umi_poseslist[np.ndarray]visible list[bool]r}r~rrnrr)rs"rrrsC   7G R\ #- 2< CF V` rc VPR,wrgVP4p.p \\W44Fgwp wr\ \ W4W44wrpV^8:gV ^8gV^8g W8gW8dV P R4KTV P WV 34Ki \\V 4^, 4Fp W,W^,,ppVeVfK$V \^\V 4^, 4, p\^P^V,,4\^^>" DA --/C C Y!89 8A?12KCa 6QUa!eqv JJt   A#; :3s8a< vsq5z1 9  As3x!|$ $R#'\"Cb1f $5s; aeQrUE1ckk: !#1 9  c As3x!|$ $aAQ"#q&s2a$_draw_axes_triads..s=*Q#a%i..*s"rr))rA)rrrt)r)Prr)r )rtr)rr)drrrq) rvrrrrrrr,tupler2rryr)rrrr}r~rrrerfr<NrrroriginRu0v0z0rr( color_full end_worldu1v1z1rr:s&&&&&&& @r_draw_axes_triadsrs >>" DA --/C IA Y!89 8A ( fIf15 B 7b1fQ"'R1W  Aq1u dQh!7 D*1d7j!88I!)[.fit2 BB U26]#Srv%7B JJsH % GGQ1Ir288 ,PQ1r6a-RA * *QVM1r6a-":L,L LMr)rrrr1) rrrrrrfittedtotal_wcanvasxrerfs &&&f @@r_hstackrs DAq$ $ec!feF $&kAoVqC 77G WWa!c'k7A.BHH =F A '(s7{A!eG#$ QW  M%sCc(V^8dQhRRRRRRRR/#rr)rs"rrrs+&?.8rcaaaVwooVVV3RlpVUu.uF qT!V4NK pp\V4^,^,pVS,V^,V,,p^S,^V,,p \P!W^3S\P4p \ V4FXwrV ^,V ^,rW,SV,,,pW-SV,,,pWZWS,1WS,13&KZ V #uupi)zYStack `cells` into a 2-column grid, aspect-fit each cell into `target_hw` with letterbox.c<VPR,wr\SV, S V, 4p\\W,44\\W#,44rT\P !WV34p\ P!SS ^3S \ P4pWgSV, ^,SV, ^,V,1S V, ^,S V, ^,V,13&V#rrrs & rr_grid_2col..fitrr)rrrr1r)rrrrrrrrowsgrid_hgrid_wrrrgcolyrrerfs&&&f @@r _grid_2colrs DAq$ $ec!feF $ K!O !D AXS( (F UQW_F WWfa("bhh 7F&!aQ3 q3w-  S/ !#$qQwa% " M%sDc$V^8dQhRRRRRR/#)r state_dirrframerrdictr)rs"rrr s!&&4&&&rcWR R2, pVP4'g \V4h\P!V4pVPUu/uF qDW4,bK up#uupi)06dz.npz)existsFileNotFoundErrorrloadfiles)rrrrks&& r _load_stater sVSk&&A 88::""  AWW %WqtGW %% %sA&c(V^8dQhRRRRRRRR/#) r rootrsubdirrnrrrr r)rs"rrrs(dC rcW, VR R2, p\P!\V44pVf \V4hV#)r.jpg)r2imreadrnr)rrrrrls&&& r _load_framers> 5+T**A **SV C {"" Jrc$V^8dQhRRRRRR/#)r metarcamrnrr r)rs"rrrs!88$8S8Z8rcT\P!W,R,\R7#)r dtype)rasarrayr)rrs&&r_K_saver s ::di) 77rc V^8dQhRRRR/#)r Mr rr)rs"rrr!s++z+j+rc\PPV4wrp\P!\PP W,44pV\P !RRV.4,V,#)zYNearest rotation matrix in Frobenius norm (Markley's method): SVD, drop determinant flip.r)rrsvdsigndetdiag)r U_Vtds& r_project_to_SO3r!sTyy}}QHA"  af%&A rwwS!}% % **rc$V^8dQhRRRRRR/#)r Tsrrrr list[int]r)rs"rrr(s"::,:::rc\P!VUu.uF q"R,NK up4p\P!V^R7p\PP W4, ^R7p\P!V4R,p\ V4UUu.uFwrxWV,8:gKVNK upp#uupiuuppi)uReturn indices of poses whose translation is within k*MAD of the median translation. Coarse outlier rejection — throws away frames where the underlying PnP obviously drifted.r'g& .>r)rrmedianrnormr) rrrtsmeddevmadrrs && r_pose_median_filterr!(s R(RU88R( )B ))BQ C ))... *C ))C.4 C#C. 9.$!ASLAA. 99 ) :sB7B<-B<c(V^8dQhRRRRRRRR/#) r dataset_rootr calib_framerforceboolrr r)rs"rrr2s/PPtP#PdPWaPrc \P4'dTV'gL\P!\P 44p\ P !VR,\R7#VR, p.p\VPR44Fp\ P!V4p\VR,4'd\VR,4'dw\VR,4'd]\ PPVR,4VR ,,pVP\VP 4V34KKKK V'g\%R V 2R R 7\'WA4p\ PPVR,4VR ,,p\P)\P*!RVP-4R R\/V4R\V4R^//^R74V#\1V!wr\3\5V 44p V U u.uF qV ,NK p p V U u.uF qV ,NK pp \ P6!R%4p\ P6!^4pV F"pWR&,, pVVR',, pK$ \9V4pV\;V 4, p\ P<!^4pVVR&&VVR'&\ P>!V Uu.uF qR',NK up4p\\ PPAVV, ^R7PC44p.pV FpVR&,VPD,p\ PF!\ PH!V4^, ^, R(R4pVP\ PJ!\ PL!V444K \\ PB!V44p\%R\;V4 R\;V 4 R2R R 7\%RVR,R RVR R2R R 7\P)\P*!RVP-4R R\/V4RRR\;V4R\;V 4R VR!,R"\OVR,^4R#\OV^4//^R74\%R$\ 2R R 7V# \"dEKdi;iuup iuup iuupi))usReturn T_wrist_cam→umi (4x4). Strategy: 1. If cached and not forced, return the cached value. 2. Otherwise sweep every state npz in the dataset, keep only frames where scene/wrist/umi are ALL visible (so both PnP solves are trustworthy), compute T_wc_umi(f) = inv(umi_pose(f)) @ wrist_cam_pose(f). 3. MAD-filter outliers on translation, then chordal-average the rotations via SVD and take the mean translation. That reduces PnP noise from a single frame by ~√N and rejects bad PnP solves (small board area / grazing angle). 4. If nothing usable is found, fall back to the single `calib_frame`. T_wrist_cam_umirstatez*.npz scene_visible wrist_visible umi_visiblerwrist_cam_poseuK[calibration] no frames with all three boards visible — falling back to fTflushrecovered_fromdatasetrn_frames)indentr'rz[calibration] u tri-visible frames → z after MAD filterz([calibration] residual std: translation=z.2fz mm, rotation=°methodz8multi-frame chordal average, MAD-filtered on translationn_frames_considered n_frames_kept frames_kept:NNtranslation_std_mmrotation_std_degz[calibration] wrote r*rrg)( CALIB_PATHrjsonloads read_textrr rsortedglobrr&rrrrstemKeyErrorprintr write_textdumpstolistrnrr!listzerosrreyerrstdrr0tracerarccosr)r#r$r%rr per_framerstrframesrkeeprTs_keep frames_keepRsumtsumR_meant_meanT_meanrt_stdang_devRdcosang_stds&&& r_load_or_calibrate_wrist_umir_2s5 JJz++- .zz!-.e<<w&I.0I INN7+ , WWQZ B'((T"_2E-F-F4PRS`PaKbKbIIMM"Z.1B7G4HH  #aff+q!12Lc-F(-  [\g[hiquv  0 IIMM"Z. )B/?,@ @djj qxxz y#l*;Wc+FVXbdef*  iJF tBx (D"#d!uudG#&*+d!99dK+ 88F D 88A;D  &  %T "F CL F VVAYFF6NF5M W-WU88W- .B "))..f1.599; **B3w<.Pa bjno 4U4Z4DNSZ[^R__a bjno$**6==? s<( P !3y> S\ ;s+ % a"8 gq 1  &    -T: Mo   $+.s%-B"S(*S;T9T( S87S8c V^8dQhRRRR/#)r gripper_radr joint_namernr)rs"rrrs 7 7% 7S 7rc^RIp\P!V4'gR#VPVPVP P V4pV^8dR#\VPPV,4p\V4VPPV&VPVPVP4R#)zWrite the servo-derived jaw angle into the UMI model's jaw joint before rendering. gripper_rad comes from state/*.npz and already carries the calibration jaw_sign, so it can be written to qpos directly (MuJoCo will clip to the joint range on step).N) rBrisfinite mj_name2idrDmjtObj mjOBJ_JOINTr jnt_qposadrrrSqposrR) layer_umirarbrBjidadrs&&& r _pose_umi_jawrms ;;{ # #   IOOV]]-F-F SC Qw ioo))#. /C$[1INN iooy~~6rc$V^8dQhRRRRRR/#)r rrrrnrr r)rs"rrrs(:a:as:a-0:a@J:arc\VPRV4p\VPRV4p\VPR, V4pVR,pVR,pVR,p\V \ VP R\ P444\WPV,WhV 4p\WP\ PPV4,V,WhV 4p\VV\R7p\VV\R7p\VR V R \!VR ,4 R \!VR ,4 R\ VP R\ R444R 24p\WPV,WzV 4p\WPV,WzV 4p\VV\R7p\VV\R7p\VRV R\!VR,4 24p\!VP"4p\!VR,4p\%\'V\)VV,^,V444p.p.pVFsp \VPR, V 4p!VP+\ P,!V!R,\R74VP+\/V!R ,44Ku \1VVVVWNR7p"\3V"VVVWNR7p"\V"R\5V4^, RV RV R24p"VV,p#\1VVVV#W^R7p$\3V$VVV#W^R7p$\V$R\5V4^, RV RV R24p$\7VVV"V$.RR7#)uxRender a single frame → horizontal-stack composite (scene | wrist | future). Returns the BGR composite ready to write.rXwristr)scene_cam_poser-rra)r7uscene · calib+UMI fz sv=r*z uv=r,z grip=nanz+.2fuwrist · calib+UMI (chained) fz wv=r+ num_framesrruscene · next z keypoints+axes fz [ref=]uwrist · next )rii)rr#rrmrrUrrrrj T_board_wrrr?CYANMAGENTArzrhorizonrIrCrrr r&rrrr)%rargsrT_wc_umiK_sceneK_wrist fovy_scene fovy_wristHsWsHwWw layer_scenerjrrXrprPrqr-rsim_scene_calib sim_scene_umi scene_combosim_wrist_calib sim_wrist_umi wrist_comborztotal fut_framesrumi_visfstf future_scenewrist_cam_derived future_wrists%&&&&&&&&&&&&&&& r _render_onersE ))7E :E ))7E :E T&&0% 8B()N()N ^H)U266-#@AB$K1F1F1WYciklO#I1D1Dryy}}U]G^1^ao1o$.B8MtDKm7KK08"2o#678SMAR=SR4, 2RR&7\'VPVP(VP*4p\-VR'4p\-VR(4p\/VPR'V^,4p V P0R),wr\/VPR(V^,4p V P0R),wr\3WzV 4p\3WV 4p\#R*RR&7\5R'\ \64\8R+R,4p\;V4\#R-VP< 2RR&7\#R.RR&7\5R/\ \>4\@R0R14p\;V4\#R2VP< 2RR&7^RI!pVPC4p\EV4EF^wpp\GVWWgVWWWVVVPHR37pVPVPJPMVR47, pVPNPQ4R?9dF\RPT!\ V4V\ \RPV4VPX.4M \RPT!\ V4V4V^,^,^8XgV^,\%V48XgEKVPC4V, p\#R5V^, R6\%V4 R7VR8 R9VV^,, R:,R; R<2 RR&7EKa \#R=VP 24R#uupi)@z--dataset-rootTz4dataset root (has meta.json, scene/, wrist/, state/))requiredtypehelpz--framesNu>which frames to render — "all" | "28" | "0-100" | "28,50,90")rdefaultrz--framez"convenience alias for --frames z --calib-frameu@frame with all three boards visible for wrist_cam→umi recoveryz--outzoutput directoryz --force-calib store_trueu+ignore cached wrist_cam→umi and re-derive)actionrz --horizonz;how many future frames to draw in the trajectory + axes vizz --name-fmtzf{frame:06d}.jpgzDoutput file naming template (jpg default so the scrubber loads fast)z--jpeg-quality)rrz--refrcenteruwhich UMI point to project as the 'gripper keypoint': 'corner' = raw cv2 GridBoard origin (marker 150 corner — off the visible gripper), 'center' = board center (much closer to gripper body). default center.)choicesrr)parentsexist_okz meta.jsonrszsupply --frames or --framez[batch] rendering z frame(s): :NNu … r.rXrpruN[layer] loading scene calib_board.xml + measure_board_world (loop-closure) …calibcR#rrjns&rmain.. sTXrz[layer] scene reproj=u<[layer] loading umi_gripper_v2.xml + measure_board_world …umiar_backcR#rrrs&rrrs4rz[layer] umi reproj=rt)rz[batch] /z done (z.1fzs, r4z.0fz ms/frame)z[done] output dir: )rz.jpeg)-argparseArgumentParser add_argumentrrnr parse_argsr<mkdirr>r?r#r@rQrerrorrrErr_r$ force_calibr rrvr r SCENE_MODELrrLreproj UMI_MODELrtimerrrname_fmtrsuffixlowerr2imwriteIMWRITE_JPEG_QUALITY jpeg_quality)apr{rrrQrr|r}r~proberrrrrrrrjrt0rrrlout_pathdts rmainrs  "BOO$t$OQOOJS$Y[OOIC=?OOO#s[]OOGdE>a>aF 2 s6{m;6{a'VS_W-DSPRPSEU-U V X_ce,D,=,=t?O?OQUQaQabHdG$GdG$G ))7F1I >E [[_FB ))7F1I >E [[_FB7+J7+J Zbfg[!1;YK !+"4"4!5 6dC HPTUeS^VYPI  0 01 2$? Bf%5%X$""%ydhh@88dmm222?? ?? "&7 7 KKH sS1I1I-JDL]L],^ _ KKH s + ER<1 AV 4r!B HQUG1S[M"SR1Xd]SVDWWabjn o o& z *+Q3s+U(U(U(__main__)rArtrt)rtrArtgRQ?gQ?)r)r)r)rvrrp)rvrrrp)g@)jaw):__doc__ __future__rrr>ossyspathlibrnumpyrr2environ setdefault__file__parentresolveAR_VIEWpathinsertrnrPr aruco_boardsr calib_boardrr=rrr rxryr?rLrjrzr UMI_BOARD_W UMI_BOARD_Hrrrrrrrrrr rr!r_rmrrr__name__rrrrsX,#  k8, x.   ' ' )3w< #3 3 ++.?? LM  > GJ,47. 3"$ # 88[3. c0A3GH" F>,2&8+:Pf 7:az:G,T zFr