
    eyi                        S r SSKrSSKrSSKJr  SSKrSSKJs  Jr	  SSK
r
SSKrSSKJr  SSKrSSKrSSKrSSKrSSKrSSKJr  \R0                  R3                  S\R0                  R5                  \R0                  R7                  \5      S5      5        \R0                  R3                  S\R0                  R7                  \5      5        SSKJrJr  SSK J!r!J"r"J#r#J$r$J%r%  SSK&J'r'  SS	K(J)r)  SS
K*J+r+  SSK,J-r-J.r.J/r/J0r0J1r1  SSK2J3r3  Sr4Sr5\Rl                  " / SQ/ SQ/ SQ/ SQ/ SQ/5      S-  r7Sr8\7\8   r9\Rl                  " / SQ5      r:\Rl                  " / SQ5      r;S r<S r=S r>SS jr?\44S jr@S rA\BS:X  a  \A" 5         gg) zYLive test trajectory predictions with 3D lifting and IK visualization from camera stream.    N)Path)Rotationz..)N_WINDOWproject_3d_to_2d)TrajectoryHeatmapPredictor
MIN_HEIGHT
MAX_HEIGHTMIN_GRIPPERMAX_GRIPPER)GridSpec)SO100AdhesiveConfig)EXOSKELETON_CONFIGS)get_link_poses_from_robotposition_exoskeleton_meshesrender_from_camera_posedetect_and_set_link_posesestimate_robot_state)Armi  z1real_dino_tracks/checkpoints/real_tracks/best.pth)     *@{GVg/@)g
ףp=%@gfffffXr   )r   r   g/)g(\1@g=
ףpTr   )g\(6@g=
ףpQr   g     @@   )g2(g݃c؁gkԅPɒ?)g8:!𗑿gMkg(!u?gB6?c                 t   [         R                  R                  U5      SS2S4   n[         R                  R                  U5      n[         R                  " U S   U S   S/5      nXE-  n[         R                  R                  U5      SS2SS24   nXv-  nU[         R                  R	                  U5      -  nX84$ )aB  Unproject 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
Nr   r            ?)nplinalginvarraynorm)	point_2dcamera_posecam_Kcam_pos_worldK_inv
point_2d_hray_camcam_rot_world	ray_worlds	            /real_dino_tracks/waypoint_live_test_model_ik.pyunproject_2d_to_rayr*   1   s     IIMM+.rr1u5M IIMM% E8A;S9:J G IIMM+.rr2A2v6M'IBIINN955I##    c                 ~    [        XU5      u  pE[        US   5      S:  a  gXS   -
  US   -  nUS:  a  gXFU-  -   nU$ )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_imageheightr!   r"   cam_posray_directiontpoint_3ds           r)   *recover_3d_from_direct_keypoint_and_heightr5   M   s`     15QG =t# 
!*	a 00A1u ]**HOr+   c                    [        U 5      S:  a  U R                  5       $ / n[        [        U 5      S-
  5       Hc  nX   nXS-      nUR                  UR                  5       5        UR                  5       n[	        US   US   5      US'   UR                  U5        Me     UR                  U S   R                  5       5        [
        R                  " U5      $ )a  Insert intermediate waypoints between each consecutive pair of trajectory points.

For each pair (start, end), creates an intermediate waypoint with:
- X, Z coordinates from the end point
- Height (Z coordinate) = max(start_height, end_height)

Args:
    trajectory_3d: (N, 3) array of 3D points [x, y, z] where z is height

Returns:
    (2*N-1, 3) array with intermediate waypoints inserted
r-   r   )lencopyrangeappendmaxr   r   )trajectory_3dexpandedistartendintermediates         r)   insert_intermediate_waypointsrC   l   s     =A!!##H3}%)* E" 	

% xxzeAhA/Q% + OOM"%**,-88Hr+   c                 f   SnSn[        U5       GH   n[        R                  " X5        UR                  UR                  5        [
        R                  " SSSSS9n	U	R                  [
        R                  " [        R                  " XC/5      S95        [
        R                  " U SS	9n
U
R                  UR                  5        [
        R                  " X)U
/US
[
        R                  " U S9/S9nUR                  X5        UR                  UR                  SS& [        R                  " X5        GM#     [        R                   " U [        R"                  R$                  S5      nUR&                  U   nX=-
  n[        R(                  R+                  U5      nUR                  R-                  5       U4$ )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?r   virtual_gripper_keypointbodygMbP?)position_costorientation_cost)wxyz_xyzg-C6?)costdaqp)model)limitsN)r:   mujoco
