o Ii=)@sdZddlZddlZddlmZddlmmZej ddZ ej ddZ dZ dZ d Zd Zd Zd Zd Zd ZgdZgdZgdZgdZdZdZdZdZGdddejZedkreej j!"rfdndZededdZ#e#$eZ#e%dddd$eZ&e'ded$eZ(e)e#e&e*ddg$ee(d\Z+Z,Z-Z.Wdn1swYe/d e+j0e/d!e,j0e/d"e-j0e/d#e.j0dSdS)$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.pth)g ףp= ?gv/?gCl?)gZd;O?gy&1?g?gi1??)n! rr)n! @r r )rrg)rrg@ @csXeZdZdZdeedffdd ZfddZdd Zd d Z d d Z dddZ Z S)TrajectoryHeatmapPredictorzPredicts pixel-aligned volume (N_WINDOW x N_HEIGHT_BINS per pixel) and per-pixel gripper (N_WINDOW x N_GRIPPER_BINS per pixel).Fc st||_||_||_t|_tdtj j t ddt d|_ |r7|j D]}d|_q'|j tdntd|j j|_td|jtt|jd |_td |jd |j}ttj||d d dttj||d d dttj||d d dt|_td|tj||jtd d|_td|jdtd|d|d tt|t||tt|d |_tt|t||tt|d t|_ td|jdtd|jdtddS)NzLoading DINOv2 model...Zdinov3_vits16pluslocal)sourceweightsFu✓ Frozen DINOv2 backboneu ✓ DINOv2 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, u✓ Gripper MLP → (B, z+, 1) [BCE sigmoid, indexed at query pixel]u✓ Rotation MLP → (B, z, 3, z ) [CE, indexed at query pixel])!super__init__ target_size pred_sizen_windowDINO_PATCH_SIZEZ patch_sizeprinttorchhubloadrrdino parameters requires_gradeval embed_dimnn Parameterrandnstart_keypoint_embedding SequentialConv2dGELU feature_convs N_HEIGHT_BINS volume_head LayerNormLinear gripper_mlp N_ROT_BINS rotation_mlp)selfrrrfreeze_backboneparamD __class__"/data/cameron/para/libero/model.pyr&sT    $"&z#TrajectoryHeatmapPredictor.__init__cs(t|t|dr|j||_|S)Nr )rtohasattrr )r4devicer8r:r;r<`s  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)zExtract patch features and CLS token. Returns: patch_features: (B, D, H_p, W_p) cls_token: (B, D) r)HWNr)dimr)shaper Zprepare_tokens_with_masksblocksZ rope_embedZuntie_cls_and_patch_normsZcls_normZn_storage_tokensnormrcatreshaper$permute contiguous) r4xBZx_tokensH_pW_pZblkZ rope_sincosZ x_norm_clsZx_norm_patches cls_tokenZ patch_tokenspatch_featuresr:r:r;_extract_dino_featuresfs   $$ z1TrajectoryHeatmapPredictor._extract_dino_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).rr>N)rClongclampraranger>viewexpand) r4feats query_pixelsrKr7r?r@NpxpyZ batch_idxr:r:r;_index_features~s   z*TrajectoryHeatmapPredictor._index_featuresc Csh|jdd\}}|||}||||j}||||}||}|||dt} || fS)aApply gripper/rotation MLP heads at the specified pixel locations. Called with GT pixels during training (teacher forcing) and with predicted pixels (from volume argmax) during inference. Args: feats: (B, D, pred_size, pred_size) query_pixels: (B, N_WINDOW, 2) in pred_size coordinate space Returns: gripper_logits: (B, N_WINDOW) raw logits (apply sigmoid for probability) rotation_logits: (B, N_WINDOW, 3, N_ROT_BINS) NrBr)rCr\detachrGr$r1r3r2) r4rWrXrKrYindexedflatZgripperZrot_flatrotationr:r:r;predict_at_pixelss z,TrajectoryHeatmapPredictor.predict_at_pixelsNcCs@|jd}||\}}|j\}}} } |dkr"|d|d}|dddf| |jd| d} |dddf| |jd| d} tj ||j d} || dd| | f|j d7<t j ||j|jfddd}||}||}|||jt|j|j}|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 rrNrQbilinearF)sizemode align_corners)rCrPrA unsqueezerVrrRrSrrTr>r(F interpolaterr,r.rUrr-ra)r4rJstart_keypoint_2drXrKrOrN_r7rLrMZ start_patch_xZ start_patch_yZ batch_indicesrWvol volume_logitsgripper_logitsrotation_logitsr:r:r;forwards"  **$   z"TrajectoryHeatmapPredictor.forward)N) __name__ __module__ __qualname____doc__ PRED_SIZEN_WINDOWrr<rPr\rarp __classcell__r:r:r8r;r #s :r __main__mpscpur T)rrr5rBrgl@)rjrXzvolume_logits zgripper_logits rozfeats )1rtosrtorch.nnr%torch.nn.functional functionalrhenvirongetrrr IMAGENET_MEAN IMAGENET_STDrv MIN_HEIGHT MAX_HEIGHT MIN_GRIPPER MAX_GRIPPERMIN_ROTMAX_ROTMIN_POSMAX_POSr-N_GRIPPER_BINSr2ruModuler rqr>backendsry is_availablemodelr<r'rJzerosZ fake_queryno_gradtensorrlZgriprotrWrrCr:r:r:r;sN 3  (