Merge pull request AtsushiSakai#53 from jwdinius/feature/fop_analytic_jerk

analytic jerk calculation in frenet_optimal_planner.py

Author: AtsushiSakai

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py

@@ -86,7 +86,11 @@ def calc_second_derivative(self, t):
         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 @@ def calc_second_derivative(self, t):
         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 @@ def __init__(self):
         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