From 39cece7fac1271517f1b9113280a04e7984cc3b7 Mon Sep 17 00:00:00 2001 From: TJU-Lu Date: Sat, 22 Nov 2025 15:47:46 +0800 Subject: [PATCH] Minor adjustments --- YOPO/config/traj_opt.yaml | 2 +- YOPO/loss/guidance_loss.py | 16 ++++++++++------ YOPO/loss/loss_function.py | 13 +++---------- YOPO/test_yopo_ros.py | 2 +- 4 files changed, 15 insertions(+), 18 deletions(-) diff --git a/YOPO/config/traj_opt.yaml b/YOPO/config/traj_opt.yaml index 6e3850b..6893372 100644 --- a/YOPO/config/traj_opt.yaml +++ b/YOPO/config/traj_opt.yaml @@ -7,7 +7,7 @@ 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 (approaching the goal) +wg: 0.15 # guidance (approaching the goal) ws: 10.0 # smoothness (smaller jerk) wa: 1.0 # acceleration (smaller acceleration) wc: 1.0 # collision (distance to obstacle) diff --git a/YOPO/loss/guidance_loss.py b/YOPO/loss/guidance_loss.py index 533e400..b26ca0a 100644 --- a/YOPO/loss/guidance_loss.py +++ b/YOPO/loss/guidance_loss.py @@ -8,6 +8,7 @@ class GuidanceLoss(nn.Module): def __init__(self): super(GuidanceLoss, self).__init__() self.goal_length = cfg['goal_length'] + self.vel_dir_weight = 0 # 5 def forward(self, Df, Dp, goal): """ @@ -27,11 +28,13 @@ class GuidanceLoss(nn.Module): 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) + # guidance_loss = self.distance_loss(traj_dir, goal_dir) + guidance_loss = self.similarity_loss(traj_dir, goal_dir) - # vel_dir_loss = self.derivative_similarity_loss(end_vel, goal_dir) - return guidance_loss # + 5 * vel_dir_loss + if self.vel_dir_weight > 0: + vel_dir_loss = self.derivative_similarity_loss(end_vel, goal_dir) + guidance_loss += self.vel_dir_weight * vel_dir_loss + return guidance_loss def distance_loss(self, traj_dir, goal_dir): """ @@ -40,7 +43,7 @@ class GuidanceLoss(nn.Module): L1Loss: L1 distance (same scale as the similarity loss) to the normalized goal (for numerical stability). closer to the goal is preferred. - Straighter flight, but slightly inferior to the similarity cost in flight speed. + Straighter flight and more precise near the goal, but slightly inferior in flight speed. """ l1_distance = F.smooth_l1_loss(traj_dir, goal_dir, reduction='none') # shape: (B, 3) l1_distance = l1_distance.sum(dim=1) # (B) @@ -53,7 +56,8 @@ class GuidanceLoss(nn.Module): SimilarityLoss: Projection length of the trajectory onto the goal direction: higher cosine similarity and longer trajectory are preferred. - Faster flight in large-scale scenario by allowing longer lateral avoidance without slowing down, but less precise near the goal. + + Adjust perp_weight to penalize deviation perpendicular to the goal; equals the distance_loss() when perp_weight = 1. """ goal_dir_norm = goal_dir / (goal_dir.norm(dim=1, keepdim=True) + 1e-8) # [B, 3] diff --git a/YOPO/loss/loss_function.py b/YOPO/loss/loss_function.py index cebca0b..c3fd488 100644 --- a/YOPO/loss/loss_function.py +++ b/YOPO/loss/loss_function.py @@ -104,15 +104,8 @@ class YOPOLoss(nn.Module): # Decision parameters (local frame) → (batch_size, 3, 3) [px, vx, ax; py, vy, ay; pz, vz, az] Dp = prediction.permute(0, 2, 1) - smoothness_cost = th.tensor(0.0, device=self.device, requires_grad=True) - safety_cost = th.tensor(0.0, device=self.device, requires_grad=True) - goal_cost = th.tensor(0.0, device=self.device, requires_grad=True) - - if self.smoothness_weight > 0: - smoothness_cost, acceleration_cost = self.smoothness_loss(Df, Dp) - if self.safety_weight > 0: - safety_cost = self.safety_loss(Df, Dp, map_id) - if self.goal_weight > 0: - goal_cost = self.goal_loss(Df, Dp, goal) + smoothness_cost, acceleration_cost = self.smoothness_loss(Df, Dp) + safety_cost = self.safety_loss(Df, Dp, map_id) + goal_cost = self.goal_loss(Df, Dp, goal) return self.smoothness_weight * smoothness_cost, self.safety_weight * safety_cost, self.goal_weight * goal_cost, self.accele_weight * acceleration_cost \ No newline at end of file diff --git a/YOPO/test_yopo_ros.py b/YOPO/test_yopo_ros.py index 4d16566..4ae4ad0 100644 --- a/YOPO/test_yopo_ros.py +++ b/YOPO/test_yopo_ros.py @@ -189,7 +189,7 @@ class YopoNet: endstate_c = endstate.reshape(-1, 3, 3).transpose(0, 2, 1) # [N, 9] -> [N, 3, 3] -> [px vx ax, py vy ay, pz vz az] endstate_w = np.matmul(self.Rotation_wc, endstate_c) - action_id = np.argmin(score_pred) if self.visualize else 0 + action_id = np.argmin(score) if self.visualize else 0 with self.lock: # Python3.8: threads are scheduled using time slices, add the lock to ensure safety start_pos = self.desire_pos if self.plan_from_reference else np.array((self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z)) start_vel = self.desire_vel if self.plan_from_reference else np.array((self.odom.twist.twist.linear.x, self.odom.twist.twist.linear.y, self.odom.twist.twist.linear.z))