
    byi                        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rSSKrSSKrSSKrSSKJr  \R(                  R+                  S\R(                  R-                  \R(                  R/                  \5      S5      5        \R(                  R+                  S\R(                  R/                  \5      5        SSKJrJr  SSKJr  SSKJr  SS	K J!r!J"r"J#r#  S
r$Sr%\RL                  " / SQ/ SQ/ SQ/ SQ/ SQ/5      S-  r'Sr(\'\(   r)\RL                  " / SQ5      r*\RL                  " / SQ5      r+\RL                  " / SQ5      r,S r-SS jr.    S S jr/S r0S r1S r2S!S jr3S r4\5S:X  a  \4" 5         gg)"zATest trajectory predictions with 3D lifting and IK visualization.    N)Path)Rotationz..)RealTrajectoryDatasetN_WINDOW)TrajectoryHeatmapPredictor)SO100AdhesiveConfig)get_link_poses_from_robotposition_exoskeleton_meshesrender_from_camera_posei  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?)r   gQQ?r   r      c                  4     SSK Jn   U $ ! [         a     gf = f)zBImport mujoco.viewer lazily (may be unavailable in some installs).r   N)mujoco.viewerviewer	Exception)r   s    *real_dino_tracks/waypoint_test_model_ik.py_try_import_viewerr   '   s!    & s   
 
