
    9g~7                        d dl Z d dlZd dl mZ d dlZd dlZd dlmZmZ d dlm	Z
 d dlZd dlZd dlZd dlZd dlZd dlmZ d dlmZ d dlZd dlmZ d dlmZmZmZmZmZmZ d d	lmZ d d
l m!Z!m"Z" d dl#Z#d dl$Z$d Z%ddZ&d Z'd Z(d Z)ddZ*d Z+d Z,d Z- G d dej\                        Z/ G d dej\                        Z0 G d dej\                        Z1 G d dej\                        Z2y)    N)nn)	rearrangerepeat)
functional)deepcopy)cm)tqdm)CallableListOptionalTuple	GeneratorDict)defaultdict)natural_cubic_spline_coeffsNaturalCubicSplinec                     t        | d      S )Nz... c x y -> ... (x y) c)r   xs    ./home/cameronsmith/repos/canon_robot/models.py<lambda>r      s    9Q :;     c           	      f    t        | d|"t        | j                  d      dz              S |      S )Nz... (x y) c -> ... c x yg      ?r   )r   intsize)srcr   s     r   r   r      s2    Y-Gijir3sxxXZ|`cOdKez xyz r   c                 d    t        j                  | t        j                  | ddgf         fd      S )N.r   )torchcat	ones_liker   s    r   r   r      s)    		1eooaaSk:;R@ r   c                 .    | dd df   d| ddd f   z   z  S )N.r   gh㈵> r   s    r   r   r      s&    !C"H+#rs(!34 r   c                 @    t        j                  | |dz  dz
  ||      S )N      )modepadding_mode)Fgrid_sampler   ypadr(   s       r   r   r      s    AMM1a!eaidY\$] r   c                     t        | j                        dk(  rt        | |||      S t        | j                  dd      |j                  dd      ||      j	                  d| j                  d d       S )N   r   r'   r&   )lenshape
grid_samp_flatten	unflattenr,   s       r   r   r      s    WZ[\[b[bWcghWhAq#d9S nxyz  zC  zC  DE  GH  zI  KL  KT  KT  UV  XY  KZ  [^  _c  od  on  on  op  rs  ry  ry  z|  {|  r}  o~ r   c                 B    t        t        j                  d||             S )Nb...cij,b...ckj->b...cki)unhomr    einsum)crdsKs     r   r   r      s    %-GD QR r   c           	      d    t        t        j                  d|t        |             dd df   |      S )Nr7   .   )projectr    r9   hom)r:   posesr;   s      r   r   r      s6    gu||4NPUWZ[_W`'abegihigibi'jlmn r   c                 b    | t        j                  t        |             j                  |          S N)r    randpermr1   tor   s    r   r   r      s#    AennSV,//23 r   c                   ,     e Zd Zd fd	Zdi fdZ xZS )CanonRobotTrajPredPCNc           
      F   t         |           || _        d}d}t        j                  ||      | _        t        j                  dd      | _        d}t        j                  d|      | _
        t        j                  t        |      D cg c]  }t        j                  ||       c}      j                         | _        t         j                  j                  t        |      D cg c]  }t        j"                  |||g       c}      | _        t         j                  j                  t        |      D cg c]  }t        j"                  |||g       c}      | _        t        j"                  ||ddg      | _        t        j*                  t        d      D cg c]  }t        j"                  |||g       c} | _        t        j*                  t        d      D cg c]  }t        j"                  |||g       c} | _        y c c}w c c}w c c}w c c}w c c}w )N@      r=   r0         <   )super__init__argsr   	Embeddingglobal_emb_latentmlp_helpersPositionalEncodingNoFreqFactorpos_encLinear	pc_upproj
ModuleListrange
CrossAttn_cudacross_attnsr    make_netpc_net_mlpsglobal_latent_nets	traj_pred
Sequentialpc_net
global_netselfrO   
latent_dimn_timestepsn_layer_transf_	__class__s         r   rN   zCanonRobotTrajPredPC.__init__"   s   	 
!#k:!F ??!D2j1==afguav)w\]+*@*@J*W)wx}} 88..rw  yG  sH  0Imn0D0DjQ[\fEg0h  0I  J"'(("5"5y~  @N  zO  7Ptu{7K7KZXbcmLn7o  7P  #Q$--z*S.NOmmhmnohp%qcdk&:&:JzR\;]&^%qr--lqrslt)ugh+*>*>
:V`?a*b)uv *x 0I 7P &r)us   
H
'H8HH!Hc                     t        | j                  | j                  | j                        D ]%  \  }}} |      } ||      |z    ||      |z   }}' |dt        iz  S )N	pred_traj)zipr]   r^   r[   pred)	rd   model_input
track_idxsoutpc_mlpglobal_latent_mlpattnpc_featglobal_latents	            r   forwardzCanonRobotTrajPredPC.forward8   s|    $ .11A1A$BYBYZ^ZjZj-k 	k)F$T 7M$*7OG$;<Mm<\]j<jMG	k 
 
 	
r   rB   __name__
__module____qualname__rN   rv   __classcell__ri   s   @r   rF   rF   !   s    w, 04 
r   rF   c                   ,     e Zd Zd fd	Zdi fdZ xZS ) CanonRobotTrajPredTransfBaselineNc           	      h   t         |           || _        t        dd      | _        d}d}t        j                  ||      | _        d}t        j                  t        |      D cg c]  }t        j                  ||       c}      | _        t        j                  ||||dg      | _        y c c}w )Nr=   T)in_chuse_first_pool         )rM   rN   rO   	ResnetFPNimg_encr   rP   rQ   rW   rX   rR   rY   r[   r\   traj_decoderc   s         r   rN   z)CanonRobotTrajPredTransfBaseline.__init__W   s    	 !q>
!#k:!F==afguav)w\]+*@*@J*W)wx&//JzR\]^0_` *xs   &B/c                 :   t        | j                  || j                  j                  sdnd   d d df               }| j                  j
                  d    j                  t        |      dd      }| j                  D ]  } |||      } | j                  |      }t        t        t        j                  dd|j                  d            j                         |            }|j!                  t        j                  ddd      j                               }	|d|	iz  S )Ncanon_pc_filtered_imgstart_img_imgr   r   r'   rI   rk   )ch_secr   rO   noncanonrQ   weightexpandr1   r[   r   r   r   r    linspacer   rZ   evaluate)
