Add velocity direction constraint

README.md

@@ -1,14 +1,6 @@
 
 # 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)

YOPO/config/traj_opt.yaml

@@ -7,10 +7,10 @@ vel_max_train: 6.0
 acc_max_train: 6.0
 
 # IMPORTANT: weight of costs for unit speed (can be visualized in tensorboard)
-wg: 0.12   # guidance
+wg: 0.12   # guidance     (approaching the goal)
-ws: 10.0   # smoothness
+ws: 10.0   # smoothness   (smaller jerk)
-wa: 1.0    # acceleration
+wa: 1.0    # acceleration (smaller acceleration)
-wc: 1.0    # collision
+wc: 1.0    # collision    (distance to obstacle)
 
 # dataset: set image_size = primitive_num × downsampling_factor (×32 for ResNet-18) in each axis
 dataset_path: "../dataset"

YOPO/loss/guidance_loss.py

@@ -22,13 +22,16 @@ class GuidanceLoss(nn.Module):
         """
         cur_pos = Df[:, :, 0]
         end_pos = Dp[:, :, 0]
+        end_vel = Dp[:, :, 1]
 
         traj_dir = end_pos - cur_pos  # [B, 3]
         goal_dir = goal - cur_pos  # [B, 3]
 
         guidance_loss = self.distance_loss(traj_dir, goal_dir)
         # guidance_loss = self.similarity_loss(traj_dir, goal_dir)
-        return guidance_loss
+
+        # vel_dir_loss = self.derivative_similarity_loss(end_vel, goal_dir)
+        return guidance_loss # + 5 * vel_dir_loss
 
     def distance_loss(self, traj_dir, goal_dir):
         """
@@ -68,4 +71,14 @@ class GuidanceLoss(nn.Module):
         # distance weighting (reduce perpendicular constraint, allow lateral exploration)
         perp_weight = 0.5   # the given weight is trained with perp_weight = 0, for higher speed in large-scale scenario
         similarity_loss = parallel_diff + perp_weight * perp_diff
-        return similarity_loss
+        return similarity_loss
+
+    def derivative_similarity_loss(self, derivative, goal_dir):
+        """
+            Constrain the velocity direction toward the goal
+        """
+        goal_dir_norm = goal_dir / (goal_dir.norm(dim=1, keepdim=True) + 1e-8)  # [B, 3]
+        derivative_norm = derivative / (derivative.norm(dim=1, keepdim=True) + 1e-8)  # [B, 3]
+
+        similarity = (derivative_norm * goal_dir_norm).sum(dim=1)  # [B]
+        return 1 - similarity