From 33f4b944129b176c760879de7015f6183bb32950 Mon Sep 17 00:00:00 2001 From: Atsushi Date: Tue, 21 Jan 2020 11:12:15 +0900 Subject: [PATCH] keep DRY --- .../frenet_optimal_trajectory.py | 58 +++++-------------- 1 file changed, 13 insertions(+), 45 deletions(-) diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 1b62ea01..b113bcbe 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -17,6 +17,17 @@ import matplotlib.pyplot as plt import copy import math import cubic_spline_planner +import sys +import os + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../QuinticPolynomialsPlanner/") + +try: + from quintic_polynomials_planner import QuinticPolynomial +except ImportError: + raise + SIM_LOOP = 500 @@ -44,50 +55,6 @@ KLON = 1.0 show_animation = True -class quintic_polynomial: - - def __init__(self, xs, vxs, axs, xe, vxe, axe, T): - - # 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, - axe - 2 * self.a2]) - x = np.linalg.solve(A, b) - - self.a3 = x[0] - self.a4 = x[1] - 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 - - 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 - - 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 - - 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: def __init__(self, xs, vxs, axs, vxe, axe, T): @@ -164,7 +131,8 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): for Ti in np.arange(MINT, MAXT, DT): fp = Frenet_path() - lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) + # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) + lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) fp.t = [t for t in np.arange(0.0, Ti, DT)] fp.d = [lat_qp.calc_point(t) for t in fp.t]