rd   rn   ro   rp   
img_tokensmotion_tokensrs   pred_traj_lowsplinerk   s
             r   rv   z(CanonRobotTrajPredTransfBaseline.forwardg   s'    DLLTXT]T]TfTf5Ll{)|}~  @A  ~A  *B  C  D
..55d;BB3z?SUVXY $$TDd:m6TmT ((7 $$?qRSUbUgUghiUj@k@p@p@r  vC  %D  EOOENN1a$<$A$A$CD	 
 
 	
r   rB   rw   r|   s   @r   r~   r~   V   s    a  04 
r   r~   c                   ,     e Zd Zd fd	Zdi fdZ xZS ) CanonRobotTrajPredResnetBaselineNc                     t         |           || _        dd lm} |j                  d      }t        j                  t        |j                               d d  | _
        t        g d      | _        y )Nr   T)
pretrainedr   )   r   rK   rL   )rM   rN   rO   torchvision.modelsmodelsresnet18r   r`   listchildrenresnetr\   r_   )rd   rO   r   r   ri   s       r   rN   z)CanonRobotTrajPredResnetBaseline.__init__}   sZ    	 	,D1mmT&//*;%<Sb%AB!"45r   c                     | j                  || j                  j                  sdnd   d d df         j                  d      j                  d      }| j	                  |      j                  dd      }|d|iz  S )Nr   r   r   r   )rI   r=   rk   )r   rO   r   squeezer_   r5   )rd   rn   ro   rp   latentrm   s         r   rv   z(CanonRobotTrajPredResnetBaseline.forward   s    [		HZHZ)@`opqrstqtuv~~  @B  C  K  K  LN  O~~f%//6: 
 
 	
r   rB   rw   r|   s   @r   r   r   |   s    	6 04 
r   r   c                   D     e Zd Z	 	 	 	 	 	 	 	 	 	 d fd	ZddZddZ xZS )r   c                 t   t         |           dd}|| _        || _         ||	      }t	        d|d        t        t        j                  |      ||      | _        |
dk7  rt        j                  |
| j                  j                  j                  j                  d   | j                  j                  j                  | j                  j                  j                  | j                  j                  j                   | j                  j                  j"                        | j                  _        t        j$                         | j                  _        t        j$                         | j                  _        g d|   | _        || _        || _        || _        || _        | j5                  d	t7        j8                  d
d
d
d
      d       | j5                  dt7        j8                  dt6        j:                        d       t        j$                  t        j                  | j*                  dd
            | _        d| _        t        j@                  ddddd| j>                  ffD cg c]  \  }}t        j                  ||d
       c}}      jC                         | _"        t        j@                  dddd| j>                  fD cg c]  }t        j                  ||d
       c}      jC                         | _#        t        j                  |
| j>                  d
      jC                         | _$        y c c}}w c c}w )Nc                 :   | dk(  r(t        j                  t        j                  dd      }|S | dk(  r(t        j                  t        j                  dd      }|S | dk(  r&t        j                  t        j
                  |      }|S | dk(  rd}|S t        d	| z        )
ax  Return a normalization layer
            Parameters:
                norm_type (str) -- the name of the normalization layer: batch | instance | none
            For BatchNorm, we use learnable affine parameters and track running statistics (mean/stddev).
            For InstanceNorm, we do not use learnable affine parameters. We do not track running statistics.
            batchT)affinetrack_running_statsinstanceFgroupnoneNz%normalization layer [%s] is not found)	functoolspartialr   BatchNorm2dInstanceNorm2d	GroupNormNotImplementedError)	norm_typegroup_norm_groups
norm_layers      r   get_norm_layerz*ResnetFPN.__init__.<locals>.get_norm_layer   s     G#&..NN4T
  j(&..%%e
  g%&..r||=NO
  f$!

  *;iG r   zUsing torchvisionencoder)r   r   r=   r   )r)   )r   rH   rK   r   r   i   r   r'   F)
