From 01c8eada391de02b0421464c13feb8b8612b5ff1 Mon Sep 17 00:00:00 2001 From: TJU-Lu Date: Fri, 7 Nov 2025 20:55:39 +0800 Subject: [PATCH] Add line integral cost --- README.md | 8 +++++++ YOPO/config/traj_opt.yaml | 2 +- YOPO/loss/loss_function.py | 4 ++-- YOPO/loss/safety_loss.py | 46 +++++++++++++++++++++----------------- 4 files changed, 36 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index d3d16aa..7d18c90 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,14 @@ # You Only Plan Once +--- +This branch includes some of the latest developments, including: + +- Add acceleration cost to prevent excessive initial acceleration + +- Change safety cost to a line integral to avoid uneven evaluation from time integration + +--- Original Paper: [You Only Plan Once: A Learning-Based One-Stage Planner With Guidance Learning](https://ieeexplore.ieee.org/document/10528860) Improvements and Applications: [YOPOv2-Tracker: An End-to-End Agile Tracking and Navigation Framework from Perception to Action](https://arxiv.org/html/2505.06923v1) diff --git a/YOPO/config/traj_opt.yaml b/YOPO/config/traj_opt.yaml index 6e5b9b2..43d0755 100644 --- a/YOPO/config/traj_opt.yaml +++ b/YOPO/config/traj_opt.yaml @@ -10,7 +10,7 @@ acc_max_train: 6.0 wg: 0.12 # guidance ws: 10.0 # smoothness wa: 1.0 # acceleration -wc: 0.1 # collision +wc: 1.0 # collision # dataset: set image_size = primitive_num × downsampling_factor (×32 for ResNet-18) in each axis dataset_path: "../dataset" diff --git a/YOPO/loss/loss_function.py b/YOPO/loss/loss_function.py index 240b7d8..cebca0b 100644 --- a/YOPO/loss/loss_function.py +++ b/YOPO/loss/loss_function.py @@ -84,9 +84,9 @@ class YOPOLoss(nn.Module): """ vel_scale = cfg["vel_max_train"] / 1.0 self.smoothness_weight = cfg["ws"] / vel_scale ** 5 - self.safety_weight = cfg["wc"] * vel_scale - self.goal_weight = cfg["wg"] self.accele_weight = cfg["wa"] / vel_scale ** 3 + self.safety_weight = cfg["wc"] + self.goal_weight = cfg["wg"] def forward(self, state, prediction, goal, map_id): """ diff --git a/YOPO/loss/safety_loss.py b/YOPO/loss/safety_loss.py index 755c64f..8dc37ab 100644 --- a/YOPO/loss/safety_loss.py +++ b/YOPO/loss/safety_loss.py @@ -22,7 +22,7 @@ class SafetyLoss(nn.Module): self.sgm_time = cfg["sgm_time"] self.eval_points = 30 self.device = self._L.device - self.truncate_cost = False # truncate cost at collision or use full trajectory cost + self.time_integral = True # SDF self.voxel_size = 0.2 @@ -59,27 +59,17 @@ class SafetyLoss(nn.Module): # get info from sdf_map cost, dist = self.get_distance_cost(pos_batch, map_id) - if not self.truncate_cost: - # Compute time integral of full trajectory cost (for general scenario) - cost_dt = (cost * dt).reshape(-1, pos_coe.shape[1]) # [B*H*V, N] - cost_colli = cost_dt.sum(dim=-1) + if self.time_integral: + # Compute average time integral of trajectory cost + # Issue: uneven eval points may undercut cost by quickly crossing obstacles + cost_colli = cost.reshape(-1, pos_coe.shape[1]).mean(dim=-1) # [B*H*V, N] else: - # Only compute cost before the first collision (better for large-obstacle scenario) - dist = dist.view(batch_size, -1) # [B*H*V, N] - cost = cost.view(batch_size, -1) # [B*H*V, N] - - N = dist.shape[1] - mask = dist <= 0 # [B*H*V, N] - index = th.where(mask, th.arange(N).to(self.device).expand(batch_size, N), N - 1) - first_colli_idx = index.min(dim=1).values # [B*H*V] - - arange = th.arange(N).to(self.device).unsqueeze(0).expand(batch_size, N) # [B*H*V, N] - valid_mask = arange <= first_colli_idx.unsqueeze(1) # [B*H*V, N] - - masked_cost = cost * valid_mask - valid_count = first_colli_idx + 1 - - cost_colli = self.sgm_time * masked_cost.sum(dim=-1) / valid_count + # Compute average line integral of trajectory cost + vel_coe = self.get_velocity_from_coeff(coe, t_list) + vel_coe = vel_coe.norm(dim=-1) + line_integral_cost = (cost.reshape(-1, pos_coe.shape[1]) * vel_coe * dt).sum(dim=1) # [B*H*V, N] -> [B*H*V] + line_length = (vel_coe * dt).sum(dim=1) # [B*H*V] + cost_colli = line_integral_cost / line_length # [B*H*V] return cost_colli @@ -143,6 +133,20 @@ class SafetyLoss(nn.Module): pos = th.stack([x, y, z], dim=-1) return pos + def get_velocity_from_coeff(self, coe, t): + t_power = th.stack([th.ones_like(t), 2 * t, 3 * t ** 2, 4 * t ** 3, 5 * t ** 4], dim=-1).squeeze(-2) + + coe_x = coe[:, 1:6] + coe_y = coe[:, 7:12] + coe_z = coe[:, 13:18] + + vx = th.sum(t_power * coe_x.unsqueeze(1), dim=-1) + vy = th.sum(t_power * coe_y.unsqueeze(1), dim=-1) + vz = th.sum(t_power * coe_z.unsqueeze(1), dim=-1) + + vel = th.stack([vx, vy, vz], dim=-1) + return vel + def get_batch_sdf(self, pos, map_id): """ Crop all maps with the corresponding map_id in the batch to the same size and cover the pos.