Merge remote-tracking branch 'origin/master'

Author: AtsushiSakai

.gitignore

@@ -70,3 +70,4 @@ target/
 .ipynb_checkpoints
 
 matplotrecorder/*
+.vscode/settings.json

PathPlanning/Dijkstra/dijkstra.py

@@ -33,7 +33,7 @@ def __init__(self, x, y, cost, pind):
             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 @@ def planning(self, sx, sy, gx, gy):
             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 @@ def calc_final_path(self, ngoal, closedset):
 
         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

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
     """

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py

@@ -17,6 +17,17 @@
 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 @@
 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]

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",
-    "So, when time is 0,\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",
-    "$x(0) = a_0 = x_s$\n",
+    "End position, velocity, and acceleration are $x_e, v_e, a_e$ respectively.\n",
     "\n",
-    "$x_s$ is a start x position.\n",
+    "So, when time is 0.\n",
     "\n",
-    "Then, differentiating this equation with t, \n",
+    "$x(0) = a_0 = x_s$ -- (2)\n",
     "\n",
-    "$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3$\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_1 = v_s$\n",
+    "$x'(0) = a_1 = v_s$ -- (4)\n",
     "\n",
-    "$v_s$ is a initial speed for x axis.\n",
+    "Then, differentiating the equation (3) with t again, \n",
     "\n",
-    "=== TBD ==== \n"
-   ],
-   "metadata": {
-    "collapsed": false
-   }
+    "$x''(t) = 2a_2+6a_3t+12a_4t^2$ -- (5)\n",
+    "\n",
+    "So, when time is 0,\n",
+    "\n",
+    "$x''(0) = 2a_2 = a_s$ -- (6)\n",
+    "\n",
+    "so, we can calculate $a_0$, $a_1$, $a_2$ with eq. (2), (4), (6) and boundary conditions.\n",
+    "\n",
+    "$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
-}
+ "nbformat_minor": 1
+}