From 0de06534d22e22d84b4fe65ad973525e16e312c0 Mon Sep 17 00:00:00 2001 From: Yilun Chen Date: Fri, 17 Jan 2020 19:17:28 -0500 Subject: [PATCH 1/6] cleanup in dynamic window approach --- .../DynamicWindowApproach/dynamic_window_approach.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 92fd08bb..9e9d34d7 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 @@ -77,9 +77,9 @@ def motion(x, u, dt): motion model """ - x[2] += u[1] * dt x[0] += u[0] * math.cos(x[2]) * dt x[1] += u[0] * math.sin(x[2]) * dt + x[2] += u[1] * dt x[3] = u[0] x[4] = u[1] @@ -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 """ From dc612e6f2000a67b5542cc48303bf0b505350926 Mon Sep 17 00:00:00 2001 From: Yilun Chen Date: Sun, 19 Jan 2020 11:25:52 -0500 Subject: [PATCH 2/6] minor fix in dijkstra.py --- .gitignore | 1 + PathPlanning/Dijkstra/dijkstra.py | 18 ++++++++---------- 2 files changed, 9 insertions(+), 10 deletions(-) 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..ee88b3f9 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 @@ -242,6 +237,9 @@ def main(): dijkstra = Dijkstra(ox, oy, grid_size, robot_radius) rx, ry = dijkstra.planning(sx, sy, gx, gy) + print(rx) + print(ry) + if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") plt.show() From f261da52f0301041829dc20d8845372cf98a9a73 Mon Sep 17 00:00:00 2001 From: Yilun Chen Date: Sun, 19 Jan 2020 11:29:05 -0500 Subject: [PATCH 3/6] remove print --- PathPlanning/Dijkstra/dijkstra.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index ee88b3f9..bb1fb3aa 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -237,9 +237,6 @@ def main(): dijkstra = Dijkstra(ox, oy, grid_size, robot_radius) rx, ry = dijkstra.planning(sx, sy, gx, gy) - print(rx) - print(ry) - if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") plt.show() From 1c081ed3e0ccfbc20214a505b2fbd925427e0fa1 Mon Sep 17 00:00:00 2001 From: Yilun Chen Date: Sun, 19 Jan 2020 18:35:52 -0500 Subject: [PATCH 4/6] address the comment --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 9e9d34d7..b8395e5f 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -77,9 +77,9 @@ def motion(x, u, dt): motion model """ + x[2] += u[1] * dt x[0] += u[0] * math.cos(x[2]) * dt x[1] += u[0] * math.sin(x[2]) * dt - x[2] += u[1] * dt x[3] = u[0] x[4] = u[1] From cd65de271462b9cc50a130c708f1d2c0e005e953 Mon Sep 17 00:00:00 2001 From: Atsushi Date: Tue, 21 Jan 2020 10:58:02 +0900 Subject: [PATCH 5/6] add math descriptions --- .../quintic_polynomials_planner.ipynb | 119 ++++++++++++++---- 1 file changed, 93 insertions(+), 26 deletions(-) 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 +} From 33f4b944129b176c760879de7015f6183bb32950 Mon Sep 17 00:00:00 2001 From: Atsushi Date: Tue, 21 Jan 2020 11:12:15 +0900 Subject: [PATCH 6/6] 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]