
    mxiT|                        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 r1 S%SSS.S\2S\34S  jjjr4S! r5\6S":X  a  \5" 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        @@   )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/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_spherer4   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?r8   皙?r   )      ?r:   r:   r9   r   maxgeomr      )r0   gffffff?g?axiscamg?g      @iir   r:   gMbP?
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ranger4   r$   r0   __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   r:   )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   daqpg-C6?)solvertol_mr   r   c                   Sn[         R                  " U [         R                  R                  S5      n	[        R
                  " SSSSS9n
U
R                  [        R                  " [        R                  " XC/5      S95        [        U5       H  n[         R                  " X5        UR                  UR                  5         [        R                  " UU
/UU[        R                  " U S9/S	9nUR-                  X5        UR.                  UR                  S
S
& [         R                  " X5        UR"                  U	   n[%        [        R&                  R)                  X=-
  5      5      nU[%        U5      ::  d  M    O   UR"                  U	   nX=-
  n[        R&                  R)                  U5      nUR                  R+                  5       U4$ ! [          ap    [         R                  " X5        UR"                  U	   n[%        [        R&                  R)                  X=-
  5      5      nUR                  R+                  5       U4s s  $ f = f)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?virtual_gripper_keypointbodyr:   r   )position_costorientation_cost)wxyz_xyz)model)limitsN)r&   
mj_name2idmjtObj
mjOBJ_BODYmink	FrameTask
set_targetSE3r   rR   rP   r_   updater^   solve_ikConfigurationLimitr   xposr$   rU   rV   copyintegrate_inplaceq)r   dataconfiguration
target_postarget_kp_quat_wxyznum_iterationsr   r   r   
kp_body_idkp_task	iterationvelcurrent_kp_poserr_now	pos_error
error_norms                    r   ik_to_cube_graspr     s   " 
B ""5&--*B*BD^_J nn"	G txx9L8Y)Z[\ >*	%&TYY'	---	//e<=C 	''0$		! 	%& :.		z'BCDeEl"= +B YYz*N+I	*J99>>Z''1  	- e*!YYz2NBIINN:+FGHG99>>#W,,	-s   ,G

A5II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                  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*                   S035        [-        [.        [0        S1S29n[        R2                  " UR*                  US39nUR5                  US4   5        UR7                  U5      nUR9                  5         SS KnS5U;   aT  S6U;   aN  US5   Ul        US6   Ul        [)        S7US5   S8 S9US6   S8 S:35        [)        S;US5   S<-  S= S>US6   S<-  S= S?35        S@U;   a?  SAU;   a9  US@   Ul         USA   Ul!        [)        SBUS@   S8 S9USA   S8 SC35        [)        SD5        [)        SEUSF    35        O[)        SG5        [)        SH5        [E        URF                  [.        SI9n[)        SJ[I        U5       SK35        XRJ                     n	U	SL   n
[)        SMURJ                   SNU
 35        U	SO   RM                  5       nU	SP   RM                  5       nU	SQ   RM                  5       nUR                  (       aD  URO                  5       nUS S 2SR4   RO                  5       nURO                  5       n[)        SS5        GO)[)        ST5        [        RP                  " US   5      R                  5       R7                  U5      n[        RR                  " USU   [        RT                  SV9R7                  U5      n[        RR                  " US   [        RT                  SV9R7                  U5      n[        RV                  " 5          U	SW   RY                  S5      R7                  U5      nU" US S1UUUSX9u  nnnS S S 5        / n[[        [0        5       H  nWSU4   n[\        R^                  " URa                  SY5      SSZ9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                  SV9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[)        S[5        US   S\:  a  S]OS^nUSY   S\:  a  S]OS^n[)        S_US`   Sa S9USb   Sa ScUS   S<-  S= SdU SeUS   Sf Sg35        [)        ShUSi   Sa S9USj   Sa ScUSY   S<-  S= SdU SeUSY   Sf Sg35        US   S\:  a  S]OS^nUSY   S\:  a  S]OS^n[)        SkUS`   Sa S9USb   Sa ScUSU   S<-  S= SdU SeUS   Sf Sg35        [)        SlUSi   Sa S9USj   Sa ScUSm   S<-  S= SdU SeUSY   Sf Sg35        U	Sn   RM                  5       n U	So   RM                  5       n!U!RO                  5       n"U"S==   [.        -  ss'   U"Sp==   [.        -  ss'   [)        Sq5        / n#[[        [0        5       HQ  nUU   nUU   n$[s        UU$U U"5      n%U%c"  [)        SrU Ss35        UU   RO                  5       n%U#Rq                  U%5        MS     [f        Rn                  " U#5      n#S n&UR                  (       Gd?  SS K	J
n  URu                  StSu9n&U&Rw                  SpSRSpSvSw9n'U'Ry                  US S 2S4   US S 2Sp4   US S 2SR4   SxSySRSzS{9  U'Ry                  U#S S 2S4   U#S S 2Sp4   U#S S 2SR4   S|S}SRS~S{9  U'R{                  US`   USb   USU   SSSS9  U'R{                  USi   USj   USm   SySSS9  U'R{                  U#Si   U#Sj   U#Sm   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,SR-  -
  U+S   U,SR-  -   5        U'R                  U+Sp   U,SR-  -
  U+Sp   U,SR-  -   5        U'R                  U+SR   U,SR-  -
  U+SR   U,SR-  -   5        U#SY   n-USY   n.[)        SU-S   S S9U-Sp   S S9U-SR   S S:35        [)        SU.S   S S9U.Sp   S S9U.SR   S S:35        [)        S[f        R                  R                  U-U.-
  5      S<-  S= S35        [)        S5        [        5       n/[        R                  R                  U/R                  5      n0[        R                  " U05      n1[        RO                  5       n2 UR                  (       d  U	R                  S5      b  U	S   n3[        U3S5      (       a  U3RM                  5       n3[f        R                  " U35      R                  SY5      n3U3R                  [I        U1R                  5      :  a@  U3S [I        U1R                  5       R                  [f        R                  5      n2[)        S5        U2U1R                  S [I        U1R                  5      & U2U1R                  S S & [        R                  " U0U15        [        R                  " U05      n4U4R                  U1R                  5        [)        S5        [)        S[0         SUR                   S35        / n5/ n6U2U1R                  S [I        U1R                  5      & U2U1R                  S S & [        R                  " U0U15        U4R                  U1R                  5        [[        [0        5       H  nU#U   n7[        U0U1U4U7[        UR                  [        UR                  5      [        UR                  5      S-  S9u  n8n9U5Rq                  U8RO                  5       5        U6Rq                  [        U95      5        U8U1R                  S S & [        R                  " U0U15        U4R                  U1R                  5        [)        SU SU7S   S S9U7Sp   S S9U7SR   S SU9S<-  S= S35        M     [f        Rn                  " U55      n5[f        Rn                  " U65      n6[        U6SY   5      n:U5SY   n;UR                  (       a  [)        S5        U2U1R                  S [I        U1R                  5      & U2U1R                  S S & [        R                  " U0U15        [        U0U1U#UU5SU
 3UR                  (       + [        UR                  5      [        UR                  5      S9	  [)        S5        g UR                  (       GdL  [)        S5        SS K	J
n  U	SW   R                  SpSRS5      RM                  5       n<[f        Rn                  " / SQ5      n=[f        Rn                  " / SQ5      n>[f        R                  " U<U>-  U=-   SSp5      n<U<Rl                  S SR u  n?n@SS/nAU!RO                  5       nBUBS==   UASp   -  ss'   UBSp==   UAS   -  ss'   U;S S U1R                  S S& [        R                  " U0U15        [        U0U1U UBUAS   UASp   S1S9S-  nC[        R                  " UCU@U?4[        R                  S9nCU<RO                  5       nDUDS\-  UCS\-  -   nDU&bq  U&Rw                  SpSRSR5      nEUER                  WD5        UER}                  SU:S<-  Sa S3SS9  UER                  S5        U&R                  SU
 3SSS9  U&R                  5         UR                  Sp[0        Sp-   S[0        Sp-   -  S4Su9u  nFnGUR                  R                  [f        R                  " SSp[0        5      5      nH[[        [0        5       GH  nWGU   nIUIR                  U<5        US:  aw  [[        U5       Hh  nJWIRy                  UUJS4   UUJSp-   S4   /UUJSp4   UUJSp-   Sp4   /S|S}SRS\S9  UIRy                  UUJS4   UUJSp-   S4   /UUJSp4   UUJSp-   Sp4   /S|SySRS\SxS9  Mj     WIR{                  UUS4   UUSp4   SySSSSRSSS9	  UIR{                  UUS4   UUSp4   WHU   SSSSSS9  [f        R                  R                  UU   UU   -
  5      nK[        UUSR4   UU   -
  5      S<-  nL[        [        UU   5      [        UU   5      -
  5      nMUIR}                  SU SUKSa SULSa SUU   S= SUU   S= SUMS= Sg3SS9  UIR                  S5        US:X  d  GM  WIR                  SSS9  GM     WG[0           R                  WD5        UG[0           R}                  SU:S<-  Sa S3SS9  UG[0           R                  S5        UR                  SU
 3SSS9  UR                  5         UR                  5         [)        S5        g ! , (       d  f       GN= f! [         a     GNf = 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--ik_solverr   z.Mink QP solver name (e.g. 'daqp', 'quadprog').z--ik_tol_mmg      ?z5Early-stop IK when position error is below this (mm).z--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_2dtrajectory_3d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_gripper)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))      )figsize3d)
projectionz--whitezGT 3D)color	linewidthlabel-limezPred 3D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): [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 initializedz9
Performing sequential IK along predicted 3D trajectory (z steps, z iters/step)...r   )r   r   r   z  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   alpha)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#   appendr   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!   r0   r]   astyper    r   r^   r_   r   Configurationr   num_ik_itersr   initial_kp_quat_wxyz	ik_solver	ik_tol_mmr   no_viewer_animateviewer_marker_size
viewer_fpspermuteclipr   cv2resizeINTER_LINEARimshowr>   suptitletight_layoutsubplotscmviridisrL   r   show)Nparserargspltr   r1  r5  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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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_errsN                                                                                 r   mainr  f  s   $$1VWF
S/6  8
	5EJ+  - (s<W7  9
S!2  4
(sB5  7
=	   D	   ),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F'G,&&q!Q4&@	}QT*M!Q$,?qRStATVZbiuv  F  	G)!Q$/1CAqD1IK]^_ab^bKcehpv  CD  LU  	V--}T/BMRVDW[aegovw-.e0DmTYFZ^eiks{|,U35G5NPbchPimswy  BL  	MObVLW%W%W%!0#.zz-+++35G5K5KQR5K5STzz-+++35G5K5KQR5K5ST

4$;-$;#%%71:14gaj8a<6OP71:14gaj8a<6OP71:14gaj8a<6OP !$Gb!I	'
3'7r'!*S9IGTUJWZK[[^
_`	%il3%7r)A,s9K2iXYl[^M__b
cd	$RYY^^Gi4G%H%Mc$RRU
VW 

)*&(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&	
"# 
FxjPXY]YjYjXkkz
{|'0GLL##gll#$GLLO
h(&8_%a(( ,,t~~&'&0	
 	!!&++-0  u. Q(G,W\\*QCvhqk#.b!S0AHQKPSCTT`afgkaklo`ppstu% ( "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
	
 !I _^  s'   3ANCAN. N
AN+N.
AN<N;AN<__main__)r   )zMuJoCo trajectory viewerTr   r   )r   )7__doc__sysospathlibr   r0  torch.nn.functionalnn
functionalrD  rl  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   r6  r)  r#   KEYPOINTS_LOCAL_M_ALLKP_INDEXkp_localcube_to_gripper_offsetrd  r^  r   r4   r   r   r   r(  r$   r   r  __name__ r   r   <module>r     s|   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J F( F( F( F(Pq"h zF r   