
    Иi                        S r SSKrSSKJr  SSKJs  Jr  SrSrSr	Sr
SrSrS	rS
rS
rSrSrSrSr\S-   r " S S\R*                  5      r\S:X  Ga  \R0                  " \R2                  R4                  R7                  5       (       a  SOS5      r\" S\SS9r\R;                  \5      r\R<                  " SSSS5      R;                  \5      r\R<                  " S\5      R;                  \5      S-  r \RB                  " S5      R;                  \5      r"\RF                  " 5          \" \\ \"S9u  r$r%SSS5        \&" S\$RN                  5        \&" S\%RN                  5        gg! , (       d  f       N6= f)u  ACT joints baseline: image + current robot state -> N_WINDOW future (6D joints + gripper).

Conditioned on current 6 joint angles and gripper state (7) concatenated to CLS before regression.
Direct regression of trajectory_joints (N_WINDOW, 6) and gripper (N_WINDOW,) — no IK.
    Nz;/Users/cameronsmith/Projects/robotics_testing/random/dinov3zt/Users/cameronsmith/Projects/robotics_testing/random/dinov3/weights/dinov3_vits16plus_pretrain_lvd1689m-4057cbaa.pth   )g
ףp=
?gv/?gCl?)gZd;O?gy&1?g?      gi1?gɿg?       c                   X   ^  \ rS rSrSrS\SS4U 4S jjrU 4S jrS r  SS	 jr	S
r
U =r$ )ACTJointsTrajectoryPredictor   zgRegress (N_WINDOW, 6) joint angles + (N_WINDOW,) gripper from image + current joints + current gripper.  Fi   c                 h  > [         TU ]  5         Xl        X l        [        U l        [        S5        [        R                  R                  [        SS[        S9U l        U(       aN  U R                  R                  5        H
  nSUl        M     U R                  R                  5         [        S5        O[        S5        U R                  R                   U l        ["        R$                  " ["        R&                  " U R                   [(        -   U5      ["        R*                  " 5       ["        R,                  " S5      ["        R&                  " XD5      ["        R*                  " 5       ["        R,                  " S5      ["        R&                  " XB[.        -  U-   5      5      U l        [        S	U S
U S35        g )NzLoading DINOv2 model...dinov3_vits16pluslocal)sourceweightsFu   ✓ Frozen DINOv2 backboneu    ✓ DINOv2 backbone is trainable皙?uM   ✓ ACT joints head: [CLS, current_joints(6), current_gripper] -> MLP -> (B, z*6 + ))super__init__target_sizen_windowDINO_PATCH_SIZE
patch_sizeprinttorchhubloadDINO_REPO_DIRDINO_WEIGHTS_PATHdino
parametersrequires_gradeval	embed_dimnn
SequentialLinearCURRENT_STATE_DIMGELUDropoutN_JOINTSmlp)selfr   r   freeze_backbone
hidden_dimparam	__class__s         g/Users/cameronsmith/Projects/robotics_testing/3dkeygrip/volume_dino_tracks_act_baseline_joints/model.pyr   %ACTJointsTrajectoryPredictor.__init__    s5   & )'(IINN%	 # 
	 --/&+# 0IINN./45,,==IIdnn'88*EGGIJJsOIIj-GGIJJsOIIjX"5"@A
 	]^f]gglmulvvwxy    c                    > [         TU ]  U5        [        U S5      (       a   U R                  R                  U5      U l        U $ )Nr   )r   tohasattrr   )r,   devicer0   s     r1   r5   ACTJointsTrajectoryPredictor.toB   s5    
64  		V,DIr3   c                    UR                   S   nU R                  R                  U5      u  nu  pEU R                  R                   HA  nU R                  R                  (       a  U R                  R	                  XES9OS nU" X75      nMC     U R                  R
                  (       a  U R                  R                  US S 2S U R                  R                  S-   24   5      nU R                  R                  US S 2U R                  R                  S-   S 24   5      n	[        R                  " X/SS9nOU R                  R                  U5      nUS S 2S4   n
U
$ )Nr   )HWr   dim)shaper   prepare_tokens_with_masksblocks
rope_embeduntie_cls_and_patch_normscls_normn_storage_tokensnormr   cat)r,   xBx_tokensH_pW_pblkrope_sincos
x_norm_clsx_norm_patches	cls_tokens              r1   _extract_cls)ACTJointsTrajectoryPredictor._extract_clsH   s   GGAJ#yyBB1E*399##C@D		@T@T$))...<Z^K81H $ 99..++HQ8X$)):T:TWX:X8X5X,YZJ!YY^^HQ		8R8RUV8V8X5X,YZNyy*!=1EHyy~~h/HQTN	r3   c
                 l   U R                  U5      n
U
R                  S   nU
R                  nU	c$  [        R                  " U[
        XR                  S9n	Uc  [        R                  " XU
R                  S9nUR                  5       S:X  a  UR                  S5      n[        R                  " XU/SS9nU R                  U5      nUSS2SU R                  [
        -  24   R                  XR                  [
        5      nUSS2U R                  [
        -  S24   R                  XR                  5      nUU4$ )a  
Args:
    x: (B, 3, H, W)
    current_joints: (B, 6) current arm joint angles. If None, zeros.
    current_gripper_state: (B,) or (B, 1) current gripper value. If None, zeros.
    current_3d, start_keypoint_2d, current_height: ignored (API compatibility)

Returns:
    trajectory_joints: (B, N_WINDOW, 6) joint angles per timestep
    gripper: (B, N_WINDOW) gripper value per timestep
r   N)r7   dtyper   r<   )rQ   r>   r7   r   zerosr*   rT   r=   	unsqueezerF   r+   r   view)r,   rG   gt_target_heatmaptrainingstart_keypoint_2dcurrent_heightcurrent_gripper
current_3dcurrent_gripper_statecurrent_jointsclsrH   r7   condouttrajectory_jointsgrippers                    r1   forward$ACTJointsTrajectoryPredictor.forwardW   s    "IIaL!"[[HV99UN ($)KK		$R! $$&!+$9$C$CA$F!yy#/DE1Mhhtn#=T]]X%=#= =>CCA}}V^_a133499!]]K '))r3   )r   r#   r+   r   r   r   )NFNNNNNN)__name__
__module____qualname____firstlineno____doc__N_WINDOWr   r5   rQ   re   __static_attributes____classcell__)r0   s   @r1   r	   r	      s5    q#&5]`  zD CLP* *r3   r	   __main__mpscpur   T)r   r   r-         r   )r_   r^   rc   rd   )(rk   r   torch.nnr$   torch.nn.functional
functionalFr   r   r   IMAGENET_MEANIMAGENET_STDrl   r*   
MIN_HEIGHT
MAX_HEIGHTMIN_GRIPPERMAX_GRIPPERN_HEIGHT_BINSN_GRIPPER_BINSr'   Moduler	   rg   r7   backendsrp   is_availablemodelr5   randnrG   
cur_jointsrandcur_gripno_gradtraj_jgripr   r>    r3   r1   <module>r      sm  
    M K %$

 qL T*299 T*n z\\5>>#5#5#B#B#D#D%%PF(S8]abEHHVEAq#s#&&v.AQ),,V4s:Jzz!}'H	QzQYZ 
	
v||,	)TZZ   
s   "E
E,