o jA;@sdZddlZddlZddlmZddlmmZej ddZ ej ddZ ej ddZ ej d d Z d Zd Zd ZdZdZdZdZdZgdZgdZgdZgdZdZdZdZdZGdddejZe dkre!ej"j#$rtdndZ!ededd Z%e%&e!Z%e'd!d"dd&e!Z(e)d!ed!&e!Z*e+e%e(e,d#d#g&e!e*d$\Z-Z.Z/Z0Wdn1swYe1d%e-j2e1d&e.j2e1d'e/j2e1d(e0j2dSdS))aeModel for trajectory volume prediction using DINOv2. Predicts a pixel-aligned volume: N_WINDOW x N_HEIGHT_BINS logits per pixel (cross-entropy). Gripper is per-pixel (N_WINDOW x N_GRIPPER_BINS per pixel): supervised at GT pixel during training, decoded at predicted pixel during inference (teacher forcing in train, argmax at pred pixel in val/inference). N DINO_REPO_DIRz;/Users/cameronsmith/Projects/robotics_testing/random/dinov3DINO_WEIGHTS_PATHzt/Users/cameronsmith/Projects/robotics_testing/random/dinov3/weights/dinov3_vits16plus_pretrain_lvd1689m-4057cbaa.pthDINO_CONVNEXT_WEIGHTS_PATHzx/Users/cameronsmith/Projects/robotics_testing/random/dinov3/weights/dinov3_convnext_small_pretrain_lvd1689m-296db49d.pth DINO_BACKBONE vits16plus)g ףp= ?gv/?gCl?)gZd;O?gy&1?g?gi1??)n! r r )n! @r r )r r g)r r @ @csleZdZdZdeeddfdedBffdd Zfdd Zd d Z d d Z ddZ ddZ dddZ ZS)TrajectoryHeatmapPredictorzPredicts pixel-aligned volume (N_WINDOW x N_HEIGHT_BINS per pixel) and per-pixel gripper (N_WINDOW x N_GRIPPER_BINS per pixel).FNbackbonec sNt||_||_||_t|_|pt|_|jdkr.t dt j j t ddtd|_d|_n!|jdkrFt dt j j t d dtd|_d |_n td |jd |rk|jD]}d |_qV|jt d|jdn t d|jd|jj|_t d|jtt |jd|_t d|jd|j}ttj||dd dttj||dd dttj||dd dt|_t d|tj||jtd d|_ t d|jdtd|d|d |j|}tj|t!d d|_"tj|dt#d d|_$|jdkrdnd} t d|jdt!d| d t d!|jd"t#d| d dS)#Nrz!Loading DINOv3 ViT-S/16+ model...Zdinov3_vits16pluslocal)sourceweightsconvnext_smallz&Loading DINOv3 ConvNeXt-Small model...Zdinov3_convnext_smallzunknown backbone z, (expected 'vits16plus' or 'convnext_small')Fu ✓ Frozen z backboneu✓ z backbone is trainableu✓ DINO embedding dim: g{Gz?u,✓ Learnable start keypoint embedding (dim=)) kernel_sizepaddingu1✓ Feature convs: 3× Conv2d(3×3) at pred_size=)ru✓ Volume head → (B, z, zfeats||CLS, integer-indexedz!feats only, grid_sample sub-pixelu✓ Gripper head → (B, u) [1×1 conv on ]u✓ Rotation head → (B, z, 3, )%super__init__ target_size pred_sizen_windowDINO_PATCH_SIZEZ patch_sizerrprinttorchhubloadrrdinoZ_head_channel_multr ValueError parameters requires_gradeval embed_dimnn Parameterrandnstart_keypoint_embedding SequentialConv2dGELU feature_convs N_HEIGHT_BINS volume_headN_GRIPPER_BINS gripper_head N_ROT_BINS rotation_head) selfr!r"r#freeze_backbonerparamDZ head_in_dimZ _head_tag __class__$/data/cameron/para/para_mac/model.pyr 4sf      $ "z#TrajectoryHeatmapPredictor.__init__cs(t|t|dr|j||_|S)Nr))rtohasattrr))r=devicerArCrDrEs  zTrajectoryHeatmapPredictor.toc Cs|jd}|j|\}\}}|jjD]}|jjr!|jj||dnd}|||}q|jjr[|j|ddd|jjdf}|j|dd|jjddf} t j || gdd}n|j|}|dddf} |dd|jjddf} | ||||j } | dddd} | | fS)zViT path: extract patch features and CLS token. Returns: patch_features: (B, D, H_p, W_p) stride=patch_size (16) cls_token: (B, D) r)HWNrdimrr)shaper)Zprepare_tokens_with_masksblocksZ rope_embedZuntie_cls_and_patch_normsZcls_normZn_storage_tokensnormr&catreshaper.permute contiguous) r=xBZx_tokensH_pW_pblkZ rope_sincosZ x_norm_clsZx_norm_patches cls_token patch_tokenspatch_featuresrCrCrD_extract_dino_featuress   $$ z1TrajectoryHeatmapPredictor._extract_dino_featuresc Cs|jd}|j|}|d}|d}|jd}tt|d}|||ks-Jd||||||j}|dddd}||fS) u=ConvNeXt path: take the stride-32 final stage feature map. Returns: patch_features: (B, D, H_p, W_p) stride 32 (448 -> 14×14) cls_token: (B, D) synthetic global-pool; ignored downstream (kept for API parity) rZx_norm_clstokenZx_norm_patchtokensrg?z$convnext patch tokens not square: N=rr) rLr)Zforward_featuresintroundrPr.rQrR) r=rSrToutrXrYNsiderZrCrCrD_extract_convnext_featuress   z5TrajectoryHeatmapPredictor._extract_convnext_featuresc Cs||j\}}}}|jd}|dd|d}|dd|d} tj||jd|d||} || dd| |fS)zIndex spatial feature map at specified pixel locations. Args: feats: (B, D, H, W) query_pixels: (B, N, 2) pixel coords [x, y] in feats coordinate space Returns: indexed: (B, N, D) r.rr.rrGN)rLlongclampr&arangerGviewexpand) r=feats query_pixelsrTr@rHrIr_pxpyZ batch_idxrCrCrD_index_featuress   z*TrajectoryHeatmapPredictor._index_featurescCs|j\}}}}|jd}||}||} |d} |d} d| t|ddd} d| t|ddd} tj| | gddd}tj ||dd d }tj | |dd d }| d  dd }| d  dd  ||d t }||fS) uApply gripper/rotation 1×1 conv heads densely, then sample at the query pixels via F.grid_sample (bilinear, sub-pixel). head_input is (B, C, H, W) where C is 2D (ViT path: feats||CLS) or D (ConvNeXt path: feats only). Called with GT pixels during training (teacher forcing) and with predicted pixels (from volume argmax) during inference. No detach: gradients flow back into the shared features. Args: head_input: (B, C, pred_size, pred_size) query_pixels: (B, N_WINDOW, 2) in pred_size coordinate space; can be float for sub-pixel queries. Returns: gripper_logits: (B, N_WINDOW, N_GRIPPER_BINS) rotation_logits: (B, N_WINDOW, 3, N_ROT_BINS) rrbrcr r rJbilinearT)mode align_cornersrr)rLr:r<floatmaxr&stack unsqueezeF grid_samplesqueeze transposerPr;)r= head_inputrkrT_rHrIr_Zgrip_mapZrot_mapqxqynorm_xZnorm_ygridgriprotgripperrotationrCrCrDpredict_at_pixelss     z,TrajectoryHeatmapPredictor.predict_at_pixelscCs|jd}|jdkr||\}}n||\}}|j\}}} } |dkr/|d|d}|dddf| |j d| d} |dddf| |j d| d} t j ||j d} || dd| | f|j d7<tj||j|jfddd }||}||}|||jt|j|j}|jdkr|ddddddfdd|j|j}t j||gdd }n|}|dur|||\}}nd}}||||fS) u; Args: x: (B, 3, H, W) start_keypoint_2d: (B, 2) or (2,) current EEF pixel in image coords query_pixels: (B, N_WINDOW, 2) pixel coords in pred_size space to query for gripper/rotation. Pass GT pixels during training; pass predicted pixels (volume argmax) during inference. If None, gripper/rotation logits are not computed. Returns: volume_logits: (B, N_WINDOW, N_HEIGHT_BINS, pred_size, pred_size) gripper_logits: (B, N_WINDOW, N_GRIPPER_BINS) or None rotation_logits: (B, N_WINDOW, 3, N_ROT_BINS) or None feats: (B, D, pred_size, pred_size) — for downstream predict_at_pixels rrrroNrdrpF)sizerqrrrJ)rLrr[rarKrvrir!rerfr&rgrGr2rw interpolater"r6r8rhr#r7rOr)r=rSstart_keypoint_2drkrTrZrXr|r@rUrVZ start_patch_xZ start_patch_yZ batch_indicesrjvol volume_logitsZ cls_broadcastr{gripper_logitsrotation_logitsrCrCrDforwards6   **$      z"TrajectoryHeatmapPredictor.forward)N)__name__ __module__ __qualname____doc__ PRED_SIZEN_WINDOWstrr rEr[rarnrr __classcell__rCrCrArDr1s M&r__main__mpscpurT)r!r#r>rrgl@)rrkzvolume_logits zgripper_logits rzfeats )3rosr&torch.nnr/torch.nn.functional functionalrwenvirongetrrrrr$Z IMAGENET_MEANZ IMAGENET_STDr MIN_HEIGHT MAX_HEIGHT MIN_GRIPPER MAX_GRIPPERMIN_ROTMAX_ROTMIN_POSMAX_POSr7r9r;rModulerrrGbackendsr is_availablemodelrEr1rSzerosZ fake_queryno_gradtensorrrrrjr%rLrCrCrCrDsX u  (