TurtleBotJEPA-PPO / policy /jepa_encoder.py
penji2803's picture
TurtleBotJEPA-PPO: ready-to-run JEPA + PPO policy for a real TurtleBot3 (LDS-02), guide incl. mapping and map goals
4421dc3 verified
Raw History Blame Contribute Delete
5.22 kB
"""Frozen LiDAR V-JEPA encoder -> 'last8' embedding. Standalone: needs only torch, numpy and safetensors.
from jepa_encoder import JepaLast8Encoder
enc = JepaLast8Encoder.load('.') # folder with encoder_last8.safetensors + encoder_config.json
z = enc.embed(scans, valid) # scans (20, 360) metres, valid (20,) bool -> (1024,) float32
Z = enc(scans_batch, valid_batch) # (B, 20, 360), (B, 20) -> (B, 1024) torch tensor
Input: the last 20 scans of a 360-ray 2-D lidar at 5 Hz (4 s), oldest first, ranges in metres in [0.12, 3.5],
3.5 = no return. At the start of an episode, left-pad with copies of the first scan and set valid=False for them.
Pipeline (identical to the target branch of the full model, see lidar_jepa/):
inverse range r_min/r + validity channel -> Conv2d tubelets (2 frames x 10 rays) -> LayerNorm
-> + circular sin-cos positions -> 6 pre-LN transformer blocks (4 heads, GELU MLP 512, key-padding mask)
-> final LayerNorm -> last time step (36 sector tokens) -> mean within 8 front-centred direction bins (8 x 128)
-> per-dimension standardisation (training-set mean / std).
Direction bins: 0 = straight ahead (-20..+20 deg), counter-clockwise: 2 = left, 4 = behind, 6 = right
(ray 0 = straight ahead, rays increase counter-clockwise, as in a ROS LaserScan from angle 0 to 2 pi).
"""
import json
import os
import numpy as np
import torch
import torch.nn.functional as F
from safetensors.torch import load_file
class JepaLast8Encoder:
def __init__(self, tensors, config, device='cpu', threads=None):
if threads:
torch.set_num_threads(threads)
self.dev = torch.device(device)
self.c = c = config
t = lambda x: x.to(self.dev, torch.float32) # noqa: E731
self.w = {k: t(v) for k, v in tensors.items() if k not in ('pos_enc', 'bins', 'norm_mean', 'norm_std')}
self.pe = t(tensors['pos_enc']) # (N, D)
M = torch.zeros(8, c['n_a'])
M[tensors['bins'].long(), torch.arange(c['n_a'])] = 1
self.M = t(M / M.sum(1, keepdim=True)) # (8, n_a) bin means
self.mu, self.sd = t(tensors['norm_mean']), t(tensors['norm_std'])
self.out_dim = int(self.mu.numel())
@classmethod
def load(cls, folder='.', device='cpu', threads=None):
cfg = json.load(open(os.path.join(folder, 'encoder_config.json')))
return cls(load_file(os.path.join(folder, 'encoder_last8.safetensors')), cfg, device, threads)
@torch.inference_mode()
def __call__(self, scans, valid=None, standardise=True):
"""scans (B, T, R) metres, valid (B, T) bool -> (B, 1024)."""
c, w = self.c, self.w
x = torch.as_tensor(np.array(scans, np.float32), device=self.dev)
B, T, R = x.shape
assert (T, R) == (c['T'], c['R']), f'expected (B, {c["T"]}, {c["R"]}) scans, got {tuple(x.shape)}'
fv = (torch.ones(B, T, dtype=torch.bool, device=self.dev) if valid is None
else torch.as_tensor(np.asarray(valid), device=self.dev).bool())
mask = ((x < c['r_max'] - c['eps']) & fv[..., None]).float()
img = torch.stack([c['r_min'] / x.clamp(min=c['r_min']) * mask, mask], 1) # (B, 2, T, R)
h = F.conv2d(img, w['tok.proj.w'], w['tok.proj.b'], stride=tuple(c['tubelet'])) # (B, D, n_t, n_a)
h = h.flatten(2).transpose(1, 2) # (B, N, D)
D = h.shape[-1]
h = F.layer_norm(h, (D,), w['tok.norm.w'], w['tok.norm.b'], c['tok_ln_eps']) + self.pe
tv = fv.view(B, c['n_t'], c['tubelet'][0]).all(-1) # (B, n_t)
tv = tv[:, :, None].expand(B, c['n_t'], c['n_a']).reshape(B, -1) # (B, N)
attn_mask = None if bool(tv.all()) else tv[:, None, None, :] # keys only
nh, eps = c['heads'], c['ln_eps']
for i in range(c['depth']):
p = f'l{i}.'
y = F.layer_norm(h, (D,), w[p + 'ln1.w'], w[p + 'ln1.b'], eps)
q, k, v = (F.linear(y, w[p + n + '.w'], w[p + n + '.b']).view(B, -1, nh, D // nh).transpose(1, 2)
for n in ('q', 'k', 'v'))
a = F.scaled_dot_product_attention(q, k, v, attn_mask=attn_mask)
h = h + F.linear(a.transpose(1, 2).reshape(B, -1, D), w[p + 'o.w'], w[p + 'o.b'])
y = F.layer_norm(h, (D,), w[p + 'ln2.w'], w[p + 'ln2.b'], eps)
h = h + F.linear(F.gelu(F.linear(y, w[p + 'fc1.w'], w[p + 'fc1.b'])), w[p + 'fc2.w'], w[p + 'fc2.b'])
h = F.layer_norm(h, (D,), w['final_ln.w'], w['final_ln.b'], eps)
last = h[:, -c['n_a']:] # (B, n_a, D)
z = torch.einsum('ga,bad->bgd', self.M, last).flatten(1) # (B, 8 * D)
return (z - self.mu) / self.sd if standardise else z
def embed(self, scans, valid=None):
"""One window: (T, R) [+ (T,)] -> (1024,) float32 numpy."""
v = None if valid is None else np.asarray(valid)[None]
return self(np.asarray(scans, np.float32)[None], v)[0].cpu().numpy()