github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 15:48:13 -05:00
Code Issues Packages Projects Releases Wiki Activity

analytic jerk calculation in frenet_optimal_planner.py

Browse Source
This commit is contained in:
jwdinius
2018-03-26 18:53:01 -07:00
parent 0d53101549
commit ad25d69983
1 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
View File

@@ -86,7 +86,11 @@ class quinic_polynomial:
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + 20 * self.a5 * t**3
return xt
def calc_third_derivative(self, t):
xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2
return xt
class quartic_polynomial:
@@ -128,6 +132,11 @@ class quartic_polynomial:
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2
return xt
def calc_third_derivative(self, t):
xt = 6 * self.a3 + 24 * self.a4 * t
return xt
class Frenet_path:
@@ -137,9 +146,11 @@ class Frenet_path:
self.d = []
self.d_d = []
self.d_dd = []
self.d_ddd = []
self.s = []
self.s_d = []
self.s_dd = []
self.s_ddd = []
self.cd = 0.0
self.cv = 0.0
self.cf = 0.0
@@ -168,6 +179,7 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
# Loongitudinal motion planning (Velocity keeping)
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
@@ -177,9 +189,10 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
Jp = sum(np.diff(tfp.d_dd)**2) # square of jerk
Js = sum(np.diff(tfp.s_dd)**2) # square of jerk
Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk
Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
# square of diff from target speed
ds = (TARGET_SPEED - tfp.s_d[-1])**2