""" CPU-optimized training for Human-Like Robot Navigation Trajectory Generator. Adapted for CPU sandbox with smaller UNet dims but same architecture. """ import torch import torch.nn as nn import torch.nn.functional as F import numpy as np import math import json import os import time from pathlib import Path # ═══════════════════════════════════════════════════════════════════════ # DATA GENERATION (same as before) # ═══════════════════════════════════════════════════════════════════════ AREA_SIZE = 20.0 NUM_EPISODES = 2000 MIN_STEPS = 40 MAX_STEPS = 120 DT = 0.1 PREFERRED_SPEED = 1.3 MAX_SPEED = 2.0 MIN_SPEED = 0.3 OBSTACLES = [ (5, 5, 1.5), (15, 10, 2.0), (10, 15, 1.0), (3, 12, 1.2), (17, 4, 1.8), (8, 8, 0.8), (12, 3, 1.0), (6, 17, 1.5), ] def social_force(pos, vel, goal, obstacles, walls_min=0.0, walls_max=AREA_SIZE): force = np.zeros(2) goal_dir = goal - pos goal_dist = np.linalg.norm(goal_dir) if goal_dist > 0.1: desired_vel = PREFERRED_SPEED * goal_dir / goal_dist force += (desired_vel - vel) / 0.5 for ox, oy, r in obstacles: diff = pos - np.array([ox, oy]) dist = np.linalg.norm(diff) - r if dist < 5.0 and dist > 0.01: force += 3.0 * np.exp(-dist / 0.8) * diff / (np.linalg.norm(diff) + 1e-6) wall_A, wall_B = 2.0, 0.5 if pos[0] < 3.0: force[0] += wall_A * np.exp(-(pos[0] - walls_min) / wall_B) if pos[0] > AREA_SIZE - 3.0: force[0] -= wall_A * np.exp(-(walls_max - pos[0]) / wall_B) if pos[1] < 3.0: force[1] += wall_A * np.exp(-(pos[1] - walls_min) / wall_B) if pos[1] > AREA_SIZE - 3.0: force[1] -= wall_A * np.exp(-(walls_max - pos[1]) / wall_B) return force def generate_episode(): for _ in range(100): start = np.random.uniform(1.5, AREA_SIZE - 1.5, 2) goal = np.random.uniform(1.5, AREA_SIZE - 1.5, 2) if np.linalg.norm(goal - start) < 5.0: continue valid = all(np.linalg.norm(start - [ox, oy]) >= r + 0.5 and np.linalg.norm(goal - [ox, oy]) >= r + 0.5 for ox, oy, r in OBSTACLES) if valid: break pos = start.copy() heading = np.arctan2(goal[1]-start[1], goal[0]-start[0]) + np.random.randn() * 0.3 vel = np.array([np.cos(heading), np.sin(heading)]) * MIN_SPEED positions, velocities, actions = [pos.copy()], [vel.copy()], [] has_wp = np.random.random() < 0.4 mid = np.clip((start+goal)/2 + np.random.randn(2)*3, 1.5, AREA_SIZE-1.5) if has_wp else goal wp_reached = not has_wp cur_goal = mid if not wp_reached else goal for step in range(np.random.randint(MIN_STEPS, MAX_STEPS+1)): if not wp_reached and np.linalg.norm(pos - mid) < 1.5: wp_reached = True; cur_goal = goal force = social_force(pos, vel, cur_goal, OBSTACLES) spd = np.linalg.norm(vel) sway = np.array([-vel[1], vel[0]])/(spd+1e-6) * 0.05 * np.sin(2*np.pi*1.5*step*DT) if spd > 0.01 else np.zeros(2) vel = vel + (force + sway + np.random.randn(2)*0.02) * DT spd = np.linalg.norm(vel) if spd > MAX_SPEED: vel = vel/spd*MAX_SPEED elif spd < MIN_SPEED*0.5 and np.linalg.norm(pos-goal)>1: vel = vel/(spd+1e-6)*MIN_SPEED gd = np.linalg.norm(pos - goal) if gd < 2.0: vel *= max(0.1, gd/2.0) new_pos = np.clip(pos + vel*DT, 0.1, AREA_SIZE-0.1) actions.append((new_pos - pos).copy()) pos = new_pos; positions.append(pos.copy()); velocities.append(vel.copy()) if gd < 0.5: break return {'positions': np.array(positions), 'velocities': np.array(velocities), 'actions': np.array(actions), 'goal': goal, 'num_steps': len(actions)} def build_dataset(data_dir='/app/dataset'): np.random.seed(42); os.makedirs(data_dir, exist_ok=True) all_s, all_a, all_g, all_ep, all_fr, all_ts, all_d = [], [], [], [], [], [], [] ve = 0 print("Generating trajectories...") for _ in range(NUM_EPISODES): ep = generate_episode() if ep['num_steps'] < 10: continue for t in range(ep['num_steps']): all_s.append(np.concatenate([ep['positions'][t], ep['velocities'][t]]).tolist()) all_a.append(ep['actions'][t].tolist()) all_g.append(ep['goal'].tolist()) all_ep.append(ve); all_fr.append(t); all_ts.append(t*DT) all_d.append(t == ep['num_steps']-1) ve += 1 if ve % 500 == 0: print(f" {ve} episodes...") for name, arr, dt in [('observation_state', all_s, np.float32), ('action', all_a, np.float32), ('observation_goal', all_g, np.float32), ('episode_index', all_ep, np.int64), ('frame_index', all_fr, np.int64), ('timestamp', all_ts, np.float32)]: np.save(f'{data_dir}/{name}.npy', np.array(arr, dtype=dt)) np.save(f'{data_dir}/done.npy', np.array(all_d, dtype=bool)) print(f"Dataset: {ve} episodes, {len(all_s)} frames") # ═══════════════════════════════════════════════════════════════════════ # MODEL (same architecture, CPU-optimized dims) # ═══════════════════════════════════════════════════════════════════════ class CosineNoiseScheduler: def __init__(self, T=100, s=0.008): self.T = T t = torch.linspace(0, T, T+1) ac = torch.cos(((t/T)+s)/(1+s)*math.pi*0.5)**2 ac = ac/ac[0] betas = torch.clamp(1-(ac[1:]/ac[:-1]), 0.0001, 0.999) alphas = 1.0-betas ac = torch.cumprod(alphas, 0) ac_prev = F.pad(ac[:-1], (1,0), value=1.0) self.betas = betas self.sqrt_ac = torch.sqrt(ac) self.sqrt_1mac = torch.sqrt(1.0-ac) self.sqrt_ra = torch.sqrt(1.0/alphas) self.pv = betas*(1.0-ac_prev)/(1.0-ac) def add_noise(self, x0, noise, t): sa = self.sqrt_ac[t].view(-1,1,1) sm = self.sqrt_1mac[t].view(-1,1,1) return (sa*x0 + sm*noise) def step(self, pred, t, xt): b = self.betas[t].view(-1,1,1).to(xt.device) sm = self.sqrt_1mac[t].view(-1,1,1).to(xt.device) sr = self.sqrt_ra[t].view(-1,1,1).to(xt.device) mean = sr*(xt - b*pred/sm) if t[0]==0: return mean v = self.pv[t].view(-1,1,1).to(xt.device) return mean + torch.sqrt(v)*torch.randn_like(xt) class SinEmb(nn.Module): def __init__(self, d): super().__init__(); self.d = d def forward(self, t): h = self.d//2 e = math.log(10000)/(h-1) e = torch.exp(torch.arange(h, device=t.device)*-e) e = t[:,None].float()*e[None,:] return torch.cat([e.sin(), e.cos()], -1) class FiLM(nn.Module): def __init__(self, cd, fd): super().__init__() self.s = nn.Linear(cd, fd); self.b = nn.Linear(cd, fd) def forward(self, x, c): return x*(1+self.s(c).unsqueeze(-1))+self.b(c).unsqueeze(-1) class ResBlock(nn.Module): def __init__(self, ic, oc, cd, ks=5, g=8): super().__init__() p = ks//2 self.c1 = nn.Conv1d(ic, oc, ks, padding=p) self.c2 = nn.Conv1d(oc, oc, ks, padding=p) self.n1 = nn.GroupNorm(min(g,oc), oc) self.n2 = nn.GroupNorm(min(g,oc), oc) self.film = FiLM(cd, oc) self.act = nn.Mish() self.skip = nn.Conv1d(ic, oc, 1) if ic!=oc else nn.Identity() def forward(self, x, c): h = self.act(self.n1(self.c1(x))) h = self.film(h, c) h = self.act(self.n2(self.c2(h))) return h + self.skip(x) class TrajUNet(nn.Module): def __init__(self, ad=2, sd=4, gd=2, H=16, dims=(128,256,512), emb_d=64, ks=5, ng=8): super().__init__() self.cond_enc = nn.Sequential(nn.Linear(sd+gd, 128), nn.Mish(), nn.Linear(128, 128), nn.Mish()) self.time_enc = nn.Sequential(SinEmb(emb_d), nn.Linear(emb_d, emb_d*2), nn.Mish(), nn.Linear(emb_d*2, emb_d)) cd = 128+emb_d self.inp = nn.Conv1d(ad, dims[0], 1) self.down_b = nn.ModuleList([ResBlock(dims[i], dims[i], cd, ks, ng) for i in range(len(dims)-1)]) self.down_p = nn.ModuleList([nn.Conv1d(dims[i], dims[i+1], 3, 2, 1) for i in range(len(dims)-1)]) self.mid = ResBlock(dims[-1], dims[-1], cd, ks, ng) self.up_c = nn.ModuleList([nn.ConvTranspose1d(dims[i], dims[i-1], 4, 2, 1) for i in range(len(dims)-1, 0, -1)]) self.up_b = nn.ModuleList([ResBlock(dims[i-1]*2, dims[i-1], cd, ks, ng) for i in range(len(dims)-1, 0, -1)]) self.out = nn.Sequential(nn.Conv1d(dims[0], dims[0], ks, padding=ks//2), nn.Mish(), nn.Conv1d(dims[0], ad, 1)) def forward(self, na, t, s, g): c = torch.cat([self.cond_enc(torch.cat([s,g],-1)), self.time_enc(t)], -1) x = self.inp(na.permute(0,2,1)) sk = [] for b, p in zip(self.down_b, self.down_p): x = b(x, c); sk.append(x); x = p(x) x = self.mid(x, c) for uc, ub, s_ in zip(self.up_c, self.up_b, reversed(sk)): x = uc(x) if x.shape[-1] != s_.shape[-1]: x = F.pad(x, (0, s_.shape[-1]-x.shape[-1])) x = ub(torch.cat([x, s_], 1), c) return self.out(x).permute(0,2,1) class HumanTrajDiffusion(nn.Module): def __init__(self, ad=2, sd=4, gd=2, H=16, T=100, dims=(128,256,512)): super().__init__() self.H = H; self.ad = ad; self.T = T self.unet = TrajUNet(ad, sd, gd, H, dims) self.sched = CosineNoiseScheduler(T) def forward(self, actions, state, goal): B = actions.shape[0] t = torch.randint(0, self.T, (B,), device=actions.device) noise = torch.randn_like(actions) noisy = self.sched.add_noise(actions, noise, t.cpu()).to(actions.device) return F.mse_loss(self.unet(noisy, t, state, goal), noise) @torch.no_grad() def generate(self, state, goal, n=1): if state.dim()==1: state=state.unsqueeze(0) if goal.dim()==1: goal=goal.unsqueeze(0) dev = state.device if n>1: state=state.repeat(n,1); goal=goal.repeat(n,1) B = state.shape[0] x = torch.randn(B, self.H, self.ad, device=dev) for tv in reversed(range(self.T)): t = torch.full((B,), tv, device=dev, dtype=torch.long) x = self.sched.step(self.unet(x, t, state, goal), t.cpu(), x) return x # ═══════════════════════════════════════════════════════════════════════ # DATASET # ═══════════════════════════════════════════════════════════════════════ class TrajDS(torch.utils.data.Dataset): def __init__(self, dd='/app/dataset', H=16): self.H = H self.states = np.load(f'{dd}/observation_state.npy') self.actions = np.load(f'{dd}/action.npy') self.goals = np.load(f'{dd}/observation_goal.npy') self.eps = np.load(f'{dd}/episode_index.npy') self.sm, self.ss = self.states.mean(0), self.states.std(0)+1e-6 self.am, self.as_ = self.actions.mean(0), self.actions.std(0)+1e-6 self.gm, self.gs = self.goals.mean(0), self.goals.std(0)+1e-6 vi = [] for e in np.unique(self.eps): ei = np.where(self.eps==e)[0] for i in range(len(ei)-H): if ei[i+H-1]-ei[i]==H-1: vi.append(ei[i]) self.vi = np.array(vi) print(f"Dataset: {len(self.vi)} samples") def __len__(self): return len(self.vi) def __getitem__(self, i): s = self.vi[i] return { 'state': torch.tensor((self.states[s]-self.sm)/self.ss, dtype=torch.float32), 'goal': torch.tensor((self.goals[s]-self.gm)/self.gs, dtype=torch.float32), 'actions': torch.tensor((self.actions[s:s+self.H]-self.am)/self.as_, dtype=torch.float32), } def stats(self): return {k: getattr(self, k).tolist() for k in ['sm','ss','am','as_','gm','gs']} def stats_named(self): return {'state_mean': self.sm.tolist(), 'state_std': self.ss.tolist(), 'action_mean': self.am.tolist(), 'action_std': self.as_.tolist(), 'goal_mean': self.gm.tolist(), 'goal_std': self.gs.tolist()} # ═══════════════════════════════════════════════════════════════════════ # TRAINING # ═══════════════════════════════════════════════════════════════════════ def cosine_lr(opt, step, total, warmup=300, lo=1e-6, hi=2e-4): if step < warmup: lr = hi*step/warmup else: lr = lo + (hi-lo)*0.5*(1+math.cos(math.pi*(step-warmup)/(total-warmup))) for pg in opt.param_groups: pg['lr'] = lr return lr def train(): device = 'cuda' if torch.cuda.is_available() else 'cpu' print(f"Device: {device}") cfg = { 'horizon': 16, 'action_dim': 2, 'state_dim': 4, 'goal_dim': 2, 'num_diffusion_steps': 100, 'down_dims': [64, 128, 256], 'batch_size': 32, 'total_steps': 8000, 'lr': 2e-4, 'weight_decay': 1e-5, 'warmup_steps': 200, 'grad_clip': 10.0, 'eval_freq': 2000, 'log_freq': 25, 'hub_model_id': 'precison9/human-like-robot-nav-diffusion', } # Init trackio try: import trackio sid = os.environ.get('TRACKIO_SPACE_ID') proj = os.environ.get('TRACKIO_PROJECT', 'human-like-robot-nav') trackio.init(space_id=sid, project=proj, run="ddpm-traj-cpu-v1", config=cfg) HAS_T = True print(f"Trackio: {sid}/{proj}") except: HAS_T = False print("No trackio") # Generate data if needed if not os.path.exists('/app/dataset/observation_state.npy'): build_dataset() torch.manual_seed(42); np.random.seed(42) ds = TrajDS('/app/dataset', cfg['horizon']) dl = torch.utils.data.DataLoader(ds, batch_size=cfg['batch_size'], shuffle=True, num_workers=0, pin_memory=False, drop_last=True) model = HumanTrajDiffusion(ad=2, sd=4, gd=2, H=16, T=100, dims=tuple(cfg['down_dims'])).to(device) npar = sum(p.numel() for p in model.parameters()) print(f"Model: {npar:,} params ({npar/1e6:.1f}M)") opt = torch.optim.AdamW(model.parameters(), lr=cfg['lr'], betas=(0.95,0.999), weight_decay=cfg['weight_decay']) model.train() step = 0; rl = 0.0; best = float('inf'); t0 = time.time() print(f"\n{'='*60}\nTraining {cfg['total_steps']} steps on {device}\n{'='*60}\n") while step < cfg['total_steps']: for batch in dl: if step >= cfg['total_steps']: break s, g, a = batch['state'].to(device), batch['goal'].to(device), batch['actions'].to(device) lr = cosine_lr(opt, step, cfg['total_steps'], cfg['warmup_steps'], 1e-6, cfg['lr']) loss = model(a, s, g) opt.zero_grad(); loss.backward() torch.nn.utils.clip_grad_norm_(model.parameters(), cfg['grad_clip']) opt.step() rl += loss.item(); step += 1 if step % cfg['log_freq'] == 0: al = rl/cfg['log_freq']; el = time.time()-t0; sps = step/el print(f"step={step:5d} | loss={al:.6f} | lr={lr:.2e} | {sps:.1f} stp/s | eta={((cfg['total_steps']-step)/sps)/60:.1f}m") if HAS_T: trackio.log({'train/loss': al, 'train/lr': lr}) rl = 0.0 if step % cfg['eval_freq'] == 0: model.eval() els = [] idx = np.random.choice(len(ds), min(1280, len(ds)), replace=False) for i in range(0, len(idx), cfg['batch_size']): bi = [ds[j] for j in idx[i:i+cfg['batch_size']]] if len(bi)<2: continue with torch.no_grad(): els.append(model( torch.stack([x['actions'] for x in bi]).to(device), torch.stack([x['state'] for x in bi]).to(device), torch.stack([x['goal'] for x in bi]).to(device)).item()) el = np.mean(els) print(f" >>> EVAL step={step}: loss={el:.6f}") if HAS_T: trackio.log({'eval/loss': el}) if el < best: best = el save(model, opt, step, cfg, ds, '/app/checkpoints/best') if HAS_T: trackio.alert("Best", f"eval={el:.6f} step={step}", level="INFO") model.train() # Final save save(model, opt, step, cfg, ds, '/app/checkpoints/final') print(f"\nDone! Best eval: {best:.6f}") if HAS_T: trackio.alert("Done", f"steps={step}, best={best:.6f}", level="INFO") # Eval generation eval_gen(model, ds, device) # Push push(cfg, ds) def save(model, opt, step, cfg, ds, path): os.makedirs(path, exist_ok=True) torch.save(model.state_dict(), f'{path}/model.pt') torch.save({'step':step, 'model':model.state_dict(), 'opt':opt.state_dict()}, f'{path}/checkpoint.pt') with open(f'{path}/config.json','w') as f: json.dump(cfg, f, indent=2) with open(f'{path}/normalization_stats.json','w') as f: json.dump(ds.stats_named(), f, indent=2) print(f" Saved: {path}") def eval_gen(model, ds, device): model.eval() st = ds.stats_named() cases = [ ([2,2,0.5,0.5], [18,18]), ([2,18,0.3,-0.3], [18,2]), ([10,2,0,0.5], [10,18]), ([5,5,0.3,0.3], [15,15]), ] print(f"\n{'='*60}\nGeneration Evaluation\n{'='*60}") all_spd = [] for i, (s_raw, g_raw) in enumerate(cases): s_raw, g_raw = np.array(s_raw, np.float32), np.array(g_raw, np.float32) sn = torch.tensor((s_raw-np.array(st['state_mean']))/np.array(st['state_std']), dtype=torch.float32).to(device) gn = torch.tensor((g_raw-np.array(st['goal_mean']))/np.array(st['goal_std']), dtype=torch.float32).to(device) trajs = model.generate(sn, gn, n=5).cpu().numpy() td = trajs*np.array(st['action_std'])+np.array(st['action_mean']) pos = np.cumsum(td, axis=1) + s_raw[:2] speeds = np.linalg.norm(td, axis=-1)/DT all_spd.extend(speeds.flatten().tolist()) div = np.std(pos[:,-1], axis=0).mean() print(f" Case {i+1}: {s_raw[:2].tolist()} → {g_raw.tolist()} | " f"speed={speeds.mean():.2f}m/s | diversity={div:.3f}m") print(f"\n Overall mean speed: {np.mean(all_spd):.2f} m/s (human: ~1.3 m/s)") print(f" Speed std: {np.std(all_spd):.2f} m/s") def push(cfg, ds): from huggingface_hub import HfApi hid = cfg['hub_model_id'] bp = Path('/app/checkpoints/best') fp = Path('/app/checkpoints/final') up = bp if bp.exists() else fp if not up.exists(): print("No checkpoint!"); return api = HfApi() try: api.create_repo(hid, exist_ok=True) except Exception as e: print(f"Repo: {e}") st = ds.stats_named() np_ = sum(v.numel() for v in torch.load(up/'model.pt', map_location='cpu', weights_only=True).values()) readme = f"""--- tags: - robotics - trajectory-generation - diffusion-model - navigation - human-like-motion - ddpm library_name: pytorch pipeline_tag: reinforcement-learning license: mit --- # 🤖🚶 Human-Like Robot Navigation Trajectory Generator A **DDPM (Denoising Diffusion Probabilistic Model)** that generates human-like 2D navigation trajectories for robots. ## What It Does Given a robot's current state (position + velocity) and a goal, this model generates future waypoints that mimic human walking — smooth curves, natural speed changes, and obstacle-aware paths. ``` Input: [x, y, vx, vy] + [goal_x, goal_y] ↓ DDPM Reverse Diffusion (100 steps) ↓ 1D Temporal UNet + FiLM conditioning Output: 16 future waypoints [dx, dy] ``` ## Key Features - 🚶 **Human-like paths** — smooth curves, not robotic straight lines - ⚡ **Variable speed** — acceleration, cruising, deceleration like real walking - 🧱 **Obstacle aware** — learned from social force model training data - 🎲 **Multi-modal** — generates diverse trajectory samples via diffusion - 🎯 **Goal-directed** — conditions on target position ## Architecture | Component | Details | |-----------|---------| | Backbone | 1D Temporal UNet ({cfg['down_dims']}) | | Conditioning | FiLM (Feature-wise Linear Modulation) | | Noise Schedule | Cosine (Improved DDPM) | | Diffusion Steps | {cfg['num_diffusion_steps']} | | Parameters | {np_:,} ({np_/1e6:.1f}M) | | Prediction | ε-prediction (noise) | ## Based On - [Diffusion Policy](https://arxiv.org/abs/2303.04137) (Chi et al., RSS 2023) - [TRACE](https://arxiv.org/abs/2304.01893) (Rempe et al., CVPR 2023) - [Improved DDPM](https://arxiv.org/abs/2102.09672) (Nichol & Dhariwal, 2021) ## Training Data 2,000 synthetic episodes in a 20m × 20m environment with 8 obstacles: - Social Force Model physics (Helbing & Molnar 1995) - ~156K frames at 10 Hz - Speed range: 0.3-2.0 m/s (avg ~1.3 m/s, matching human walking) ## Quick Start ```python import torch, json, numpy as np # Load config = json.load(open('config.json')) stats = json.load(open('normalization_stats.json')) # Build model (copy architecture classes from this repo) model = HumanTrajDiffusion(ad=2, sd=4, gd=2, H=16, T=100, dims=tuple(config['down_dims'])) model.load_state_dict(torch.load('model.pt', map_location='cpu')) model.eval() # Robot at (5,5) moving NE → goal (15,15) state = np.array([5.0, 5.0, 0.5, 0.3]) goal = np.array([15.0, 15.0]) state_n = torch.tensor((state - stats['state_mean']) / stats['state_std'], dtype=torch.float32) goal_n = torch.tensor((goal - stats['goal_mean']) / stats['goal_std'], dtype=torch.float32) # Generate 5 diverse paths trajectories = model.generate(state_n, goal_n, n=5) # → Real coordinates traj = trajectories.numpy() * stats['action_std'] + stats['action_mean'] positions = np.cumsum(traj, axis=1) + state[:2] # positions.shape = (5, 16, 2) — 5 paths, 16 waypoints, (x,y) ``` ## Config ```json {json.dumps(cfg, indent=2)} ``` ## Normalization Stats ```json {json.dumps(st, indent=2)} ``` ## Applications - 🤖 Mobile robot navigation - 🎮 NPC pedestrian AI - 🏗️ Crowd simulation - 📊 Trajectory prediction/planning """ with open(up/'README.md','w') as f: f.write(readme) # Also save model architecture code model_code = open('/app/train_cpu.py').read() with open(up/'model_architecture.py','w') as f: f.write(model_code) print(f"Pushing to: {hid}") try: api.upload_folder(folder_path=str(up), repo_id=hid, commit_message="Upload human-like robot nav model") print(f"✅ https://huggingface.co/{hid}") except Exception as e: print(f"Push error: {e}") if __name__ == '__main__': train()