Minor adjustments

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)

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]
 

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

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))