Simplify yaw calculation, fine-tune goal cost

Controller/src/so3_control/src/NetworkControl.cpp

@@ -300,9 +300,17 @@ void NetworkControl::imu_callback(const sensor_msgs::Imu &imu)
     Eigen::Vector3d acc(imu.linear_acceleration.x,
                         imu.linear_acceleration.y,
                         imu.linear_acceleration.z);
+    if (is_simulation_)
+    {
+        cur_acc_ = acc;
+    }
+    else
+    {
         Eigen::Vector3d acc_world = cur_att_ * acc;
         acc_world(2) -= 9.8;
         cur_acc_ = acc_world;
+    }
+
     // so3_controller_.setAcc(acc_world);
 }
 

YOPO/loss/guidance_loss.py

@@ -18,7 +18,7 @@ class GuidanceLoss(nn.Module):
         Returns:
             guidance_loss: (batch_size) → guidance loss
 
-        GuidanceLoss: distance_loss (better near the goal) or similarity_loss (better during flight) or terminal_aware_similarity_loss (balanced)
+        GuidanceLoss: distance_loss (for straighter flight) or similarity_loss (for faster flight in large scenario)
         """
         cur_pos = Df[:, :, 0]
         end_pos = Dp[:, :, 0]
@@ -26,10 +26,8 @@ class GuidanceLoss(nn.Module):
         traj_dir = end_pos - cur_pos  # [B, 3]
         goal_dir = goal - cur_pos  # [B, 3]
 
-        # NOTE: trajectory with distance_loss is straighter and reaches the goal more accurately,
-        #       while our pre-trained model choose terminal_aware_similarity_loss only for higher speed in large-scale scenario.
         guidance_loss = self.distance_loss(traj_dir, goal_dir)
-        # guidance_loss = self.terminal_aware_similarity_loss(traj_dir, goal_dir)
+        # guidance_loss = self.similarity_loss(traj_dir, goal_dir)
         return guidance_loss
 
     def distance_loss(self, traj_dir, goal_dir):
@@ -39,7 +37,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.
-        Better near the goal, but slightly inferior to the similarity cost in general situations.
+        Straighter flight, but slightly inferior to the similarity cost 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)
@@ -52,34 +50,22 @@ class GuidanceLoss(nn.Module):
 
         SimilarityLoss: Projection length of the trajectory onto the goal direction:
                         higher cosine similarity and longer trajectory are preferred.
-        Performs better in general by allowing longer lateral avoidance without slowing down, but less precise near the goal.
+        Faster flight in large-scale scenario by allowing longer lateral avoidance without slowing down, but less precise near the goal.
         """
-        goal_length = goal_dir.norm(dim=1)
+        goal_dir_norm = goal_dir / (goal_dir.norm(dim=1, keepdim=True) + 1e-8)  # [B, 3]
 
-        goal_dir_norm = goal_dir / (goal_length.unsqueeze(1) + 1e-8)  # [B, 3]
+        # projection length of trajectory on goal direction
-        similarity = th.sum(traj_dir * goal_dir_norm, dim=1)  # [B]
+        traj_along = (traj_dir * goal_dir_norm).sum(dim=1)  # [B]
+        goal_length = goal_dir.norm(dim=1)  # [B]
 
-        similarity_loss = th.abs(goal_length - similarity)
+        # length difference along goal direction (cosine similarity)
-        return similarity_loss
+        parallel_diff = (goal_length - traj_along).abs()  # [B]
-
+
-    def terminal_aware_similarity_loss(self, traj_dir, goal_dir):
+        # length perpendicular to goal direction
-        """
+        traj_perp = traj_dir - traj_along.unsqueeze(1) * goal_dir_norm  # [B, 3]
-        Returns:
+        perp_diff = traj_perp.norm(dim=1)  # [B]
-            similarity: (batch_size) → guidance loss
+
-
+        # distance weighting (reduce perpendicular constraint, allow lateral exploration)
-        SimilarityLoss: Projection length of the trajectory onto the goal direction:
+        perp_weight = 0.5   # the given weight is trained with perp_weight = 0, for higher speed in large-scale scenario
-                        higher cosine similarity and longer trajectory are preferred.
+        similarity_loss = parallel_diff + perp_weight * perp_diff
-        Reduce perpendicular deviation when approaching the goal, and apply dynamic weighting to ensure loss continuity.
-        """
-        goal_length = goal_dir.norm(dim=1)
-
-        goal_dir_norm = goal_dir / (goal_length.unsqueeze(1) + 1e-8)  # [B, 3]
-        similarity = th.sum(traj_dir * goal_dir_norm, dim=1)  # [B]
-
-        traj_dir_proj = similarity.unsqueeze(1) * goal_dir_norm  # [B, 3]
-        perp_component = (traj_dir - traj_dir_proj).norm(dim=1)  # [B]
-
-        perp_weight = ((self.goal_length - goal_length) / self.goal_length).clamp(min=0.0, max=0.6)   # [B]
-        perp_weight[perp_weight < 1e-4] = 0.0  # eliminate tiny numerical errors for stability
-        similarity_loss = (1 - perp_weight) * th.abs(goal_length - similarity) + perp_weight * perp_component
         return similarity_loss

YOPO/policy/poly_solver.py

@@ -58,72 +58,33 @@ class Polys5Solver:
                   self.A[3][:, np.newaxis] * t ** 3 + self.A[4][:, np.newaxis] * t ** 4 + self.A[5][:, np.newaxis] * t ** 5)
         return result.flatten()
 
