hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Simplify yaw calculation, fine-tune goal cost

Browse Source
This commit is contained in:
TJU-Lu 2025-10-25 16:53:11 +08:00
parent 27acd66598
commit 1925c16b7a
4 changed files with 53 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Controller/src/so3_control/src/NetworkControl.cpp
View File

@ -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);
}

46
YOPO/loss/guidance_loss.py
View File

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

77
YOPO/policy/poly_solver.py
View File

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

14
YOPO/yopo.rviz
View File

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