From ad25d6998366514fcd73b15d5eb02f2f0ca7bbef Mon Sep 17 00:00:00 2001 From: jwdinius Date: Mon, 26 Mar 2018 18:53:01 -0700 Subject: [PATCH] analytic jerk calculation in frenet_optimal_planner.py --- .../frenet_optimal_trajectory.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 8a146760..d8978414 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -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