diff --git a/.gitignore b/.gitignore index 272dc63b..c971b8f9 100644 --- a/.gitignore +++ b/.gitignore @@ -70,3 +70,4 @@ target/ .ipynb_checkpoints matplotrecorder/* +.vscode/settings.json diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index 62da6d76..bb1fb3aa 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -33,7 +33,7 @@ class Dijkstra: self.x = x # index of grid self.y = y # index of grid self.cost = cost - self.pind = pind + self.pind = pind # index of previous Node def __str__(self): return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) @@ -88,10 +88,10 @@ class Dijkstra: closedset[c_id] = current # expand search grid based on motion model - for i, _ in enumerate(self.motion): - node = self.Node(current.x + self.motion[i][0], - current.y + self.motion[i][1], - current.cost + self.motion[i][2], c_id) + for move_x, move_y, move_cost in self.motion: + node = self.Node(current.x + move_x, + current.y + move_y, + current.cost + move_cost, c_id) n_id = self.calc_index(node) if n_id in closedset: @@ -124,11 +124,6 @@ class Dijkstra: return rx, ry - def calc_heuristic(self, n1, n2): - w = 1.0 # weight of heuristic - d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) - return d - def calc_position(self, index, minp): pos = index*self.reso+minp return pos diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 92fd08bb..b8395e5f 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -22,7 +22,7 @@ def dwa_control(x, config, goal, ob): dw = calc_dynamic_window(x, config) - u, trajectory = calc_final_input(x, dw, config, goal, ob) + u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob) return u, trajectory @@ -101,7 +101,7 @@ def calc_dynamic_window(x, config): x[4] - config.max_dyawrate * config.dt, x[4] + config.max_dyawrate * config.dt] - # [vmin,vmax, yaw_rate min, yaw_rate max] + # [vmin, vmax, yaw_rate min, yaw_rate max] dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]), max(Vs[2], Vd[2]), min(Vs[3], Vd[3])] @@ -124,7 +124,7 @@ def predict_trajectory(x_init, v, y, config): return traj -def calc_final_input(x, dw, config, goal, ob): +def calc_control_and_trajectory(x, dw, config, goal, ob): """ calculation final input with dynamic window """ 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] diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb index 804b7fba..f094ae2c 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb @@ -2,45 +2,112 @@ "cells": [ { "cell_type": "markdown", + "metadata": {}, "source": [ "# Quintic polynomials planner" - ], - "metadata": { - "collapsed": false - } + ] }, { "cell_type": "markdown", + "metadata": {}, "source": [ "## Quintic polynomials for one dimensional robot motion\n", "\n", - "We assume a dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n", + "We assume a one-dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n", "\n", - "$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4$\n", + "$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(1)\n", "\n", - "a_0, a_1. a_2, a_3 are parameters of the quintic polynomial.\n", + "$a_0, a_1. a_2, a_3, a_4, a_5$ are parameters of the quintic polynomial.\n", + "\n", + "It is assumed that terminal states (start and end) are known as boundary conditions.\n", + "\n", + "Start position, velocity, and acceleration are $x_s, v_s, a_s$ respectively.\n", + "\n", + "End position, velocity, and acceleration are $x_e, v_e, a_e$ respectively.\n", + "\n", + "So, when time is 0.\n", + "\n", + "$x(0) = a_0 = x_s$ -- (2)\n", + "\n", + "Then, differentiating the equation (1) with t, \n", + "\n", + "$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4$ -- (3)\n", "\n", "So, when time is 0,\n", "\n", - "$x(0) = a_0 = x_s$\n", + "$x'(0) = a_1 = v_s$ -- (4)\n", "\n", - "$x_s$ is a start x position.\n", + "Then, differentiating the equation (3) with t again, \n", "\n", - "Then, differentiating this equation with t, \n", - "\n", - "$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3$\n", + "$x''(t) = 2a_2+6a_3t+12a_4t^2$ -- (5)\n", "\n", "So, when time is 0,\n", "\n", - "$x'(0) = a_1 = v_s$\n", + "$x''(0) = 2a_2 = a_s$ -- (6)\n", "\n", - "$v_s$ is a initial speed for x axis.\n", + "so, we can calculate $a_0$, $a_1$, $a_2$ with eq. (2), (4), (6) and boundary conditions.\n", "\n", - "=== TBD ==== \n" - ], - "metadata": { - "collapsed": false - } + "$a_3, a_4, a_5$ are still unknown in eq(1).\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We assume that the end time for a maneuver is $T$, we can get these equations from eq (1), (3), (5):\n", + "\n", + "$x(T)=a_0+a_1T+a_2T^2+a_3T^3+a_4T^4+a_5T^5=x_e$ -- (7)\n", + "\n", + "$x'(T)=a_1+2a_2T+3a_3T^2+4a_4T^3+5a_5T^4=v_e$ -- (8)\n", + "\n", + "$x''(T)=2a_2+6a_3T+12a_4T^2+20a_5T^3=a_e$ -- (9)\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "From eq (7), (8), (9), we can calculate $a_3, a_4, a_5$ to solve the linear equations.\n", + "\n", + "$Ax=b$\n", + "\n", + "$\\begin{bmatrix} T^3 & T^4 & T^5 \\\\ 3T^2 & 4T^3 & 5T^4 \\\\ 6T & 12T^2 & 20T^3 \\end{bmatrix}\n", + "\\begin{bmatrix} a_3\\\\ a_4\\\\ a_5\\end{bmatrix}=\\begin{bmatrix} x_e-x_s-v_sT-0.5a_sT^2\\\\ v_e-v_s-a_sT\\\\ a_e-a_s\\end{bmatrix}$\n", + "\n", + "We can get all unknown parameters now" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Quintic polynomials for two dimensional robot motion (x-y)\n", + "\n", + "If you use two quintic polynomials along x axis and y axis, you can plan for two dimensional robot motion in x-y plane.\n", + "\n", + "$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(10)\n", + "\n", + "$y(t) = b_0+b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5$ --(11)\n", + "\n", + "It is assumed that terminal states (start and end) are known as boundary conditions.\n", + "\n", + "Start position, orientation, velocity, and acceleration are $x_s, y_s, \\theta_s, v_s, a_s$ respectively.\n", + "\n", + "End position, orientation, velocity, and acceleration are $x_e, y_e. \\theta_e, v_e, a_e$ respectively.\n", + "\n", + "Each velocity and acceleration boundary condition can be calculated with each orientation.\n", + "\n", + "$v_{xs}=v_scos(\\theta_s), v_{ys}=v_ssin(\\theta_s)$\n", + "\n", + "$v_{xe}=v_ecos(\\theta_e), v_{ye}=v_esin(\\theta_e)$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": { @@ -52,25 +119,25 @@ "language_info": { "codemirror_mode": { "name": "ipython", - "version": 2 + "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", - "pygments_lexer": "ipython2", - "version": "2.7.6" + "pygments_lexer": "ipython3", + "version": "3.7.5" }, "pycharm": { "stem_cell": { "cell_type": "raw", - "source": [], "metadata": { "collapsed": false - } + }, + "source": [] } } }, "nbformat": 4, - "nbformat_minor": 0 -} \ No newline at end of file + "nbformat_minor": 1 +}