mj_forwardupdateqposmink	FrameTask
set_targetSE3r   concatenatePostureTasksolve_ikConfigurationLimitintegrate_inplaceq
mj_name2idmjtObj
mjOBJ_BODYxposr   r   r9   )rM   dataconfiguration
target_postarget_kp_quat_wxyznum_iterationsdtdamping	iterationkp_taskposture_taskvel
kp_body_idcurrent_kp_pos	pos_error
error_norms                   r)   ik_to_cube_grasprp      s[    
BG >*	%&TYY' ..!;VSVino488R^^=P<]-^_` ''D9		* mmM\+BBX\XoXov{X|W}~''0$		! 	%&' +, ""5&--*B*BD^_JYYz*N+I	*J99>>Z''r+   c                 0   U R                  5       S:  a"  U R                  [        R                  5      S-  n U R                  SS u  p#X!:w  d  X1:w  a%  [
        R                  " XU4[
        R                  S9n [        R                  " U 5      R                  SSS5      R                  5       n[        R                  " / SQ5      R                  S	SS5      n[        R                  " / S
Q5      R                  S	SS5      nXE-
  U-  nU R                  5       nXG4$ )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]
r   g     o@Nr-   interpolationr   r   )g
ףp=
?gv/?gCl?r   )gZd;O?gy&1?g?)r<   astyper   float32shapecv2resizeINTER_LINEARtorch
from_numpypermutefloattensorviewr9   )rgb
image_sizeH_origW_orig
rgb_tensormeanstdrgb_viss           r)   preprocess_imager      s     wwy3jj$u,YYr]NF v3jj:6cFVFVW !!#&..q!Q7==?J<<-.33Aq!<D
,,,
-
2
21a
;C#s*J hhjGr+   c                  ;   [         R                  " SS9n U R                  SSSS9  U R                  S[        [        SS	9  U R                  S
[
        SSS	9  U R                  S[
        SSS	9  U R                  SSSS9  U R                  S[        S[        [        R                  " 5       5      SS9  U R                  SSSS9  U R                  SSSS9  U R                  5       n[        R                  " [        R                  R                  R                  5       (       a  SO&[        R                  R                  5       (       a  SOS5      n[!        SU 35        [!        SUR"                   S35        [%        [&        [(        S S!9n[        R*                  " UR"                  US"9nUR-                  US#   5        UR/                  U5      nUR1                  5         SS KnS$U;   aU  S%U;   aO  US$   Ul        US%   Ul        [!        S&US$   S' S(US%   S' S)35        [!        S*US$   S+-  S, S-US%   S+-  S, S.35        Ob[!        S/5        [!        S0UR4                  S' S1UR6                  S' 35        UR4                  UR6                  :X  a  [!        S25        [!        S35        S4U;   a@  S5U;   a:  US4   Ul        US5   Ul        [!        S6US4   S' S(US5   S' S735        [!        S85        O2[!        S95        [!        S:UR8                  S' S;UR:                  S' 35        [!        S<US=    35        S nS>nS?n[<        R>                  RA                  U5      (       aF  URB                  (       d5  [E        [F        R*                  " [I        US@5      5      5      n[!        SA5        O[!        SBU SC35        S n[J        RL                  " / SDQ5      n	URO                  5       n
URQ                  U	S SE9   URO                  5       n[J        RR                  " [J        RT                  " X-
  5      5      SF:  a  OUn
ME  [!        SG5        [W        5       n[X        RZ                  R]                  UR^                  5      n[X        R`                  " U5      n[b        Rd                  " URf                  5      nURi                  5       (       d  [k        SHURf                   35      eURm                  5       u  nnU(       d  URm                  5       u  nnU(       d  M  [b        Rn                  " U[b        Rp                  5      nURr                  S SI u  nn[!        SJU SKU 35        S n[!        SL5        [!        SM5        [!        SN5        S [t        l;        S>n  [x        Rx                  " 5       nURm                  5       u  nnU(       d  [!        SO5        M=  [b        Rn                  " U[b        Rp                  5      nS nU(       aU  URB                  (       dD  URO                  5       nU=URz                  S S & UR|                  S S & [X        R~                  " X5         [        UXUUSP9u  nnnnnn[        XUU5        U(       dJ  [        XUUSQSR9nUR                  =URz                  S S & UR|                  S S & [X        R~                  " X5         Ub  Uc  [!        SY5        GMG  [        UR                  5       [&        5      u  n"n#[        XU5      SZ   n$U$S S[2S S[24   n%U$S S[2S[4   n&U%[        -  U&-   n'[        U'UU5      n(U(c  [!        S\5        GM  [&        U-  n)[&        U-  n*U([J        RL                  " U)U*/5      -  n+U'SI   n,URO                  5       n[        US]   5      n-SS^KQJRn.  U." U-5      n/[        R                  " 5          U"R                  S5      R/                  U5      n0[        R                  " U+5      R                  5       R/                  U5      n1[        R                  " U,[        R                  S_9R/                  U5      n2[        R                  " U/[        R                  S_9R/                  U5      n3U" U0S S U1U2U3S`9u  n4n5n6S S S 5        / n7/ n8[        [(        5       H  n9W4SU94   n:[        R                  " U:R                  S]5      SSa9R                  U:5      R                  5       R                  5       n;U8R                  U;5        [J        R                  " U;R                  5       U;Rr                  5      u  n<n=[J        RL                  " U=U</[J        R                  S_9n>U7R                  U>5        M     [J        RL                  " U75      n7[J        RL                  " U85      n8W5S   R                  5       R                  5       n5W6S   R                  5       R                  5       n6UR                  5       n?U?S==   U-  ss'   U?ST==   U-  ss'   U?R                  5       n@U@S==   [&        -  ss'   U@ST==   [&        -  ss'   [        [t        Sb5      (       d  [!        Sc5        [!        SdU SKU SeUSf   S, SgUSh   S, SiUSj   S, SkUSl   S, 35        [!        SmU?Sf   Sn SgU?Sh   Sn SiU?Sj   Sn SkU?Sl   Sn 35        [!        So[&         SK[&         SeW@Sf   S, SgU@Sh   S, SiU@Sj   S, SkU@Sl   S, 35        S>[t        lc        / nA[        [(        5       H  n9U7U9   n>U5U9   nB[        U>UBUW@5      nCUCc  [!        SpU9 Sq35        M/  SrSX/SrSs/SsSt/Su.nDUDSK   S   WCS   s=::  a  WDSK   ST   ::  a@  O  O=WDSv   S   WCST   s=::  a  WDSv   ST   ::  a!  O  OWDSw   S   WCSI   s=::  a  WDSw   ST   ::  dF  O  [!        SxU9 SyWC 35        [        WA5      S:  a"  WAS]   R                  5       nC[!        SzUC 35        OM  WAR                  WC5        M     [        WA5      S:X  a  [!        S{5        GM|  [J        RL                  " WA5      nA[!        S|5        [        UA5      nE[        UE5      nF[!        S}[        UA5       S~UF S35        / nG[        UA5      nH[        UF5       H  nIUISI-  S:X  a@  WISI-  nJUJ[        U65      :  a  WGR                  U6WJ   5        M6  WGR                  U6S]   5        ML  WIST-
  SI-  nJUJ[        U65      :  a  WGR                  U6WJ   5        My  WGR                  U6S   5        M     [J        RL                  " WG5      nG[!        S[        U65       S~[        UG5       S35        U(       a~  URB                  (       dm  URO                  5       nKUKS [        UR|                  5       URz                  S [        UR|                  5      & UKS [        UR|                  5       UR|                  S S & [X        R~                  " X5        [        R                  " U5      nUR                  URz                  5        / nL/ nMURz                  R                  5       nN[        WF5       GHL  n9WEU9   nCWNS S  URz                  S S & UNS [        UR|                  5       UR|                  S S & [X        R~                  " X5        UR                  URz                  5        [        UUUUC[        UR                  S9u  nOnPSnQUPUQ:  Ga/  [!        SU9 SWPS+-  S, S35        [        WL5      S:  a  WLS]   R                  5       nOUOS S  URz                  S S & [X        R~                  " X5        [X        R                  " U[X        R                  R                  S5      nRUR                  UR   nS[J        R                  R                  WCUS-
  5      nPOmWNR                  5       nO[J        R                  R                  WCUR                  [X        R                  " U[X        R                  R                  S5         -
  5      nPWLR                  WOR                  5       5        WMR                  WP5        UOR                  5       nNU9SI-  ST:X  a  U9WFST-
  :  a  SOSnT[!        SU9 SUT SWCS   Sn S(UCST   Sn S(UCSI   Sn SWPS+-  S, S35        GMO     [J        RL                  " WL5      nL[J        RL                  " WM5      nM[!        SUMR                  5       S+-  S, S35        UR                  (       G
d  [b        R                  " U#UU4[b        R                  SW9n!UR                  (       d  WLS]   S S URz                  S S& [X        R~                  " X5        USI-  USI-  /nUU?R                  5       nVUVS==   UUST   -  ss'   UVST==   UUS   -  ss'   [        XUUVUUS   UUST   S SU9SV-  nW[b        R                  " UWUU4[b        R                  SW9nWU!SX-  UWSX-  -   nXOU!nX[        R                  " S[(        ST-   -  S4S9nY[        S[(        ST-   UY/ SQSSS9nZU#R                  5       n[[        R                  R                  [J        R                  " SST[(        5      5      n\[        [(        5       GH#  n9WYR                  WZSU94   5      n]U]R                  W[5        U9S:  aD  [        U95       H5  n^W]R                  U7U^S4   U7U^ST-   S4   /U7U^ST4   U7U^ST-   ST4   /SSSISXS9  M7     W]R                  U7U9S4   U7U9ST4   W\U9   SSKS[SSS9  U]R                  U7U9S4   U\U9   SSTSXS9  U]R                  U7U9ST4   U\U9   SSTSXS9  U5U9   S+-  n_U6U9   n`U]GR                  SU9 SU_S, SU`S, 3SS9  U]GR                  S5        U9S:X  d  GM  W]GR                  SSS9  GM&     WYR                  WZS[(        4   5      naUaR                  WX5        UaGR                  S[        WL5      ST-
   SWMS]   S+-  S S3SS9  UaGR                  S5        [        [(        5       HW  n9WYR                  WZSTU94   5      n]U]R                  U8U9   S-  SSSS9  U]GR                  SU9 3SS9  U]GR                  S5        MY     WYR                  WZST[(        4   5      nbUbGR                  S5        [        [(        5       H  n9WYR                  WZSIU94   5      n][4        S+-  U5U9   S+-  [6        S+-  /ncSW\U9   S/nd/ SQneU]GR                  UeUcUdSS9nfUfST   GR	                  S5        U]GR                  Ue5        U]GR                  SSU9 3S/SSSS9  U]GR                  SSS9  U]GR                  SU5U9   S+-  S, S3SS9  U]GR                  StSvS9  U]GR                  [4        S+-  S-
  [6        S+-  S-   /5        M     WYR                  WZSI[(        4   5      ng[J        GR                  " [(        5      nhUgR                  UhU5S+-  SSSISSS9  UgR                  [4        S+-  SSSTSXSS9  UgR                  [6        S+-  SSSTSXSS9  UgGR                  SSS9  UgGR                  SSS9  UgGR                  SSS9  UgGR                  StS9  UgGR                  SS9  UgGR                  5       niUiR                  UhU6SSSISSS9  UiR                  [8        SSSTSXSS9  UiR                  [:        SSSTSXSS9  UiGR                  SSSS9  UiGR                  SvSS9  UiGR                  [8        S-
  [:        S-   /5        UiGR                  SSS9  [        [(        5       H  n9WYR                  WZS[U94   5      n]U6U9   n`SST/ne[8        U`/njSW\U9   /ndU]GR                  UeUjUdSS9nfUfST   GR	                  S5        U]GR                  Ue5        U]GR                  SU`S, /SSSS9  U]GR                  SSS9  U]GR                  SU`S, 3SS9  U]GR                  StSvS9  U]GR                  [8        S-
  [:        S-   /5        M     WYR                  WZS[[(        4   5      ni[J        GR                  " [(        5      nhUiR                  UhU6SSSISS9  UiR                  [8        SSSTSXSS9  UiR                  [:        SSSTSXSS9  UiGR                  SSS9  UiGR                  SSS9  UiGR                  SSS9  UiGR                  StS9  UiGR                  SS9  UiGR                  [8        S-
  [:        S-   /5        G[        [        WL5      [(        5      nk[        Uk5       GH  n9WYR                  WZSU94   5      n]UR                  (       d  WLU9   S S URz                  S S& [X        R~                  " X5        USI-  USI-  /nUU?R                  5       nVUVS==   UUST   -  ss'   UVST==   UUS   -  ss'   [        XUUVUUS   UUST   S SU9SV-  nl[b        R                  " UlUU4[b        R                  SW9nlU!SX-  UlSX-  -   nmOU!nmW]R                  Wm5        U]GR                  SU9 SWMU9   S+-  S S3SS9  U]GR                  S5        GM      WYR                  WZS[(        4   5      nn[J        GR                  " [        WM5      5      nhUnR                  UhUMS+-  SSSISS9  UnGR                  SSS9  UnGR                  SSS9  UnGR                  SSS9  UnGR                  StS9  UnGR                  S[S        UMRS                  5       S+-  S-  S5      /5        [        GR                  " SSSS9  [        GR                   " SSSSSSS9  [        GR"                  " 5         [        GR$                  " SF5        [        [t        S5      (       a%  [        GR&                  " [t        Rv                  5        WY[t        l;        [        GR(                  " SFS9(       a  GO\[!        U65        [x        Rx                  " 5       U-
  no[S        SSUo-
  5      npUpS:  a  [x        GR*                  " Wp5        [!        URO                  5       5        U(       d  UGR,                  (       a  G[/        S5      Sv:X  a  G[1        WLS S 5       H  u  nqnr[!        Ur5        Uq[        WG5      :  a  WGWq   nsO[        WG5      S:  a  WGS]   OU6S]   nsWsWrS]'   URQ                  UrS SE9   URO                  5       n[J        RR                  " [J        RT                  " X-
  5      5      nt[!        SUt5        UtS:  a  OUn
MS  [!        SG5        M     S nGM  UGR5                  5         UR                  (       d  [        GR&                  " S5        [!        S5        g ! [         a  n[!        SSU 35        S n S nAGM  S nAff = f! , (       d  f       GN= f! G[2         a    [!        S5         Nf = f! UGR5                  5         UR                  (       d  [        GR&                  " 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)actionhelpz--checkpointzPath to model checkpoint)typedefaultr   z--camerar   zCamera device ID (default: 0)z--num_ik_iters2   zNumber of IK iterationsz--no_armz,No arm connected, use image-based estimationz--exoso100_adhesivez Exoskeleton configuration to use)r   r   choicesr   z--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)mpscudacpuzUsing device: z
Loading model from z...F)target_sizen_windowfreeze_backbone)map_locationmodel_state_dict
min_height
max_heightu*   ✓ Loaded height range from checkpoint: [z.6fz, z] mz  (i  z.2fzmm to zmm)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.pklrbu5   ✓ Connected to robot for direct joint state readingu%   ⚠️ Calibration file not found at z(, falling back to image-based estimation)gm?g1>[@gb]f?gX ;O?g2BO?gc! YV?)slowrE   zdone moving to high positionzFailed to open camera device r-   zCamera resolution: xz=
============================================================z*Live inference started. Press 'q' to quit.z<============================================================z Failed to read frame from camera)r"   #   )ik_iterationszError detecting link poses: r   )segmentation   rr   g      ?uA   ⚠️ Could not detect camera pose or intrinsics, skipping framefixed_gripperr   u6   ⚠️ Starting keypoint behind camera, skipping framer7   )process_gripper_value)dtype)gt_target_heatmaptrainingstart_keypoint_2dcurrent_heightcurrent_gripper)dimprinted_intrinsicsz
Debug - Intrinsics scaling:z  Original cam_K (at z): fx=)r   r   z, fy=)r   r   z, cx=)r   r-   z, cy=)r   r-   z  Normalized cam_K_norm: fx=z.4fz  Scaled cam_K_model (at u   ✗ Failed to lift waypoint z to 3Dg      g        g333333?)r   yzr   r   u   ⚠️ Waypoint z 3D target out of workspace: z  Using previous waypoint: u9   ⚠️ Could not lift any waypoints to 3D, skipping framezM
Inserting intermediate waypoints (X,Z from target, height=max(start,end))...u   ✓ Expanded trajectory from z to z
 waypointsu    ✓ Expanded gripper array from z values)re   g?u     ⚠️ t=z: IK error too large (z& mm), using previous timestep's resultrF   rB   originalz  t=z (z): 3D=[z] m, IK err=z mmu(   ✓ Sequential IK complete - Avg error:       )figsize   )r   r         ?r   r   g?)figureheight_ratioshspacewspace-lime)color	linewidthalphad   Pred
   )csmarker
linewidthslabelzorderz--)r   r   	linestyler   r   )r   r   r   r   r   zt=z
H: zmm | G:    )fontsizeoffzupper right)locr   zFinal IK (t=z)
Err: z.1fmmg      Y@g?hotbilinear)r   cmaprs   z
Heatmap t=gray)r   r   r-   gffffff?)r   r   r   MinMax-   right)r   rotationhazHeight (mm)   zH: )r   axiszo-blue)r   r   
markersizer   zMin H)r   r   r   r   r   r   zMax HTimestepzHeight Trajectory)r   zs-redGripperzMin GgreenzMax G)r   r   )r   
labelcolorzG: )r   r   r   zGripper Trajectory   zIK t=z
Err: zIK Error (mm)zIK Error Trajectoryg333333?u;   Live Trajectory Prediction → 3D Lifting → Sequential IK   bold)r   
fontweightg{Gz?g\(\?gQ?)leftr   topbottomr   r   prev_fig)timeoutzWrite to robot? (y/n): zstill movinggQ?z#
Live inference interrupted by userallu   
✓ Live inference complete!)argparseArgumentParseradd_argumentstrCHECKPOINT_PATHintlistr   keys
parse_argsrz   devicebackendsr   is_availabler   print
checkpointr   
IMAGE_SIZEr   loadload_state_dicttoevalrM   r   r	   r
   r   ospathexistsno_armr   pickleopenr   r   get_pos	write_posr<   r.   r   rO   MjModelfrom_xml_stringxmlMjDatarw   VideoCapturecameraisOpenedRuntimeErrorreadcvtColorCOLOR_BGR2RGBrv   mainr   timerR   ctrlrP   r   r   r   r\   	Exceptionr   rx   ry   r9   pltimshowshowr   r   kp_localr   r}   ra   r   no_grad	unsqueezer{   r~   ru   r:   Fsoftmaxr   view_asr   numpyr;   unravel_indexargmaxhasattrr   r5   r8   rC   rS   ConfigurationrQ   rp   initial_kp_quat_wxyznum_ik_itersr]   r^   r_   r`   r   r   r   dont_visualize	no_renderr   r   cmviridislinspaceadd_subplotplotscatteraxvlineaxhline	set_titler   legendbar	set_alpha
set_xticksset_xticklabels
set_ylabelgridset_ylimarange
set_xlabeltwinxtick_paramsminsuptitlesubplots_adjustdrawpauseclosewaitforbuttonpresssleepask_for_writeinput	enumerateKeyboardInterruptrelease)uparserargsr   rM   r   model_modulearmuse_robot_state
calib_path	start_poslast_poscurr_posrobot_configmj_modelmj_datacapretframer   r0   widthr"   first_writeframe_start_timecamera_pose_worldjoint_state
link_posescorners_cachecorners_visobj_img_ptsrb   erendered_img_gtr   r   rgb_vis_resizedfixed_gripper_posegripper_rotgripper_poskp_3d_startkp_2d_startscale_xscale_yr   r   current_gripper_rawr   r   	rgb_batchstart_keypoint_tensorcurrent_height_tensorcurrent_gripper_tensorpred_logitspred_heightpred_gripperpred_trajectory_2dpred_heatmapsr3   pred_logits_tpred_probs_tpred_ypred_x	pred_2d_t
cam_K_normcam_K_modelpred_trajectory_3dpred_height_t	pred_3d_tworkspace_boundspred_trajectory_3d_expandedn_waypointspred_gripper_expanded
n_originalr?   orig_idxcurrent_joint_stateik_trajectory_qpos	ik_errorscurrent_qposoptimized_qposik_errormax_ik_errorrl   rm   waypoint_type
render_resrender_cam_Krendered_img_finaloverlay_finalfiggsrgb_vis_trajcolorsaxt_prev
height_valgripper_valax_ikax_heatmap_ikheights_to_show
colors_barx_posbarsax_height_traj	timestepsax_gripper_trajgripper_valsn_ik_rendersrendered_img_t	overlay_t
ax_ik_trajelapsed
sleep_timetraj_itarg_pos	pred_gripdeltasu                                                                                                                        r)   r  r     ss   $$1noF
),B  D
S/6  8

a;  =
(sB5  7

<J  L
c3C#$7$<$<$>?>  @ *<v  x
lw  yD \\5>>#5#5#B#B#D#D%#(::#:#:#<#<%IF	N6(
#$ 
!$//!2#
67&:bghEDOO&AJ	*%789HHVE	JJL z!lj&@",\":",\":::l;STW:XXZ[efr[stwZxx{|}J|,T1#6fZ=UVZ=Z[^<__bcd 	BD5l6M6Mc5RR_`l`w`wx{_|}~""l&=&==egfh
"}
'B#-m#< #-m#< ;J}<UVY;ZZ\]ghu]vwz\{{|}~ceDF?@X@XY\?]]klx  mE  mE  FI  lJ  K  	L	(G)<(=
>? CO@J	ww~~j!!$++&++d:t456EF5j\Aijkhh_`I[[]HMM)M'
66"&&*+,T15  

() '(L~~--l.>.>?HmmH%G 

4;;
'C<<>>:4;;-HII JCXXZ
U c
,,uc//
0CIIbqMMFE	wax
01E	-	
67	&M DMKG0#yy{ JC89,,uc&7&78C !%t{{!kkm4??Q',,q/!!(4
`yLa]
-um[R] ,LGZX&$8LZdtv$wM8EGGLLOgll1o%%h8  !(EMYZ +;388:z*R'J ";<SZ![\k!l,RaR!V4K,RaRU3K%0;>K +;8I5QK"NO !5(G 6)G +bhh7I.J J )^N ++-K"'B"8 334GHO &00366v>	(-(8(89J(K(Q(Q(S(V(VW](^%(-^5==(Y(\(\]c(d%).oU]])[)^)^_e)f&9>&*"&;#8$::6[, ! "$M8_ +AqD 1 yy););B)?QGOOP]^bbdjjl$$\2!#!1!1,2E2E2GI[I[!\HHff%5RZZH	")))4 % "$*<!=HH]3M%a.,,.446K'?..0668L JqMU"MqMV#M %//+KNj(NNj(N 4!56657-eWAfXVE#JsCSSXY^_bYcdgXhhmnstwnxy|m}  ~C  DI  JM  DN  OR  CS  T  U4Z_S4IzZ]_bNcchistwixy|h}  ~C  DN  OR  DS  TW  CX  Y  Z1*Qzl&Q\]`QabePffklwx{l|  ~A  lB  BG  HS  TW  HX  Y\  G]  ]b  cn  or  cs  tw  bx  y  z*.' "$8_.q1	 +AF}.?	 $86BC
 s$  )-a0IaL\DTUXDYZ[D\\(-a0IaL\DTUXDYZ[D\\(-a0IaL\DTUXDYZ[D\\,QC/LYKXY-.2$6r$:$?$?$A	 ;I;GH ")))4= %@ %&!+QR!#*<!= bc*GHZ*['9:K1#6H2I1J${m[efg
 %'!/0J;'q5A: AvH#l"33-44\(5KL .44\"5EF !"A!|H#l"33-44\(5KL .44\!_E! (" %'HH-B$C!4S5F4GtCPeLfKggnop t{{&)kkm#3FGYGLLHY3Z/c',,/0"56Hs7<<7H"IQ h0 !..x8M  . "$I"<<,,.L;'7:	 #/q/Q"./AGLL0A"BQ!!(4$$W\\2 ,<!(#'#4#4,(  #l*Ks*@$s@SSyz{-.2);B)?)D)D)F*8*;Q))(<%+%6%6xAYAY[u%v
)0j)A#%99>>)n2L#M *6):):)<#%99>>)gll6K\K\]egmgtgtgg  B\  L]  ?^  3^  $_")).*=*=*?@  *  .22434q5A:AVWDW^hQCr-	!S7IIVWLY\K]]_`ijk`lmp_qq}  G  HL  L  MP  ~Q  QT  U  VW (Z "$*<!=+I<Y^^=Md=RSV<WWZ[\ &&&**_ufoUXUeUef ~~'9"'=bq'AGLL!$%%h8"(A+uz!:J#-??#4L Oz!}4O Oz!}4O)@ +<l"1z!}5* *& *-4FPVgjgwgw)x&$+cM4F4L$LM %,M jj!X\*:B)?@aAcI\ehqtu  /335 Aq((CDxAAqD2BIIl+ 1u&+AhFGG%7	%BDVW]^_W_abWbDc$d$6vqy$ACUV\]^V^`aVaCb#c#&f $ N '/ JJ1!Q$79KAqD9QU[\]U^be%(QfR  Q JJ!3AqD!9VZfgorJsJJ!3AqD!9VZfgorJs!,Q$!6J".q/KLL2aSj-=XkRUEV!WbcLdGGENAv		ma	@/ )4 1h;8]+,s3E/Fq/H.IR[\^R_`dRdehQiik lwyz

5! xAAqD2B IImA.s2#EYcId LL:aS!1AL>GGEN ) !$1h; @""5) xAAqD2B'1D'8+a.4:OQ[^bQb&cO"(&)V!<J%E66%
RU6VDG%%c*MM%(&&A3x'?!VX]d&eMM-!M<LL3{1~d':3&?r!BQLOGG#CG0KKd!2Q!6
T8IA8M NO )  "%AxK!AIIh/	##I{T/A4vabop  yF#  G&&d):&TXdempx&  A&&d):&TXdempx&  A))*q)A))-!)D(()<q(I####.%%q%1 #1"6"6"8$$Yd%[\ijr{$|''+Ud^_gjry'z''+WPT`ailt{'|**9q*N+++G((+*;[3=N)OP&&=1&E xAAqD2B".q/KFE$/#=L"(&)!4J66%Zs6SDG%%c*MM%(&&+c1B'Dq[]bi&jMM)aM8LL3{3&7!81LEGG#CG0KKs!2K#4E FG )" #&//"Q[/"BIIh/	$$Yd%[\ij$k''+Ud^_gjry'z''+WPT`ailt{'|**:*B**9q*A))*>)K$$3$/&&&2((+*;[3=N)OP  #3'9#:HE|,AAqD2B>>+=a+@!+DRa())(< '-k5A:%>
'1'8$Q:a=8$Q:a=8)@$/@,&qM:a=u*  *  *-NUFOcfcscs)t %,cMNS4H$H	 %,	IIi(LL579Q<3DS2I!LWXLYGGEN7 -< !__R8_=
IIc)n5		9t+;TZ[hij%%j1%=%%o%B$$%:Q$Gc*##QIMMOd,BS,H"(M$NOZegtz{##TtDY\ehi
		$ 4,,IIdmm, # ))$7 , iik$44GQg.JA~

:&#++- %%%%0I*JS*P+45G5L+Mxh!C(=$>>(=f(EI FII^E_bcEc(=b(AiuvxiyI &/hE:"%([[]H"$&&0A)B"CE!.7$Tz5%-H # <=% ,N& Ku F 	""IIe./O  4QC89$(!b !^  6456 	""IIe./s   3C Au5 4A1At: %C6Au5 CAu#"k*Au5 MaAu5 nAv nEAu5 t:
Au uAuuAu5 uAu u Au5 u#
Au2u-Au5 u5AvvAv vAvvAv vAAw__main__)r   )C__doc__sysr  pathlibr   rz   torch.nn.functionalnn
functionalr  rw   r"  r   matplotlib.pyplotpyplotr  r   rO   rS   r  r  scipy.spatial.transformr   Rr  insertjoindirname__file__ra   r   r   rM   r   r   r	   r
   r   matplotlib.gridspecr   ExoConfigs.so100_adhesiver   
ExoConfigsr   	exo_utilsr   r   r   r   r   robot_models.so100_controllerr   r   r   r   KEYPOINTS_LOCAL_M_ALLKP_INDEXr  cube_to_gripper_offsetr'  r*   r5   rC   rp   r   r  __name__ r+   r)   <module>r     sL   _ 
 	     
        1 277<< 94@ A 277??8, - + ^ ^ ( 9 *  . 
E "79JLb#57I"K LNTU  * "HI xx RS $8> F&(R &0 Bp	0f zF r+   