c                 T   [         R                  " U[         R                  S9R                  S5      n[         R                  " U[         R                  S9R                  S5      n[         R
                  " [        U5      SS/[         R                  S9n[         R                  " S[         R                  S9R                  S5      n[        R                  " U [        R                  R                  UUUU5         [        R                  R                  U l        g! [         a     gf = f)zOConfigure an mjvGeom as a sphere marker (robust across MuJoCo Python versions).dtyper              	   N)npasarrayfloat64reshapefloat32arrayfloateyemujocomjv_initGeommjtGeommjGEOM_SPHERE	mjtCatBitmjCAT_DECORcategoryr   )geompos_xyzrgbasizepossize3mat9s          r   _set_marker_spherer3   0   s    
**WBJJ
/
7
7
:C::d"**-55a8DHHeDk3,BJJ?E66!2::&..q1D $$((44 s   :D 
D'&D'c	                    [        5       n	U	c  [        S5      eU	R                  XSSS9 n
 U
R                  R	                  U5        [        R                  " / SQ[        R                  S9n[        R                  " / SQ[        R                  S9n[        R                  " U[        R                  S9n[        R                  " U[        R                  S9n[        UR                  S   5      n[        UR                  S   5      nU
R                  n[        [        US	S5      =(       d    [        UR                   5      5      nUU-   nUU:  a  [#        S
US-  5      n[#        S
UU-
  5      n[        R$                  " SUS
-
  U[        S9n[        R$                  " SUS
-
  U[        S9nUU   nUU   n[        UR                  S   5      n[        UR                  S   5      n U
R'                  5       nUb  UR)                  5          SUl        [-        U5       HI  nUR                   UR*                     n[/        UUU   U[1        U5      S9  U=R*                  S
-  sl        MK     [-        U5       HI  nUR                   UR*                     n[/        UUU   U[1        U5      S9  U=R*                  S
-  sl        MK     US:  aS  [1        U5      S-  UR                   S   R2                  S'   [1        U5      S-  UR                   US
-
     R2                  S'   US:  aV  [1        U5      S-  UR                   U   R2                  S'   [1        U5      S-  UR                   UU-   S
-
     R2                  S'   Ub  UR5                  SSS5        UU-   S:  a  [        R6                  " X/SS9OSnUb  UR2                  S:  a  [9        U
S5      (       a  UR;                  SS9n[1        [        R"                  " [        R<                  R?                  UUSSS24   -
  S
S95      5      n UU
R@                  RB                  SS& [#        SUS-  5      U
R@                  l"        SU
R@                  l#        SU
R@                  l$        [        R                  " U[        R                  S9nSn[J        RJ                  " 5       n Sn!S[#        [1        U5      S5      -  n" U
RM                  5          [9        U
S5      (       a   U
RO                  5       (       d  GO [J        RJ                  " 5       n#U#U -
  n$U#n U!U$-  n!U(       a  UR2                  S:  a  U!U":  a"  U!U"-  n!US
-   UR                  S   -  nU!U":  a  M"  UUS[        URP                  5      24   URR                  S[        URP                  5      & URR                  S[        URP                  5       URP                  SS& [T        RV                  " X5         U
R'                  5       nUb  UR)                  5          SUl        [-        U5       HI  nUR                   UR*                     n[/        UUU   U[1        U5      S9  U=R*                  S
-  sl        MK     [-        U5       HI  nUR                   UR*                     n[/        UUU   U[1        U5      S9  U=R*                  S
-  sl        MK     Ub  UR5                  SSS5         U
RM                  5         [J        RX                  " [#        SSU$-
  5      5        GM4  SSS5        g! [
         a     GNf = f! [
         a    Sn GNLf = f! Ub  UR5                  SSS5        f f = f! [
         a     GNf = f! [
         a     GNf = f! [
         a     GNf = f! [
         a    Sn GNf = f! Ub  UR5                  SSS5        f f = f! [
         a     M  f = f! , (       d  f       g= f)zFLaunch MuJoCo viewer and draw predicted/GT 3D trajectories as spheres.NzMuJoCo viewer is not available in this Python environment. Install a mujoco build with viewer support or run without --mujoco_viewer.F)show_left_uishow_right_ui)皙?g?r7   皙?r   )      ?r9   r9   r8   r   maxgeomr      )r/   gffffff?g?axiscamg?g      @iir   r9   MbP?
is_runningg?)-r   RuntimeErrorlaunch_passive_render_window	set_titler   r   r"   r!   r   r   intshapeuser_scngetattrlengeomsmaxlinspacelock	__enter__ngeomranger3   r#   r/   __exit__concatenatehasattrmeanlinalgnormr>   lookatdistanceazimuth	elevationtimesyncr@   ctrlqposr%   
mj_forwardsleep)%mj_modelmj_datapred_trajectory_3dgt_trajectory_3dik_trajectory_qpostitleanimate_robotmarker_sizewaypoint_hz
viewer_modr   	pred_rgbagt_rgbapred_positionsgt_positionsn_predn_gtscn	max_geomsneeded	keep_predkeep_gtpred_idxgt_idxlock_ctxigall_ptscenterspanik_qpossteplastt_accumwaypoint_dtnowdts%                                        r   %_launch_mujoco_viewer_with_trajectoryr   G   s    $%JY
 	

 
	"	"85X]	"	^bh	!!++E2 HH1D	((/rzzB$6bjjIzz"2"**E^))!,-<%%a()ooY2Dc#))nE	$IAyA~.I!Y23G{{1fqj)3GH[[D1HgSAF+H5N'/L--a01F|))!,-D	{{}H  	4CI4[IIcii("1l1owU;EWX		Q	 ! 6]IIcii("1nQ&7{I[\		Q	 # ax',['9C'?		!!!!$.3K.@3.F		$(#((+z*/*<s*B		$$$Q'7<[7IC7O		$-!+,11!4#!!$d3 NRTZ]^_L_"..,!?aHei7<<!#38N8N\\q\)F		wa/Hq QRSD'-

!!!$&)#tcz&:

#%(

"'*

$ **/rzzByy{Ck 2D99	KKM v|,,!,,.. / ))+CtBDrMG!1,{*G 1Ha(88D , 5<DBUCDUBU<U4V0s7<<01"),,/BW\\1B"CQ!!(4 !;;= #""$8	tA		#)),A&q,q/7{I[\IINI % vA		#)),A&q.*;YUS^M_`IINI '
 '%%dD$7 JJs3r 123_ o 
_	^  		:  	H	. #!!$d3 $    		 ! &     '%%dD$7 (
  G 
_	^sD  _\!F_%\25_	E,]5B/_%A]!>A_]2#_6^A!_.B_0^ _B7^'_"_2&_!
\/+_.\//_2]>_]_]_!
]/+_.]//_2
^ <_?^  _
^_^_^$ _#^$$_'_  _
____
_"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   r   r9   )r   rU   invr"   rV   )	point_2dcamera_posecam_Kcam_pos_worldK_inv
point_2d_hray_camcam_rot_world	ray_worlds	            r   unproject_2d_to_rayr      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
r;   ư>Nr   )r   abs)kp_2d_imageheightr   r   cam_posray_directiontpoint_3ds           r   *recover_3d_from_direct_keypoint_and_heightr      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   )rI   copyrP   appendrK   r   r"   )trajectory_3dexpandedry   startendintermediates         r   insert_intermediate_waypointsr     s     =A!!##H3}%)* E" 	

% xxzeAhA/Q% + OOM"%**,-88Hr   c                 d   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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?r9   virtual_gripper_keypointbodyr   )position_costorientation_cost)wxyz_xyzr?   )costdaqp)model)limitsN)rP   r%   r_   updater^   mink	FrameTask
set_targetSE3r   rR   PostureTasksolve_ikConfigurationLimitintegrate_inplaceq
mj_name2idmjtObj
mjOBJ_BODYxposrU   rV   r   )r   dataconfiguration
target_postarget_kp_quat_wxyznum_iterationsr   damping	iterationkp_taskposture_taskvel
kp_body_idcurrent_kp_pos	pos_error
error_norms                   r   ik_to_cube_graspr   A  sW    
BG >*	%&TYY' ..!;VSVilm488R^^=P<]-^_` ''D9		* mmM;FDLcLcjoLpKqr''0$		! 	%&' +. ""5&--*B*BD^_JYYz*N+I	*J99>>Z''r   c                  '   [         R                  " SS9n U R                  S[        [        SS9  U R                  S[        S/ SQ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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S!S9  U R                  S"SS#S9  U R                  5       nUR                  (       d  SS K	J
n  SS$KJn  S nS nS nUR                  (       Gd  [        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        [-        [.        [0        S+S,9n[        R2                  " UR*                  US-9nUR5                  US.   5        UR7                  U5      nUR9                  5         SS KnS/U;   aT  S0U;   aN  US/   Ul        US0   Ul        [)        S1US/   S2 S3US0   S2 S435        [)        S5US/   S6-  S7 S8US0   S6-  S7 S935        S:U;   a?  S;U;   a9  US:   Ul         US;   Ul!        [)        S<US:   S2 S3US;   S2 S=35        [)        S>5        [)        S?US@    35        O[)        SA5        [)        SB5        [E        URF                  [.        SC9n[)        SD[I        U5       SE35        XRJ                     n	U	SF   n
[)        SGURJ                   SHU
 35        U	SI   RM                  5       nU	SJ   RM                  5       nU	SK   RM                  5       nUR                  (       aD  URO                  5       nUS S 2SL4   RO                  5       nURO                  5       n[)        SM5        GO)[)        SN5        [        RP                  " US   5      R                  5       R7                  U5      n[        RR                  " USO   [        RT                  SP9R7                  U5      n[        RR                  " US   [        RT                  SP9R7                  U5      n[        RV                  " 5          U	SQ   RY                  S5      R7                  U5      nU" US S+UUUSR9u  nnnS S S 5        / n[[        [0        5       H  nWSU4   n[\        R^                  " URa                  SS5      SST9Rc                  U5      Re                  5       RM                  5       n[f        Rh                  " URk                  5       URl                  5      u  nn[f        Rn                  " UU/[f        RT                  SP9nURq                  U5        M     [f        Rn                  " U5      nWS   Re                  5       RM                  5       nWS   Re                  5       RM                  5       n[)        SU5        US   SV:  a  SWOSXnUSS   SV:  a  SWOSXn[)        SYUSZ   S[ S3US\   S[ S]US   S6-  S7 S^U S_US   S` Sa35        [)        SbUSc   S[ S3USd   S[ S]USS   S6-  S7 S^U S_USS   S` Sa35        US   SV:  a  SWOSXnUSS   SV:  a  SWOSXn[)        SeUSZ   S[ S3US\   S[ S]USO   S6-  S7 S^U S_US   S` Sa35        [)        SfUSc   S[ S3USd   S[ S]USg   S6-  S7 S^U S_USS   S` Sa35        U	Sh   RM                  5       n U	Si   RM                  5       n!U!RO                  5       n"U"S==   [.        -  ss'   U"Sj==   [.        -  ss'   [)        Sk5        / n#[[        [0        5       HQ  nUU   nUU   n$[s        UU$U U"5      n%U%c"  [)        SlU Sm35        UU   RO                  5       n%U#Rq                  U%5        MS     [f        Rn                  " U#5      n#[)        Sn5        [u        U#5      n&[I        U&5      n'[)        So[I        U#5       SpU' Sq35        S n(UR                  (       Gdj  SS K	J
n  URw                  SrSs9n(U(Ry                  SjSLSjStSu9n)U)R{                  US S 2S4   US S 2Sj4   US S 2SL4   SvSwSLSxSy9  U)R{                  U#S S 2S4   U#S S 2Sj4   U#S S 2SL4   SzS{SLS|Sy9  U)R{                  U&S S 2S4   U&S S 2Sj4   U&S S 2SL4   SzS}S~SSS9  U)R}                  USZ   US\   USO   SSSS9  U)R}                  USc   USd   USg   SwSSS9  U)R}                  U&Sc   U&Sd   U&Sg   S{SSS9  U)R                  SSSS9  U)R                  S5        U)R                  S5        U)R                  S5        U)R                  SSS9  U)R                  SSS9  [f        R                  " UR                  SS9U&R                  SS95      n*[f        R                  " UR                  SS9U&R                  SS95      n+[f        R                  " U+U*-
  S5      n,U*U+-   S-  n-[        U,R                  5       5      n.U)R                  U-S   U.SL-  -
  U-S   U.SL-  -   5        U)R                  U-Sj   U.SL-  -
  U-Sj   U.SL-  -   5        U)R                  U-SL   U.SL-  -
  U-SL   U.SL-  -   5        U&SS   n/USS   n0[)        SU/S   S S3U/Sj   S S3U/SL   S S435        [)        SU0S   S S3U0Sj   S S3U0SL   S S435        [)        S[f        R                  R                  U/U0-
  5      S6-  S7 S35        [)        S5        [        5       n1[        R                  R                  U1R                  5      n2[        R                  " U25      n3[        RO                  5       n4 UR                  (       d  U	R                  S5      b  U	S   n5[        U5S5      (       a  U5RM                  5       n5[f        R                  " U55      R                  SS5      n5U5R                  [I        U3R                  5      :  a@  U5S [I        U3R                  5       R                  [f        R                  5      n4[)        S5        U4U3R                  S [I        U3R                  5      & U4U3R                  S S & [        R                  " U2U35        [        R                  " U25      n6U6R                  U3R                  5        [)        S5        [)        SU' SUR                   S35        / n7/ n8U4U3R                  S [I        U3R                  5      & U4U3R                  S S & [        R                  " U2U35        U6R                  U3R                  5        [[        U'5       H  nU&U   n9[        U2U3U6U9[        UR                  S9u  n:n;U7Rq                  U:RO                  5       5        U8Rq                  [        U;5      5        U:U3R                  S S & [        R                  " U2U35        U6R                  U3R                  5        USL-  Sj:X  a  UU'Sj-
  :  a  SOSn<[)        SU S_U< SU9S   S S3U9Sj   S S3U9SL   S SU;S6-  S7 S35        M     [f        Rn                  " U75      n7[f        Rn                  " U85      n8[        U8SS   5      n=U7SS   n>UR                  (       a  [)        S5        U4U3R                  S [I        U3R                  5      & U4U3R                  S S & [        R                  " U2U35        [        U2U3U&UU7SU
 3UR                  (       + [        UR                  5      [        UR                  5      S9	  [)        S5        g UR                  (       GdL  [)        S5        SS K	J
n  U	SQ   R                  SjSLS5      RM                  5       n?[f        Rn                  " / SQ5      n@[f        Rn                  " / SQ5      nA[f        R                  " U?UA-  U@-   SSj5      n?U?Rl                  S SL u  nBnCSS/nDU!RO                  5       nEUES==   UDSj   -  ss'   UESj==   UDS   -  ss'   U>S S U3R                  S S& [        R                  " U2U35        [        U2U3U UEUDS   UDSj   S+S9S-  nF[        R                  " UFUCUB4[        R                  S9nFU?RO                  5       nGUGSV-  UFSV-  -   nGU(bq  U(Ry                  SjSLSL5      nHUHR                  WG5        UHR                  SU=S6-  S[ S3SS9  UHR                  S5        U(R                  SU
 3SSS9  U(R                  5         UR                  Sj[0        Sj-   S[0        Sj-   -  S4Ss9u  nInJUR                  R                  [f        R                  " SSj[0        5      5      nK[[        [0        5       GH  nWJU   nLULR                  U?5        US:  aw  [[        U5       Hh  nMWLR{                  UUMS4   UUMSj-   S4   /UUMSj4   UUMSj-   Sj4   /SzS{SLSVS9  ULR{                  UUMS4   UUMSj-   S4   /UUMSj4   UUMSj-   Sj4   /SzSwSLSVSvS9  Mj     WLR}                  UUS4   UUSj4   SwSSSSLSSS9	  ULR}                  UUS4   UUSj4   WKU   SSSSSS9  [f        R                  R                  UU   UU   -
  5      nN[        UUSL4   UU   -
  5      S6-  nO[        [        UU   5      [        UU   5      -
  5      nPULR                  SU SUNS[ SUOS[ SUU   S7 SUU   S7 SUPS7 Sa3SS9  ULR                  S5        US:X  d  GM  WLR                  SSS9  GM     WJ[0           R                  WG5        UJ[0           R                  SU=S6-  S[ S3SS9  UJ[0           R                  S5        UR                  SU
 3SSS9  UR                  5         UR                  5         [)        S5        g ! , (       d  f       GN= f! [         a     GN|f = f)Nz#Test trajectory predictions with IK)descriptionz--checkpointzPath to model checkpoint)typedefaulthelpz--splitval_viewpoints)trainr   val_cube_poszDataset split)r   r   choicesr   z--dataset_rootzscratch/parsed_school_capzRoot directory of datasetz--sample_idxr   zSample index to testz--num_ik_iters2   zNumber of IK iterationsz--mujoco_viewer
store_truezRShow 3D trajectory + IK animation in MuJoCo viewer instead of Matplotlib rendering)actionr   z--no_viewer_animatezGWhen using --mujoco_viewer, disable robot animation (show markers only)z--viewer_marker_size~jt?zAMarker sphere radius (meters) for MuJoCo viewer trajectory pointsz--viewer_fps      @zHPlayback rate for IK waypoint animation in MuJoCo viewer (waypoints/sec)z--use_gtzVSkip model inference and use dataset GT 2D+height(+gripper) trajectories for debuggingz--use_fixed_start_poszUAlways initialize MuJoCo from hard-coded start_pos (ignore episode start joint state))Axes3D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)min_grippermax_gripperu+   ✓ Loaded gripper range from checkpoint: []z3  (Binary classification: 0.0 = closed, 1.0 = open)u   ✓ Loaded model from epoch epochz,Using GT trajectories (skipping model load).z
Loading dataset...)dataset_root
image_sizeu   ✓ Loaded z samples
episode_idz
Testing sample z: trajectory_2dr   trajectory_gripperr;   zC
Using dataset GT trajectory for debugging (2D + height + gripper).z
Running model inference...)r   r;   r   rgb)gt_target_heatmaptrainingstart_keypoint_2dcurrent_heightcurrent_gripperr   )dimu*   ✓ Predicted trajectory (first and last):g      ?openclosedz
  Start: [)r   r   z.1f)r   r   z	] px, H: z mm, G: z (z.3f)z
  Final: [)r   r   )r   r   z  GT Start: [z  GT Final: [)r   r;   r   
cam_K_normr   z
Lifting trajectory to 3D...u   ✗ Failed to lift waypoint z< to 3D (point behind camera or ray parallel to height plane)zM
Inserting intermediate waypoints (X,Z from target, height=max(start,end))...u   ✓ Expanded trajectory from z to z
 waypoints)      )figsize3d)
projectionz--whitezGT 3D)color	linewidthlabel-limezPred 3D (original)yellowg      ?gffffff?zPred 3D (with waypoints))r  r  alphar  cyan(   Start)csr  zGT EndzPred Endz3D trajectoryr   bold)fontsize
fontweightzX (m)zY (m)zZ (m)   best)r  loc   i)elevazimr<   r   g       @u%   ✓ Predicted 3D (final, expanded): [z.4fz#  GT 3D (final):                  [z#  3D error (final):                z mmz
Setting up MuJoCo and IK...episode_start_joint_statenumpyu=   ✓ Using episode start joint state for MuJoCo initializationu   ✓ MuJoCo initializedzB
Performing sequential IK along expanded predicted 3D trajectory (z steps, z iters/step)...)r   r   originalz  t=z): 3D=[z] m, IK err=z
Launching MuJoCo viewer...z,Trajectory spheres (Pred=green, GT=white) | )rf   rg   rh   ri   u   
✓ Viewer closed.z
Rendering result...)g
ףp=
?gv/?gCl?)gZd;O?gy&1?g?i  i     )segmentation   )interpolationz'Rendered robot @ last timestep
IK err: mm   )r  offz-3D Lifted Trajectory + Last-Step IK Render |    r   r   )r  r  r  )r  r  r  	linestyled   oblackGT
   )r  r  marker
edgecolors
linewidthsr  zorderxr   Pred)r  r  r(  r*  r  r+  zt=z
Px:zpx | H Err:zmm
G:z (GT:z, Err:zupper right)r  r  zPred last-step IK
IK Err: u.   Trajectory Prediction → 3D Lifting → IK |    u   
✓ Test complete!){argparseArgumentParseradd_argumentstrCHECKPOINT_PATHrE   r#   
parse_argsmujoco_viewermatplotlib.pyplotpyplotmpl_toolkits.mplot3dr   use_gttorchdevicebackendsr   is_availabler   print
checkpointr   
IMAGE_SIZEr   loadload_state_dicttoevalr   
MIN_HEIGHT
MAX_HEIGHTMIN_GRIPPERMAX_GRIPPERr   r   rI   
sample_idxr  r   
from_numpytensorr!   no_grad	unsqueezerP   Fsoftmaxviewview_asr   r   unravel_indexargmaxrF   r"   r   r   r   figureadd_subplotplotscatterrD   
set_xlabel
set_ylabel
set_zlabellegend	view_initminimumminmaximumrK   set_xlimset_ylimset_zlimrU   rV   r   r%   MjModelfrom_xml_stringxmlMjData	start_posuse_fixed_start_posgetrS   r   r    r/   r]   astyper   r   r^   r_   r   Configurationr   num_ik_itersr   initial_kp_quat_wxyzr   no_viewer_animateviewer_marker_size
viewer_fpspermuteclipr   cv2resizeINTER_LINEARimshowr=   suptitletight_layoutsubplotscmviridisrL   r   show)Qparserargspltr   r;  r?  r   model_moduledatasetsampler   r   r   r   pred_trajectory_2dpred_heightpred_gripperr   r   r   r   pred_logitsr   pred_logits_tpred_probs_tpred_ypred_x	pred_2d_tgripper_start_statusgripper_end_statusgt_gripper_start_statusgt_gripper_end_statusr   r   r   rc   pred_height_t	pred_3d_tpred_trajectory_3d_expandedn_waypointsfig_3d	ax_traj3dminsmaxsspanscentersmax_spanpred_3d	target_3drobot_configra   rb   	init_qposjsr   re   ik_trajectory_errtarg_posqpos_terr_twaypoint_typeik_error_lastoptimized_qpos_lastrgb_visrT   stdHW
render_resrender_cam_Krendered_img_predrendered_overlay_pred	ax_renderfigaxescolorsaxt_prev	pixel_err
height_errgripper_errsQ                                                                                    r   mainr  j  s   $$1VWF
S/6  8
	5EJ+  - (s<W7  9
S!2  4
(sB5  7
),p  r
-le  g
.UE_  a
UCf  h

<t  v
/s  uD '/FJE;;;u~~'9'9'F'F'H'He',zz'>'>'@'@VeMvh'( 	%doo%6c:;*zHfklZZfE
j);<= 

$:%,**D&0&>L#&0&>L#>z,?WX[>\\^_ijv_wx{^||  A  BC
<05c:&LAYZ^A^_b@ccfghJ&=J+F'1-'@L$'1-'@L$?
=@YZ]?^^`aklyaz{~`  @A  B  CGI,Z-@,ABC<= 
 "#&&G 
KG~X
./ __%F%J	doo.b
=> ?+113M?+113M 45;;={{*//1#AqD)..0)..0TU 	,-!,,]1-=>DDFII&QmD&9ORRSYZ,,'9!'<EMMRUUV\] ]]_-))!,//7C5:"&"3- /62Kl   xA'1-M99]%7%7%;CKKMZ^^`ffhL--l.A.A.C\EWEWXNFF&&!1DI%%i0 !  XX&89!!n((*002#A**,224	68%1!_%;6#/#3s#:	J)$/4B7I$7OPS6TT]^ijk^lmq^qru]vv~  @T  U  UW  Xd  ef  Xg  hk  Wl  lm  n  o	J)%05R8J58QRU7VV_`kln`opt`tux_y  zB  CU  BV  VX  Ye  fh  Yi  jm  Xn  no  p  q(:1(=(Df(&8&<&CF	M--c2"]45H4MYWdeiWjkoWopsVtt|  ~U  }V  VX  Yk  lm  Yn  or  Xs  st  u  v	M-.s32mE6J35OyYfglYmnrYrsvXww  AV  @W  WY  Zl  mo  Zp  qt  Yu  uv  w  x '--/K%++-J OOE	!H
H	!H
H 

)*8_&q)	#A>y-Ydfkl	03opq%a(--/I!!),  "45 

Z["?@R"S12K	)#.@*A)B${mS]
^_F'G,&&q!Q4&@	}QT*M!Q$,?qRStATVZbiuv  F  	G)!Q$/1CAqD1IK]^_ab^bKcehpv  CD  L`  	a21a48:UVWYZVZ:[]xyz|}y}]~  AD  LT  `c  kn  vP  	Q--}T/BMRVDW[aegovw-.e0DmTYFZ^eiks{|5e<>YZ_>`b}  D  cE  IO  SU  ]g  	hObVLW%W%W%!0#.zz-+++35P5T5TZ[5T5\]zz-+++35P5T5TZ[5T5\]

4$;-$;#%%71:14gaj8a<6OP71:14gaj8a<6OP71:14gaj8a<6OP *"-Gb!I	1'!*S1AGAJsCSSUV]^_V`adUeeh
ij	/	!S/AIaLQTCUUWXabcXdehWiil
mn	/		w?R0STX0XY\/]]`
ab 

)*&(L~~--l.>.>?HmmH%G  I
((vzz:U/V/b34Br7##XXZB''+Bww#gll++1GLL 1299"**E	UV (1GLL##gll#$GLLO
h(&&x0M&	
"# 
OP[}\deievevdw  xG  H  I'0GLL##gll#$GLLO
h(&;.q1( ,,
 	!!&++-0  u. Q(G,W\\*+,q5A:Aa<OV`QCr-C/@8A;sBSSUV^_`VabeUffrsxy}s}  B  sC  CF  G  	H#  & "45!23+B/0M,R0 ,-+4'c',,'(#Q(G,-'@M#555d556doo.
	
 	$%%&'-''1a0668xx-.hh,-'''C-$.15}}Ra 1F#
__&Q:a=(Q:a=( /r2Ra(G,3g{L*Q-TUej
  JJ'81a&PSP`P`a ' 5 ;>ORU>U U **1a3I23"J=Y]K]^aJbbd eprsNN5!OOKJ<XcerxOy! LLHqL1hl;KQ:OLP	T Aq( ;<xAaBIIg 1u#AhFGG/	:<NvVWxYZ{<[\.vqy9;MfUVhXYk;Z[f  F GG]6195}VAXq[7QR)&!)4mF1HaK6PQg#QU  W	 ' JJ}QT*M!Q$,?7c W$WY  [JJ)!Q$/1CAqD1IVTUYZ] QfR  I 		}Q'7:LQ:O'OPI]1a40;q>ABTIJe$6q$9:U<PQ?=SSTKLLQCuYsO;z#6Ff\Z[_]`Laafgyz{g|  ~A  gB  BH  IT  UX  HY  YZ  [   GGENAv		ma	8A !F 	X34X  #>}T?QRU>VVX!Ydf gXE"Ej\R]_lrs
	
 !W _n  s'   .3AN=CAO N=
AOO
AOOAO__main__)r   )zMuJoCo trajectory viewerTr   r   )r   )6__doc__sysospathlibr   r:  torch.nn.functionalnn
functionalrN  rs  r  r   r/  r%   r   r[   scipy.spatial.transformr   Rpathinsertjoindirname__file__r   r   r   r   r   ExoConfigs.so100_adhesiver   	exo_utilsr	   r
   r   r@  r3  r"   KEYPOINTS_LOCAL_M_ALLKP_INDEXkp_localcube_to_gripper_offsetrm  rg  r   r3   r   r   r   r   r   r  __name__ r   r   <module>r     sW   G 
 	     
      1 277<< 94@ A 277??8, - 0 , 9 e e 
E "79JLb#57I"K LNTU  * "HI xx RS HH45	: %Y4x$8> F'(Rl"^ zF r   