diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index e7c744d1..8600f1d7 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -24,18 +24,17 @@ show_animation = True class QuinticPolynomial: - def __init__(self, xs, vxs, axs, xe, vxe, axe, T): - + def __init__(self, xs, vxs, axs, xe, vxe, axe, time): # calc coefficient of quintic polynomial self.a0 = xs self.a1 = vxs self.a2 = axs / 2.0 - A = np.array([[T**3, T**4, T**5], - [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], - [6 * T, 12 * T ** 2, 20 * T ** 3]]) - b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T**2, - vxe - self.a1 - 2 * self.a2 * T, + A = np.array([[time ** 3, time ** 4, time ** 5], + [3 * time ** 2, 4 * time ** 3, 5 * time ** 4], + [6 * time, 12 * time ** 2, 20 * time ** 3]]) + b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2, + vxe - self.a1 - 2 * self.a2 * time, axe - 2 * self.a2]) x = np.linalg.solve(A, b) @@ -44,24 +43,24 @@ class QuinticPolynomial: self.a5 = x[2] def calc_point(self, t): - xt = self.a0 + self.a1 * t + self.a2 * t**2 + \ - self.a3 * t**3 + self.a4 * t**4 + self.a5 * t**5 + xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ + self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5 return xt def calc_first_derivative(self, t): xt = self.a1 + 2 * self.a2 * t + \ - 3 * self.a3 * t**2 + 4 * self.a4 * t**3 + 5 * self.a5 * t**4 + 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4 return xt def calc_second_derivative(self, t): - xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + 20 * self.a5 * t**3 + 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 + xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2 return xt @@ -146,7 +145,7 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_ plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + lambda event: [exit(0) if event.key == 'escape' else None]) plt.grid(True) plt.axis("equal") plot_arrow(sx, sy, syaw)