From 1925c16b7a632d808fd5c3b374b484b553865223 Mon Sep 17 00:00:00 2001 From: TJU-Lu Date: Sat, 25 Oct 2025 16:53:11 +0800 Subject: [PATCH] Simplify yaw calculation, fine-tune goal cost --- .../src/so3_control/src/NetworkControl.cpp | 14 +++- YOPO/loss/guidance_loss.py | 46 ++++------- YOPO/policy/poly_solver.py | 77 +++++-------------- YOPO/yopo.rviz | 14 ++-- 4 files changed, 53 insertions(+), 98 deletions(-) diff --git a/Controller/src/so3_control/src/NetworkControl.cpp b/Controller/src/so3_control/src/NetworkControl.cpp index e847725..fdd55ac 100644 --- a/Controller/src/so3_control/src/NetworkControl.cpp +++ b/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); - Eigen::Vector3d acc_world = cur_att_ * acc; - acc_world(2) -= 9.8; - cur_acc_ = acc_world; + 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); } diff --git a/YOPO/loss/guidance_loss.py b/YOPO/loss/guidance_loss.py index e8f00b2..9d77583 100644 --- a/YOPO/loss/guidance_loss.py +++ b/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] - similarity = th.sum(traj_dir * goal_dir_norm, dim=1) # [B] + # projection length of trajectory on goal direction + 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) - return similarity_loss + # length difference along goal direction (cosine similarity) + parallel_diff = (goal_length - traj_along).abs() # [B] - def terminal_aware_similarity_loss(self, traj_dir, goal_dir): - """ - Returns: - similarity: (batch_size) → guidance loss + # length perpendicular to goal direction + traj_perp = traj_dir - traj_along.unsqueeze(1) * goal_dir_norm # [B, 3] + perp_diff = traj_perp.norm(dim=1) # [B] - SimilarityLoss: Projection length of the trajectory onto the goal direction: - higher cosine similarity and longer trajectory are preferred. - 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 + # 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 \ No newline at end of file diff --git a/YOPO/policy/poly_solver.py b/YOPO/policy/poly_solver.py index 26a34fd..92c249d 100644 --- a/YOPO/policy/poly_solver.py +++ b/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): - YAW_DOT_MAX_PER_SEC = max_yaw_rate * np.pi - # Direction of velocity +def calculate_yaw(vel_dir, goal_dir, last_yaw, dt, max_yaw_rate=0.5): + # Normalize velocity and goal directions 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 = (delta_yaw + np.pi) % (2 * np.pi) - np.pi # wrap to [-π, π] - weight = 6 * abs(delta_yaw) / np.pi # weight ∈ 6 * [0, 1] equal weight at 30°, goal weight increases as angle grows + delta_yaw = wrap_to_pi(goal_yaw - last_yaw) + weight = 6 * abs(delta_yaw) / np.pi # weight ∈ [0,6]; equal weight at 30°, goal weight increases as delta_yaw 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_temp = np.arctan2(dir_des[1], dir_des[0]) if goal_dist > 0.2 else last_yaw - max_yaw_change = YAW_DOT_MAX_PER_SEC * dt + # Yaw difference and limit + yaw_diff = wrap_to_pi(yaw_desired - last_yaw) + 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 - if 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 - 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 + # Updated yaw and yaw rate + yaw = wrap_to_pi(last_yaw + yaw_change) + yawdot = yaw_change / dt return yaw, yawdot + diff --git a/YOPO/yopo.rviz b/YOPO/yopo.rviz index dd44c99..30a8451 100644 --- a/YOPO/yopo.rviz +++ b/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 - X: 72 - Y: 27 + Width: 2548 + X: 1920 + Y: 378