persistentlatent_scalingr&   )dtyper   rH   )r   rK   )rK   rH   )rH   rH   rK   )r       )%rM   rN   feature_scaler   printgetattrtorchvisionr   modelr   Conv2dconv1r   r2   kernel_sizestridepaddingr)   r`   fcavgpoollatent_size
num_layersindex_interpindex_paddingupsample_interpregister_bufferr    emptyfloat32rp   out_dimrW   rZ   combs_1combs_2last_conv_up)rd   backboner   r   r   r   r   r   r   r   r   r   r   d1d2dri   s                   r   rN   zResnetFPN.__init__   s    		2 +,#I.
!8Y7:W[//:j]gh
A:!yy

  ''--a0

  ,,

  ''

  ((!ZZ--:: DJJ 

]]_

7
C$(*.Xu{{1aA'>5Qekk!5==Ae 	 	
 ==IId&&Q/
 }})T\]delnpquq}q}m~I  &A2ryyR';  &A  B  G  G  I}}C2bQUQ]Q]C^%_aryyAq'9%_`eeg))E4<<;@@B &A%_s   $!L/
L5c                 v   t        |j                        dkD  r6 | |j                  dd      |      j                  d|j                  d d       S | j                  dk7  rFt        j                  || j                  | j                  dkD  rdnd| j                  dkD  rdnd d	      }g }| j                  j                  |      }| j                  j                  |      }| j                  j                  |      }| j                  j                  |      }|j                  |       | j                  dkD  rS| j                  r| j                  j                  |      }| j                  j                  |      }|j                  |       | j                  dkD  r,| j                  j!                  |      }|j                  |       | j                  d
kD  r,| j                  j#                  |      }|j                  |       | j                  dkD  r,| j                  j%                  |      }|j                  |       |d   S )Nr0   r   r'   r&         ?bilinearareaTscale_factorr(   align_cornersrecompute_scale_factorr=   r   )r1   r2   r4   r5   r   r*   interpolater   r   maxpoolbn1reluappendr   r   layer1layer2layer3layer4r   r   r   )rd   r   custom_sizelatentsr   	latent_sz	up_latents          r   rv   zResnetFPN.forward   s   qww<!D1a+$F$P$PQRTUT[T[\^]^T_$``$qt/A/A#'#5#5#;Z_c_q_qtw_wW[  ~B  [_aAJJQJJq!JJNN1JJOOAq??Q""JJ&&q)

!!!$ANN1??Q

!!!$ANN1??Q

!!!$ANN1??Q

!!!$ANN1 r{r   c                 :   t        |j                        dkD  r6 | |j                  dd      |      j                  d|j                  d d       S | j                  dk7  rFt        j                  || j                  | j                  dkD  rdnd| j                  dkD  rdnd d	      }| j                  j                  |      }| j                  j                  |      }| j                  j                  |      }|g}| j                  dkD  rS| j                  r| j                  j                  |      }| j                  j                  |      }|j                  |       | j                  dkD  r,| j                  j!                  |      }|j                  |       | j                  d
kD  r,| j                  j#                  |      }|j                  |       | j                  dkD  r,| j                  j%                  |      }|j                  |       | j&                  dk(  rd nd}|d   j                  dd  }t)        t        |            D ]/  }t        j                  ||   ||n|| j*                  |      ||<   1 t-        j.                  |d      | _        | j0                  j                  d   | j2                  d<   | j0                  j                  d   | j2                  d<   | j2                  | j2                  dz
  z  dz  | _        | j5                  | j0                        S )Nr0   r   r'   r&   r   r   r   Tr   r=   znearest r   )r(   r   )dimr   g       @)r1   r2   r4   r5   r   r*   r   r   r   r   r   r   r   r   r   r   r   r   r   r   rX   r   r    r!   r   r   rp   )rd   r   r   r   r   r   is          r   forward_zResnetFPN.forward_  s   qww<!		!Q5??1772A;OO$!//#'#5#5#;Z&*&8&83&>dD'+A JJQJJNN1JJOOA #??Q""JJ&&q)

!!!$ANN1??Q

!!!$ANN1??Q

!!!$ANN1??Q

!!!$ANN1 $ 1 1Z ?TAJ$$RS)	s7|$ 	A
(0	k))+	GAJ	 iiQ/!%!2!22!6A!%!2!22!6A"11T5H5H15LMPSSxx$$r   )
resnet34Tr0   r   borderr   r   Tr   r=   rB   )rx   ry   rz   rN   rv   r   r{   r|   s   @r   r   r      s6     "OCb)n1%r   r   rB   )r   r   )3r    r   r   korniar   einopsr   r   torch.nnr   r*   numpynpsysrandomtimeoscopyr   
matplotlibr   wandbr	   typingr
   r   r   r   r   r   collectionsr   torchcubicspliner   r   geometryrR   r   ch_fstr?   r8   r3   	grid_sampr>   warpshuffleModulerF   r~   r   r   r$   r   r   <module>r     s        $ $        C C # M  
<	z@4]
 	
Rn
33
299 3
j%
ryy %
L
ryy 
,z%		 z%r   