+def wrap_to_pi(angle):
+    """将角度限制在 [-pi, pi]"""
+    return (angle + np.pi) % (2 * np.pi) - np.pi
 
-def calculate_yaw(vel_dir, goal_dir, last_yaw, dt, max_yaw_rate=0.3):
+def calculate_yaw(vel_dir, goal_dir, last_yaw, dt, max_yaw_rate=0.5):
-    YAW_DOT_MAX_PER_SEC = max_yaw_rate * np.pi
+    # Normalize velocity and goal directions
-    # Direction of velocity
     vel_dir = vel_dir / (np.linalg.norm(vel_dir) + 1e-5)
-
-    # Direction of goal
     goal_dist = np.linalg.norm(goal_dir)
-    goal_dir = goal_dir / (goal_dist + 1e-5)  # Prevent division by zero
+    goal_dir = goal_dir / (goal_dist + 1e-5)
 
-    # Dynamically adjust weights between goal and velocity directions in yaw planning
+    # Goal yaw and weighting
     goal_yaw = np.arctan2(goal_dir[1], goal_dir[0])
-    delta_yaw = goal_yaw - last_yaw
+    delta_yaw = wrap_to_pi(goal_yaw - last_yaw)
-    delta_yaw = (delta_yaw + np.pi) % (2 * np.pi) - np.pi  # wrap to [-π, π]
+    weight = 6 * abs(delta_yaw) / np.pi  # weight ∈ [0,6]; equal weight at 30°, goal weight increases as delta_yaw grows
-    weight = 6 * abs(delta_yaw) / np.pi  #  weight ∈ 6 * [0, 1]  equal weight at 30°, goal weight increases as angle grows
 
-    # Desired direction
+    # Desired direction and yaw
     dir_des = vel_dir + weight * goal_dir
+    yaw_desired = np.arctan2(dir_des[1], dir_des[0]) if goal_dist > 0.5 else last_yaw
 
-    # Temporary yaw calculation
+    # Yaw difference and limit
-    yaw_temp = np.arctan2(dir_des[1], dir_des[0]) if goal_dist > 0.2 else last_yaw
+    yaw_diff = wrap_to_pi(yaw_desired - last_yaw)
-    max_yaw_change = YAW_DOT_MAX_PER_SEC * dt
+    max_yaw_change = max_yaw_rate * np.pi * dt
+    yaw_change = np.clip(yaw_diff, -max_yaw_change, max_yaw_change)
 
-    # Logic for yaw adjustment
+    # Updated yaw and yaw rate
-    if yaw_temp - last_yaw > np.pi:
+    yaw = wrap_to_pi(last_yaw + yaw_change)
-        if yaw_temp - last_yaw - 2 * np.pi < -max_yaw_change:
+    yawdot = yaw_change / dt
-            yaw = last_yaw - max_yaw_change
-            if yaw < -np.pi:
-                yaw += 2 * np.pi
-            yawdot = -YAW_DOT_MAX_PER_SEC
-        else:
-            yaw = yaw_temp
-            if yaw - last_yaw > np.pi:
-                yawdot = -YAW_DOT_MAX_PER_SEC
-            else:
-                yawdot = (yaw_temp - last_yaw) / dt
-    elif yaw_temp - last_yaw < -np.pi:
-        if yaw_temp - last_yaw + 2 * np.pi > max_yaw_change:
-            yaw = last_yaw + max_yaw_change
-            if yaw > np.pi:
-                yaw -= 2 * np.pi
-            yawdot = YAW_DOT_MAX_PER_SEC
-        else:
-            yaw = yaw_temp
-            if yaw - last_yaw < -np.pi:
-                yawdot = YAW_DOT_MAX_PER_SEC
-            else:
-                yawdot = (yaw_temp - last_yaw) / dt
-    else:
-        if yaw_temp - last_yaw < -max_yaw_change:
-            yaw = last_yaw - max_yaw_change
-            if yaw < -np.pi:
-                yaw += 2 * np.pi
-            yawdot = -YAW_DOT_MAX_PER_SEC
-        elif yaw_temp - last_yaw > max_yaw_change:
-            yaw = last_yaw + max_yaw_change
-            if yaw > np.pi:
-                yaw -= 2 * np.pi
-            yawdot = YAW_DOT_MAX_PER_SEC
-        else:
-            yaw = yaw_temp
-            if yaw - last_yaw > np.pi:
-                yawdot = -YAW_DOT_MAX_PER_SEC
-            elif yaw - last_yaw < -np.pi:
-                yawdot = YAW_DOT_MAX_PER_SEC
-            else:
-                yawdot = (yaw_temp - last_yaw) / dt
 
     return yaw, yawdot
+

YOPO/yopo.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Map1/Autocompute Value Bounds1
         - /Trajectory1
       Splitter Ratio: 0.6625221967697144
-    Tree Height: 387
+    Tree Height: 653
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -215,7 +215,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 49.83913803100586
+      Distance: 134.25611877441406
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -240,10 +240,10 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1016
+  Height: 1495
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000002700000033afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001c0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680100000203000001740000001600ffffff00000001000001b90000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c20000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002700000051dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002c8000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006801000003090000024f0000001600ffffff00000001000001b90000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009f40000005efc0100000002fb0000000800540069006d00650100000000000009f40000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000077e0000051d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -252,6 +252,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1848
+  Width: 2548
-  X: 72
+  X: 1920
-  Y: 27
+  Y: 378