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

move test_yopo_ros.py and yopo_planner_node.cpp to single script

Browse Source
This commit is contained in:
TJU_Lu
2024-12-24 18:03:49 +08:00
parent 09e832c829
commit 1a9f7c9f42
7 changed files with 892 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

131
flightpolicy/yopo/primitive_utils.py
View File

@@ -5,7 +5,7 @@ from scipy.spatial.transform import Rotation as R
class LatticeParam():
def __init__(self, cfg):
self.vel_max = cfg["vel_max"]
segment_time = 2 * cfg["radio_range"] / self.vel_max
self.segment_time = 2 * cfg["radio_range"] / self.vel_max
self.horizon_num = cfg["horizon_num"]
self.vertical_num = cfg["vertical_num"]
self.radio_num = cfg["radio_num"]
@@ -17,10 +17,10 @@ class LatticeParam():
self.radio_range = cfg["radio_range"]
self.vel_fov = cfg["vel_fov"]
self.vel_prefile = cfg["vel_prefile"]
self.acc_max = self.vel_max / segment_time
self.acc_max = self.vel_max / self.segment_time
print("---------------------")
print("| max speed = ", round(self.vel_max, 1), " |")
print("| traj time = ", round(segment_time, 1), " |")
print("| traj time = ", round(self.segment_time, 1), " |")
print("| max radio = ", round(2 * self.radio_range, 1), " |")
print("---------------------")
@@ -95,6 +95,63 @@ class LatticePrimitive():
return self.lattice_Rbp_list[id]
class Poly5Solver:
def __init__(self, pos0, vel0, acc0, pos1, vel1, acc1, Tf):
""" 5-th order polynomial at each Axis """
State_Mat = np.array([pos0, vel0, acc0, pos1, vel1, acc1])
t = Tf
Coef_inv = np.array([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1 / 2, 0, 0, 0],
[-10 / t ** 3, -6 / t ** 2, -3 / (2 * t), 10 / t ** 3, -4 / t ** 2, 1 / (2 * t)],
[15 / t ** 4, 8 / t ** 3, 3 / (2 * t ** 2), -15 / t ** 4, 7 / t ** 3, -1 / t ** 2],
[-6 / t ** 5, -3 / t ** 4, -1 / (2 * t ** 3), 6 / t ** 5, -3 / t ** 4, 1 / (2 * t ** 3)]])
self.A = np.dot(Coef_inv, State_Mat)
def get_snap(self, t):
"""Return the scalar jerk at time t."""
return 24 * self.A[4] + 120 * self.A[5] * t
def get_jerk(self, t):
"""Return the scalar jerk at time t."""
return 6 * self.A[3] + 24 * self.A[4] * t + 60 * self.A[5] * t * t
def get_acceleration(self, t):
"""Return the scalar acceleration at time t."""
return 2 * self.A[2] + 6 * self.A[3] * t + 12 * self.A[4] * t * t + 20 * self.A[5] * t * t * t
def get_velocity(self, t):
"""Return the scalar velocity at time t."""
return self.A[1] + 2 * self.A[2] * t + 3 * self.A[3] * t * t + 4 * self.A[4] * t * t * t + \
5 * self.A[5] * t * t * t * t
def get_position(self, t):
"""Return the scalar position at time t."""
return self.A[0] + self.A[1] * t + self.A[2] * t * t + self.A[3] * t * t * t + self.A[4] * t * t * t * t + \
self.A[5] * t * t * t * t * t
class Polys5Solver:
def __init__(self, pos0, vel0, acc0, pos1, vel1, acc1, Tf):
""" multiple 5-th order polynomials at each Axis (only used for visualization of multiple trajectories) """
N = len(pos1)
State_Mat = np.array([[pos0] * N, [vel0] * N, [acc0] * N, pos1, vel1, acc1])
t = Tf
Coef_inv = np.array([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1 / 2, 0, 0, 0],
[-10 / t ** 3, -6 / t ** 2, -3 / (2 * t), 10 / t ** 3, -4 / t ** 2, 1 / (2 * t)],
[15 / t ** 4, 8 / t ** 3, 3 / (2 * t ** 2), -15 / t ** 4, 7 / t ** 3, -1 / t ** 2],
[-6 / t ** 5, -3 / t ** 4, -1 / (2 * t ** 3), 6 / t ** 5, -3 / t ** 4, 1 / (2 * t ** 3)]])
self.A = np.dot(Coef_inv, State_Mat)
def get_position(self, t):
"""Return the position array at time t."""
t = np.atleast_1d(t)
result = (self.A[0][:, np.newaxis] + self.A[1][:, np.newaxis] * t + self.A[2][:, np.newaxis] * t ** 2 +
self.A[3][:, np.newaxis] * t ** 3 + self.A[4][:, np.newaxis] * t ** 4 + self.A[5][:, np.newaxis] * t ** 5 )
return result.flatten()
"""
From body to world
p_w = Rwb * p_b + t_w
@@ -135,3 +192,71 @@ def rotate_inv(q_wb, pos_w): # quat: wxzy
def transform_inv(q_wb, tw, pos_w):
pos_b = rotate_inv(q_wb, pos_w - tw)
return pos_b
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
# Normalize 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
# Desired direction
dir_des = vel_dir + goal_dir
# 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
# Initialize yaw and yawdot
yaw = last_yaw_
yawdot = 0
# 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
return yaw, yawdot