_zi&SrSSKrSSKrSSKJr SSKrSSKJs Jr SSK r SSK r SSK Jr SSKrSSKrSSKrSSKrSSKrSSKrSSKJr \R2R5S\R2R7\R2R9\5S55 \R2R5S\R2R9\55 SSKJrJ r SSK!J"r"J#r#J$r$J%r%J&r& SSK'J(r( SS K)J*r* SS K+J,r,J-r-J.r.J/r/J0r0 SS K1J2r2 SS K3J4r5 S r7Sr8\ Rr"/SQ/SQ/SQ/SQ/SQ/5S- r:Sr;\:\;r<\ Rr"/SQ5r=\ Rr"/SQ5r>Sr?Sr@SSjrA\74SjrBSrC\DS:Xa\C"5 gg!\6a Sr5Nf=f)zYLive test trajectory predictions with 3D lifting and IK visualization from camera stream.N)Path)Rotationz..)N_WINDOWproject_3d_to_2d)TrajectoryHeatmapPredictor MIN_HEIGHT MAX_HEIGHT MIN_GRIPPER MAX_GRIPPER)SO100AdhesiveConfig)EXOSKELETON_CONFIGS)get_link_poses_from_robotposition_exoskeleton_meshesrender_from_camera_posedetect_and_set_link_posesestimate_robot_state)Arm) make_gridiz1real_dino_tracks/checkpoints/real_tracks/best.pth)*@{GVg/@)g ףp=%@gfffffXr)rrg/)g(\1@g= ףpTr)g\(6@g= ףpQr@@)g2(g݃c؁gkԅPɒ?)g8:!𗑿gMkg(!u?gB6 ?ct[RRU5SS2S4n[RRU5n[R"USUSS/5nXE-n[RRU5SS2SS24nXv-nU[RR U5- nX84$)aBUnproject 2D point to camera ray in world coordinates. Args: point_2d: (2,) 2D pixel coordinates camera_pose: (4, 4) camera pose matrix (world-to-camera) cam_K: (3, 3) camera intrinsics Returns: cam_pos: (3,) camera position in world frame ray_direction: (3,) normalized ray direction in world frame Nrr?)nplinalginvarraynorm) point_2d camera_posecam_K cam_pos_worldK_inv point_2d_hray_cam cam_rot_world ray_worlds .real_dino_tracks/vistest_live_test_model_ik.pyunproject_2d_to_rayr+6sIIMM+.rr1u5M IIMM% E8A; S9:J GIIMM+.rr2A2v6M'IBIINN955I  ##c~[XU5upE[US5S:agXS- US- nUS:agXFU--nU$)a!Recover 3D keypoint from 2D projection and height. Args: kp_2d_image: (2,) 2D pixel coordinates height: height (z-coordinate in world frame) camera_pose: (4, 4) camera pose matrix cam_K: (3, 3) camera intrinsics Returns: (3,) 3D keypoint position, or None if invalid gư>Nr)r+abs) kp_2d_imageheightr"r#cam_pos ray_directiontpoint_3ds r**recover_3d_from_direct_keypoint_and_heightr6Rs`15QG = t# !*  a 00A1u]**H Or,c fSnSn[U5GH n[R"X5 URUR5 [ R "SSSSS9n U R[ R"[R"XC/5S95 [ R"USS 9n U RUR5 [ R"X)U /US [ R"US 9/S 9n URX5 URURS S &[R"X5 GM# [R "U[R"R$S5n UR&U n X=- n[R(R+U5nURR-5U4$)zRun IK to move virtual gripper keypoint to maintain offset from cube. Returns: optimized_qpos: (nq,) array of optimized joint positions final_error: scalar error in meters {Gz?rvirtual_gripper_keypointbodygMbP?) position_costorientation_cost)wxyz_xyzg-C6?)costdaqp)model)limitsN)rangemujoco mj_forwardupdateqposmink FrameTask set_targetSE3r concatenate PostureTasksolve_ikConfigurationLimitintegrate_inplaceq mj_name2idmjtObj mjOBJ_BODYxposrr copy)r@data configuration target_postarget_kp_quat_wxyznum_iterationsdtdamping iterationkp_task posture_taskvel kp_body_idcurrent_kp_pos pos_error error_norms r*ik_to_cube_graspreqs[ BG>* %&TYY'..!;VSVino488R^^=P<]-^_`''D9  *mmM\+BBX\XoXov{X|W}~''0$ !  %&'+,""5&--*B*BD^_JYYz*N+I *J 99>> Z ''r,c0UR5S:a"UR[R5S- nURSSup#X!:wdX1:wa%[ R "XU4[ RS9n[R"U5RSSS5R5n[R"/SQ5RS SS5n[R"/S Q5RS SS5nXE- U- nUR5nXG4$) zPreprocess RGB image for model input. Args: rgb: (H, W, 3) RGB image in [0, 255] or [0, 1] image_size: Target image size Returns: rgb_tensor: (3, H, W) normalized tensor rgb_vis: (H, W, 3) visualization image in [0, 1] ro@Nr. interpolationrr)g ףp= ?gv/?gCl?r)gZd;O?gy&1?g?)maxastyperfloat32shapecv2resize INTER_LINEARtorch from_numpypermutefloattensorviewrU)rgb image_sizeH_origW_orig rgb_tensormeanstdrgb_viss r*preprocess_imagers wwy3jj$u,YYr]NFv3jj:6cFVFVW!!#&..q!Q7==?J <<- . 3 3Aq! nS?n[<R>RAU5(aFURB(d5[E[FR*"[IUS@555n[!SA5 O[!SBUSC35 S n[JRL"/SDQ5n URO5n URQU S SE9 URO5n [JRR"[JRT"X- 55SF:aOU n ME[!SG5 [W5n [XRZR]U R^5n [XR`"U 5n[bRd"URf5nURi5(d[kSHURf35eURm5unnU(dURm5unnU(dM[bRn"U[bRp5nURrSSIunn[!SJUSKU35 Sn[!SL5 [!SM5 [!SN5 S[tl;S>n[xRx"5nURm5unnU(d [!SO5 M=[bRn"U[bRp5nSnU(aUURB(dDURO5nU=URzSS&UR|SS&[XR~"X5 [UXU USP9unnnnnn[XUU5 U(dJ[XU USQSR9nUR=URzSS&UR|SS&[XR~"X5 UbUc[!SX5 GMG[UR5[&5un"n#[XU5SYn$U$SSZ2SSZ24n%U$SSZ2SZ4n&U%[-U&-n'[U'UU5n(U(c[!S[5 GM[&U- n)[&U- n*U([JRL"U)U*/5-n+U'SIn,URO5n[US\5n-S S]KQJRn. U."U-5n/[R"5 U"RS 5R/U5n0[R"U+5R5R/U5n1[R"U,[RS^9R/U5n2[R"U/[RS^9R/U5n3U"U0SS U1U2U3S_9un4n5n6SSS5 /n7/n8[[(5Hn9W4S U94n:[R"U:RS\5S S`9RU:5R5R5n;U8RU;5 [JR"U;R5U;Rr5unU7RU>5 M [JRL"U75n7[JRL"U85n8W5S R5R5n5W6S R5R5n6UR5n?U?S ==U-ss'U?ST==U-ss'U?R5n@U@S ==[&-ss'U@ST==[&-ss'[[tSa5(d[!Sb5 [!ScUSKUSdUSeS,SfUSgS,ShUSiS,SjUSkS,3 5 [!SlU?SeSmSfU?SgSmShU?SiSmSjU?SkSm35 [!Sn[&SK[&SdW@SeS,SfU@SgS,ShU@SiS,SjU@SkS,3 5 S>[tlc/nA[[(5Hn9U7U9n>U5U9nB[U>UBUW@5nCUCc[!SoU9Sp35 M/SqSW/SqSr/SrSs/St.nDUDSKS WCS s=::a WDSKST::a@O O=WDSuS WCSTs=::a WDSuST::a!O OWDSvS WCSIs=::a WDSvST::dFO [!SwU9SxWC35 [WA5S :a"WAS\R5nC[!SyUC35 OMWARWC5 M [WA5S :Xa[!Sz5 GM|[JRL"WA5nA[!S{[UA5S|35 U(a~URB(dmURO5nEUES[UR|5URzS[UR|5&UES[UR|5UR|SS&[XR~"X5 [R"U 5nURURz5 /nF/nGURzR5nH[[WA55GH3n9WAU9nCWHSSURzSS&UHS[UR|5UR|SS&[XR~"X5 URURz5 [U UUUC[URS}9unInJS~nKUJUK:Ga/[!SU9SWJS+-S,S35 [WF5S :aWFS\R5nIUISSURzSS&[XR~"X5 [XR"U [XRRS5nLURULnM[JRRWCUM- 5nJOmWHR5nI[JRRWCUR[XR"U [XRRS5- 5nJWFRWIR55 WGRWJ5 UIR5nH[!SU9SWCS SmS(UCSTSmS(UCSISmSUJS+-S,S3 5 GM6 [JRL"WF5nF[JRL"WG5nG[!SUGR5S+-S,S35 UR(Gd[bR"U#UU4[bRSV9n!U#R5nNUR(dWFS\SSURzSS&[XR~"X5 USI-USI-/nOU?R5nPUPS ==UOST-ss'UPST==UOS -ss'[U UUUPUOS UOSTS SU9S- nQ[bR"UQUU4[bRSV9nQU!SW-UQSW--nROU!nRSnS[Gb[R"U85R5RST5nT[ [R"[R"UTRrS 555nU[UTUUSIS>S>S9nVUVR5R5R5nWUWRrS ST:Xa[JR"WWSZS S9nW[JR"WWS5nW[bR"UWUU4[bRSV9nSOU8RSS S9nTUTUTR5- UTRS5UTR5- S-- nT[JR"UTSSZSIS9nX[bR"UXUU4[bRSV9nS[[tS5(a[tRGc[R"5 [R"SISISS>S9unYnZUZSen[UZSn\UZSn]UZSgn^U[RWN5n_U[GR//SSSISS9un`[GRGR[JGR"S ST[(55naU[GR //SSKSIS9nbU[GR S5 U[GR S5 U\RWS5ncU\GR S5 U\GR S5 [JGR"[(5ndU]GRUdU5S-SSSISS9uneU]GR[4S-SSSTSS9 U]GR[6S-SSSTSS9 U]GRS5 U]GRSSS9 U]GRSuSS9 U]GRSsS9 U]GR5nfUfGRUdU6SSSISS9ungUfGR[8SSSTSS9 UfGR[:SSSTSS9 UfGRSSS9 UfGRSuSS9 U]GR S5 U^RWR5nhU^GR S5niU^GR S5 UYU[U_U`UbUaU\UcU]UeUfUgU^UhUiS.[tl}OG[tRnjUjSnYUjSn[UjSn_UjSn`UjSnbUjSnaUjSncUjSneUjSngUjSnhUjSniW_GRWN5 W`GRU7SS2S 4U7SS2ST45 WbGRU7SS2SSI245 UbGR!Wa5 WcGRWS5 WeGR#U5S-5 WgGR#U65 WhGRWR5 WiGR%SWGS\S+-SS.35 WYGR&GR)5 UYGR&GR+5 [!U65 [xRx"5U- nk[SS SUk- 5nlUlS :a[xGR,"Wl5 [!URO55 U(dS nGM\![an[!SSU35 SnSnAGM~SnAff=f!,(df  GN=f!G[4a [!S5 Of=fUGR75 UR(d[GR8"S5 [!S5 g!UGR75 UR(d[GR8"S5 [!S5 f=f)Nz;Live test trajectory predictions with IK from camera stream) descriptionz--ask_for_write store_truez$Ask for user input to write to robot)actionhelpz --checkpointzPath to model checkpoint)typedefaultrz--camerarzCamera device ID (default: 0)z--num_ik_iters2zNumber of IK iterationsz--no_armz,No arm connected, use image-based estimationz--exoso100_adhesivez Exoskeleton configuration to use)rrchoicesrz--dont_visualizezXSkip all visualizations (MuJoCo rendering and matplotlib plotting) to speed up inferencez --no_renderzYSkip MuJoCo rendering but keep matplotlib visualizations (faster than full visualization)mpscudacpuzUsing device: z Loading model from z...F) target_sizen_windowfreeze_backbone) map_locationmodel_state_dict min_height max_heightu*✓ Loaded height range from checkpoint: [z.6fz, z] mz (iz.2fzmm to zmm)u2⚠ Checkpoint doesn't have min_height/max_height!z% Using hardcoded values: MIN_HEIGHT=z , MAX_HEIGHT=uQ ⚠ WARNING: MIN_HEIGHT == MAX_HEIGHT! All height predictions will be constant!zR Please recompute from dataset or use a newer checkpoint with height range saved. min_gripper max_gripperu+✓ Loaded gripper range from checkpoint: []zS (Regression: [-0.2, 0.8], values >4.0 mapped to -0.2, values >0.8 clamped to 0.8)u4⚠ Checkpoint doesn't have min_gripper/max_gripper!z/ Using default regression values: MIN_GRIPPER=z, MAX_GRIPPER=u✓ Loaded model from epoch epochTz-robot_models/arm_offsets/rescrew2_fromimg.pklrbu5✓ Connected to robot for direct joint state readingu%⚠️ Calibration file not found at z(, falling back to image-based estimation)gm?g1>[@gb]f?gX ;O?g2BO?g`ܙKE?)slowr8zdone moving to high positionzFailed to open camera device r.zCamera resolution: xz= ============================================================z*Live inference started. Press 'q' to quit.z<============================================================z Failed to read frame from camera)r##) ik_iterationszError detecting link poses: r) segmentationrhg?uA⚠️ Could not detect camera pose or intrinsics, skipping frame fixed_gripperru6⚠️ Starting keypoint behind camera, skipping frame)process_gripper_value)dtype)gt_target_heatmaptrainingstart_keypoint_2dcurrent_heightcurrent_gripper)dimprinted_intrinsicsz Debug - Intrinsics scaling:z Original cam_K (at z): fx=)rrz, fy=)rrz, cx=)rr.z, cy=)rr.z Normalized cam_K_norm: fx=z.4fz Scaled cam_K_model (at u✗ Failed to lift waypoint z to 3Dggg333333?)ryzrru⚠️ Waypoint z 3D target out of workspace: z Using previous waypoint: u9⚠️ Could not lift any waypoints to 3D, skipping frameu!✓ Predicted 3D trajectory with z waypoints)rZg?u ⚠️ t=z: IK error too large (z& mm), using previous timestep's resultr9z t=z: 3D=[z ] m, IK err=z mmu(✓ Sequential IK complete - Avg error: rg)nrowpadding normalize scale_each)axis)rr.rg:0yE>).N vis_state) )figsizeconstrained_layout)rr)rr-limeg333333?)color linewidthalpha()smarker linewidthsz'2D predicted trajectory (all timesteps)offz#Heatmaps (make_grid) resized to RGBrzo-blue)rr markersizegrayz--g?)rr linestylerrTimestepz Height (mm))r)r labelcolor)rzs-redgffffff?greenGripperzHeight + Gripper trajectoryzFinal MuJoCo render overlay)figax_trajim_traj traj_linescatcolorsax_hmim_hmax_lines height_lineax_g gripper_line ax_render im_render title_renderrrrrrrrrrrrz$Final MuJoCo render overlay (IK err z.1fg?z# Live inference interrupted by userallu ✓ Live inference complete!)argparseArgumentParser add_argumentstrCHECKPOINT_PATHintlistr keys parse_argsrqdevicebackendsr is_availablerprint checkpointr IMAGE_SIZErloadload_state_dicttoevalr@rr r r ospathexistsno_armrpickleopenrrget_pos write_posrjr/r rCMjModelfrom_xml_stringxmlMjDatarn VideoCapturecameraisOpened RuntimeErrorreadcvtColor COLOR_BGR2RGBrmmainprev_figtimerFctrlrDrrrrP ExceptionrrorprUpltimshowshowrrkp_localrrtrVrno_grad unsqueezerrrurlrBFsoftmaxrvview_asrnumpyappend unravel_indexargmaxhasattrrr6lenrG ConfigurationrEreinitial_kp_quat_wxyz num_ik_itersrQrRrSrTrr r|dont_visualize no_render tv_make_gridmathceilsqrtdetachrepeat transposeminrionsubplotsplotcmviridislinspacescatter set_titlerarangeaxhline set_xlabel set_ylabel tick_paramsgridtwinxset_data set_offsets set_color set_ydataset_textcanvas draw_idle flush_eventssleep ask_for_writeinput enumerateKeyboardInterruptreleaseclose)qparserargsrr@r model_modulearmuse_robot_state calib_path start_poslast_poscurr_pos robot_configmj_modelmj_datacapretframerwr1widthr# first_writeframe_start_timecamera_pose_world joint_state link_poses corners_cache corners_vis obj_img_ptsrWerendered_img_gtr~r{rgb_vis_resizedfixed_gripper_pose gripper_rot gripper_pos kp_3d_start kp_2d_startscale_xscale_yrrcurrent_gripper_rawrr rgb_batchstart_keypoint_tensorcurrent_height_tensorcurrent_gripper_tensor pred_logits pred_height pred_gripperpred_trajectory_2d pred_heatmapsr4 pred_logits_t pred_probs_tpred_ypred_x pred_2d_t cam_K_norm cam_K_modelpred_trajectory_3d pred_height_t pred_3d_tworkspace_boundscurrent_joint_stateik_trajectory_qpos ik_errors current_qposoptimized_qposik_error max_ik_errorrarb rgb_vis_model render_res render_cam_Krendered_img_final overlay_finalheatmap_grid_vishmrr.grid_nphm3raxsrrrrrrrrrtsrrrrrstelapsed sleep_timetraj_itarg_pos pred_gripdeltasq r*rrsJ  $ $1n oF ),BD S/68  a;= (sB57  <JL c3C#$7$<$<$>?>@ *<vx  lwy    D\\5>>#5#5#B#B#D#D%#(::#:#:#<#<%IF N6( #$ !$//!2# 67 &:bg hEDOO&AJ *%789 HHV E JJL z!lj&@",\": ",\":  ::l;STW:XXZ[efr[stwZxx{|} J|,T1#6fZ =UVZ=Z[^<__bcd BD 5l6M6Mc5RR_`l`w`wx{_|}~  " "l&=&= = e g f h "} 'B#-m#<  #-m#<   ;J}? CO@J ww~~j!!$++&++d:t456 EF 5j\Aijkhh_`I [[]HMM)M'  66"&&*+ ,T 15  ()'(L~~--l.>.>?HmmH%G   4;; 'C <<>>:4;;-HIIJCXXZ Uc ,,uc// 0CIIbqMMFE wax 01 E - 67 &MDMKk0#yy{ JC89,,uc&7&78C!% t{{!kkm 4?? Q',,q/!!(4 `yLa] -um[R],LGZX&$8LZdtv$wM8EGGLLOgll1o%%h8 !(EMYZ+;388:z*R 'J";K+;8I5QK"NO!5(G 6)G +bhh7I.J J )^N++-K"' B"8  334GHO&00366v> (-(8(89J(K(Q(Q(S(V(VW](^%(- ^5==(Y(\(\]c(d%).oU]])[)^)^_e)f&9>&*"&;#8$: :6 [, !"$ M8_ +AqD 1  yy););B)?QGOOP]^bbdjjl $$\2!#!1!1,2E2E2GI[I[!\HHff%5RZZH ")))4 %"$*>)n2L#M*6):):)<#%99>>)gll6K\K\]egmgtgtggB\L]?^3^$_")).*=*=*?@  * .224 QCvil3%7r)A,s9K2iXYl[^M__kltuylyz}k~BCDU4X"$*>@JJ1MBtyy288A;)?@AD'qD]abD"kkm//1779G}}Q'1,"$))GQQ"? ll7I>G'*zz'E6?Z]ZjZj'k$'***2Brvvx-BFFHrvvx,?$,FGB))ByM11=C'*zz#vVYVfVf'g$t[11dnn6LGGI"||Aq(W[\HC!$iGIE"4yH #D I&nn];G!(b"cST\_!`JI VV^^BKK1h,GHF"??2rRPQ?RD%%&OPLL'"LL)9:EOO$IJJJu%8,B#+==[65I4W]ijwx=#yLK$$zF':&TXdemp$q$$zF':&TXdemp$q'' 3'' V'D((cf(EMMM,#>>+D$(IIb,E]^klI$mMLLL;etWX`dLeLL;gYZbfLgOOIUO;$$#%$@&&'DE!* 0 0 ?I#,#6#67T#ULNN5) ##*#*%. $"(!&!&$,'2 $(4%.%.(4&DN$BU)C mG mG ";If:D\FwKE"$]"3K#%n#5L ";I#%n#5L  /""#5ad#;=OPQSTPT=UV  !3ArrE!:;v&/0%%kF&:;&&|4""=1%%(LYWY][_M_`cLddg&hi $$& '') , iik$44GQg .JA~ :& #++- 1"K} > 4QC89$(! b!f 6 456 "" IIe  ./ "" IIe  ./s3CA\ 4A1A[%C6A\ CA[9"y.A\ [ A[6[A[1[*A\ [1A[6[6A\ [9 A\\A\ \ A\$\!A],\#A\$\$A],],AA^2__main__)r)E__doc__sysrpathlibrrqtorch.nn.functionalnn functionalr rnrrmatplotlib.pyplotpyplotrrrrCrGrrscipy.spatial.transformrRrinsertjoindirname__file__rVrrr@rrr r r ExoConfigs.so100_adhesiver ExoConfigsr exo_utilsrrrrrrobot_models.so100_controllerrtorchvision.utilsrrrrrrKEYPOINTS_LOCAL_M_ALLKP_INDEXrcube_to_gripper_offsetrr+r6rerr__name__r,r*rse_     1277<< 94@A277??8,-+^^9*.;  E"79JLb#57I"KLNTU   *"HIxx RS$8>&(R&0BT0n zF]LsFFF