From 049709fd8b38817842fd1d93e02a1c745c0b8dca Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 27 Nov 2021 22:13:55 +0900 Subject: [PATCH] fix #581 (#582) * update docs * update docs --- SLAM/GraphBasedSLAM/graphSLAM_doc.ipynb | 682 ------------------ docs/how_to_contribute_main.rst | 2 +- .../Figure_1.png | Bin .../Figure_2.png | Bin .../path_planning/bspline_path}/Figure_1.png | Bin .../closed_loop_rrt_star_car}/Figure_1.png | Bin .../closed_loop_rrt_star_car}/Figure_3.png | Bin .../closed_loop_rrt_star_car}/Figure_4.png | Bin .../closed_loop_rrt_star_car}/Figure_5.png | Bin .../path_planning/cubic_spline}/Figure_1.png | Bin .../path_planning/cubic_spline}/Figure_2.png | Bin .../path_planning/cubic_spline}/Figure_3.png | Bin .../lookuptable.png | Bin .../path_planning/path_planning_main.rst | 92 +-- .../quintic_polynomials_planner.rst | 2 +- .../modules/path_planning/rrt}/figure_1.png | Bin .../rrt_star_reeds_shepp}/figure_1.png | Bin .../state_lattice_pathplanner}/Figure_1.png | Bin .../state_lattice_pathplanner}/Figure_2.png | Bin .../state_lattice_pathplanner}/Figure_3.png | Bin .../state_lattice_pathplanner}/Figure_4.png | Bin .../state_lattice_pathplanner}/Figure_5.png | Bin .../state_lattice_pathplanner}/Figure_6.png | Bin .../path_tracking/path_tracking_main.rst | 2 +- 24 files changed, 32 insertions(+), 748 deletions(-) delete mode 100644 SLAM/GraphBasedSLAM/graphSLAM_doc.ipynb rename docs/modules/path_planning/{Bezier_path_planning => bezier_path_planning}/Figure_1.png (100%) rename docs/modules/path_planning/{Bezier_path_planning => bezier_path_planning}/Figure_2.png (100%) rename {PathPlanning/BSplinePath => docs/modules/path_planning/bspline_path}/Figure_1.png (100%) rename {PathPlanning/ClosedLoopRRTStar => docs/modules/path_planning/closed_loop_rrt_star_car}/Figure_1.png (100%) rename {PathPlanning/ClosedLoopRRTStar => docs/modules/path_planning/closed_loop_rrt_star_car}/Figure_3.png (100%) rename {PathPlanning/ClosedLoopRRTStar => docs/modules/path_planning/closed_loop_rrt_star_car}/Figure_4.png (100%) rename {PathPlanning/ClosedLoopRRTStar => docs/modules/path_planning/closed_loop_rrt_star_car}/Figure_5.png (100%) rename {PathPlanning/CubicSpline => docs/modules/path_planning/cubic_spline}/Figure_1.png (100%) rename {PathPlanning/CubicSpline => docs/modules/path_planning/cubic_spline}/Figure_2.png (100%) rename {PathPlanning/CubicSpline => docs/modules/path_planning/cubic_spline}/Figure_3.png (100%) rename {PathPlanning/ModelPredictiveTrajectoryGenerator => docs/modules/path_planning/model_predictive_trajectry_generator}/lookuptable.png (100%) rename {PathPlanning/RRT => docs/modules/path_planning/rrt}/figure_1.png (100%) rename {PathPlanning/RRTStarReedsShepp => docs/modules/path_planning/rrt_star_reeds_shepp}/figure_1.png (100%) rename {PathPlanning/StateLatticePlanner => docs/modules/path_planning/state_lattice_pathplanner}/Figure_1.png (100%) rename {PathPlanning/StateLatticePlanner => docs/modules/path_planning/state_lattice_pathplanner}/Figure_2.png (100%) rename {PathPlanning/StateLatticePlanner => docs/modules/path_planning/state_lattice_pathplanner}/Figure_3.png (100%) rename {PathPlanning/StateLatticePlanner => docs/modules/path_planning/state_lattice_pathplanner}/Figure_4.png (100%) rename {PathPlanning/StateLatticePlanner => docs/modules/path_planning/state_lattice_pathplanner}/Figure_5.png (100%) rename {PathPlanning/StateLatticePlanner => docs/modules/path_planning/state_lattice_pathplanner}/Figure_6.png (100%) diff --git a/SLAM/GraphBasedSLAM/graphSLAM_doc.ipynb b/SLAM/GraphBasedSLAM/graphSLAM_doc.ipynb deleted file mode 100644 index 53364e32..00000000 --- a/SLAM/GraphBasedSLAM/graphSLAM_doc.ipynb +++ /dev/null @@ -1,682 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": 11, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [], - "source": [ - "import copy\n", - "import math\n", - "import itertools\n", - "import numpy as np \n", - "import matplotlib.pyplot as plt\n", - "from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \\\n", - " calc_input, observation, motion_model, Edge, pi_2_pi\n", - "\n", - "%matplotlib inline\n", - "np.set_printoptions(precision=3, suppress=True)\n", - "np.random.seed(0)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Introduction\n", - "\n", - "In contrast to the probabilistic approaches for solving SLAM, such as EKF, UKF, particle filters, and so on, the graph technique formulates the SLAM as an optimization problem. It is mostly used to solve the full SLAM problem in an offline fashion, i.e. optimize all the poses of the robot after the path has been traversed. However, some variants are availble that uses graph-based approaches to perform online estimation or to solve for a subset of the poses.\n", - "\n", - "GraphSLAM uses the motion information as well as the observations of the environment to create least square problem that can be solved using standard optimization techniques.\n", - "\n", - "### Minimal Example\n", - "The following example illustrates the main idea behind graphSLAM. \n", - "A simple case of a 1D robot is considered that can only move in 1 direction. The robot is commanded to move forward with a control input $u_t=1$, however, the motion is not perfect and the measured odometry will deviate from the true path. At each timestep the robot can observe its environment, for this simple case as well, there is only a single landmark at coordinates $x=3$. The measured observations are the range between the robot and landmark. These measurements are also subjected to noise. No bearing information is required since this is a 1D problem.\n", - "\n", - "To solve this, graphSLAM creates what is called as the system information matrix $\\Omega$ also referred to as $H$ and the information vector $\\xi$ also known as $b$. The entries are created based on the information of the motion and the observation." - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [ - { - "data": { - "text/plain": "
", - "image/png": "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\n" - }, - "metadata": { - "needs_background": "light" - }, - "output_type": "display_data" - }, - { - "name": "stdout", - "text": [ - "The determinant of H: 0.0\n", - "The determinant of H after anchoring constraint: 18.750000000000007\n", - "Running graphSLAM ...\n", - "Odometry values after optimzation: \n", - " [[-0. ]\n", - " [ 0.9]\n", - " [ 1.9]]\n" - ], - "output_type": "stream" - }, - { - "data": { - "text/plain": "
", - "image/png": "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\n" - }, - "metadata": { - "needs_background": "light" - }, - "output_type": "display_data" - } - ], - "source": [ - "R = 0.2\n", - "Q = 0.2\n", - "N = 3\n", - "graphics_radius = 0.1\n", - "\n", - "odom = np.empty((N,1))\n", - "obs = np.empty((N,1))\n", - "x_true = np.empty((N,1))\n", - "\n", - "landmark = 3\n", - "# Simulated readings of odometry and observations\n", - "x_true[0], odom[0], obs[0] = 0.0, 0.0, 2.9\n", - "x_true[1], odom[1], obs[1] = 1.0, 1.5, 2.0\n", - "x_true[2], odom[2], obs[2] = 2.0, 2.4, 1.0\n", - "\n", - "hxDR = copy.deepcopy(odom)\n", - "# Visualization\n", - "plt.plot(landmark,0, '*k', markersize=30)\n", - "for i in range(N):\n", - " plt.plot(odom[i], 0, '.', markersize=50, alpha=0.8, color='steelblue')\n", - " plt.plot([odom[i], odom[i] + graphics_radius],\n", - " [0,0], 'r')\n", - " plt.text(odom[i], 0.02, \"X_{}\".format(i), fontsize=12)\n", - " plt.plot(obs[i]+odom[i],0,'.', markersize=25, color='brown')\n", - " plt.plot(x_true[i],0,'.g', markersize=20)\n", - "plt.grid() \n", - "plt.show()\n", - "\n", - "\n", - "# Defined as a function to facilitate iteration\n", - "def get_H_b(odom, obs):\n", - " \"\"\"\n", - " Create the information matrix and information vector. This implementation is \n", - " based on the concept of virtual measurement i.e. the observations of the\n", - " landmarks are converted into constraints (edges) between the nodes that\n", - " have observed this landmark.\n", - " \"\"\"\n", - " measure_constraints = {}\n", - " omegas = {}\n", - " zids = list(itertools.combinations(range(N),2))\n", - " H = np.zeros((N,N))\n", - " b = np.zeros((N,1))\n", - " for (t1, t2) in zids:\n", - " x1 = odom[t1]\n", - " x2 = odom[t2]\n", - " z1 = obs[t1]\n", - " z2 = obs[t2]\n", - " \n", - " # Adding virtual measurement constraint\n", - " measure_constraints[(t1,t2)] = (x2-x1-z1+z2)\n", - " omegas[(t1,t2)] = (1 / (2*Q))\n", - " \n", - " # populate system's information matrix and vector\n", - " H[t1,t1] += omegas[(t1,t2)]\n", - " H[t2,t2] += omegas[(t1,t2)]\n", - " H[t2,t1] -= omegas[(t1,t2)]\n", - " H[t1,t2] -= omegas[(t1,t2)]\n", - "\n", - " b[t1] += omegas[(t1,t2)] * measure_constraints[(t1,t2)]\n", - " b[t2] -= omegas[(t1,t2)] * measure_constraints[(t1,t2)]\n", - "\n", - " return H, b\n", - "\n", - "\n", - "H, b = get_H_b(odom, obs)\n", - "print(\"The determinant of H: \", np.linalg.det(H))\n", - "H[0,0] += 1 # np.inf ?\n", - "print(\"The determinant of H after anchoring constraint: \", np.linalg.det(H))\n", - "\n", - "for i in range(5):\n", - " H, b = get_H_b(odom, obs)\n", - " H[(0,0)] += 1\n", - " \n", - " # Recover the posterior over the path\n", - " dx = np.linalg.inv(H) @ b\n", - " odom += dx\n", - " # repeat till convergence\n", - "print(\"Running graphSLAM ...\") \n", - "print(\"Odometry values after optimzation: \\n\", odom)\n", - "\n", - "plt.figure()\n", - "plt.plot(x_true, np.zeros(x_true.shape), '.', markersize=20, label='Ground truth')\n", - "plt.plot(odom, np.zeros(x_true.shape), '.', markersize=20, label='Estimation')\n", - "plt.plot(hxDR, np.zeros(x_true.shape), '.', markersize=20, label='Odom')\n", - "plt.legend()\n", - "plt.grid()\n", - "plt.show()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "In particular, the tasks are split into 2 parts, graph construction, and graph optimization. \n", - "### Graph Construction\n", - "\n", - "Firstly the nodes are defined $\\boldsymbol{x} = \\boldsymbol{x}_{1:n}$ such that each node is the pose of the robot at time $t_i$\n", - "Secondly, the edges i.e. the constraints, are constructed according to the following conditions:\n", - "\n", - "* robot moves from $\\boldsymbol{x}_i$ to $\\boldsymbol{x}_j$. This edge corresponds to the odometry measurement. Relative motion constraints (Not included in the previous minimal example).\n", - "* Measurement constraints, this can be done in two ways:\n", - " * The information matrix is set in such a way that it includes the landmarks in the map as well. Then the constraints can be entered in a straightforward fashion between a node $\\boldsymbol{x}_i$ and some landmark $m_k$\n", - " * Through creating a virtual measurement among all the node that have observed the same landmark. More concretely, robot observes the same landmark from $\\boldsymbol{x}_i$ and $\\boldsymbol{x}_j$. Relative measurement constraint. The \"virtual measurement\" $\\boldsymbol{z}_{ij}$, which is the estimated pose of $\\boldsymbol{x}_j$ as seen from the node $\\boldsymbol{x}_i$. The virtual measurement can then be entered in the information matrix and vector in a similar fashion to the motion constraints.\n", - "\n", - "An edge is fully characterized by the values of the error (entry to information vector) and the local information matrix (entry to the system's information vector). The larger the local information matrix (lower $Q$ or $R$) the values that this edge will contribute with.\n", - "\n", - "Important Notes:\n", - "\n", - "* The addition to the information matrix and vector are added to the earlier values.\n", - "* In the case of 2D robot, the constraints will be non-linear, therefore, a Jacobian of the error w.r.t the states is needed when updated $H$ and $b$.\n", - "* The anchoring constraint is needed in order to avoid having a singular information matri.\n", - "\n", - "\n", - "### Graph Optimization\n", - "\n", - "The result from this formulation yields an overdetermined system of equations.\n", - "The goal after constructing the graph is to find: $x^* = \\underset{x}{\\mathrm{argmin}} ~ \\underset{ij} \\Sigma ~ f(e_{ij}) $, where $f$ is some error function that depends on the edges between to related nodes $i$ and $j$. The derivation in the references arrive at the solution for $x^* = H^{-1}b$ \n", - "\n", - "\n", - "### Planar Example:\n", - "\n", - "Now we will go through an example with a more realistic case of a 2D robot with 3DoF, namely, $[x, y, \\theta]^T$" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [ - { - "data": { - "text/plain": "
", - "image/png": "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\n" - }, - "metadata": { - "needs_background": "light" - }, - "output_type": "display_data" - } - ], - "source": [ - "# Simulation parameter\n", - "Qsim = np.diag([0.01, np.deg2rad(0.010)])**2 # error added to range and bearing\n", - "Rsim = np.diag([0.1, np.deg2rad(1.0)])**2 # error added to [v, w]\n", - "\n", - "DT = 2.0 # time tick [s]\n", - "SIM_TIME = 100.0 # simulation time [s]\n", - "MAX_RANGE = 30.0 # maximum observation range\n", - "STATE_SIZE = 3 # State size [x,y,yaw]\n", - "\n", - "# TODO: Why not use Qsim ? \n", - "# Covariance parameter of Graph Based SLAM\n", - "C_SIGMA1 = 0.1\n", - "C_SIGMA2 = 0.1\n", - "C_SIGMA3 = np.deg2rad(1.0)\n", - "\n", - "MAX_ITR = 20 # Maximum iteration during optimization\n", - "timesteps = 1\n", - "\n", - "# consider only 2 landmarks for simplicity\n", - "# RFID positions [x, y, yaw]\n", - "RFID = np.array([[10.0, -2.0, 0.0],\n", - "# [15.0, 10.0, 0.0],\n", - "# [3.0, 15.0, 0.0],\n", - "# [-5.0, 20.0, 0.0],\n", - "# [-5.0, 5.0, 0.0]\n", - " ])\n", - "\n", - "# State Vector [x y yaw v]'\n", - "xTrue = np.zeros((STATE_SIZE, 1))\n", - "xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning\n", - "xTrue[2] = np.deg2rad(45)\n", - "xDR[2] = np.deg2rad(45)\n", - "# history initial values\n", - "hxTrue = xTrue\n", - "hxDR = xTrue\n", - "_, z, _, _ = observation(xTrue, xDR, np.array([[0,0]]).T, RFID)\n", - "hz = [z]\n", - "\n", - "for i in range(timesteps):\n", - " u = calc_input()\n", - " xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)\n", - " hxDR = np.hstack((hxDR, xDR))\n", - " hxTrue = np.hstack((hxTrue, xTrue))\n", - " hz.append(z)\n", - "\n", - "# visualize\n", - "graphics_radius = 0.3\n", - "plt.plot(RFID[:, 0], RFID[:, 1], \"*k\", markersize=20)\n", - "plt.plot(hxDR[0, :], hxDR[1, :], '.', markersize=50, alpha=0.8, label='Odom')\n", - "plt.plot(hxTrue[0, :], hxTrue[1, :], '.', markersize=20, alpha=0.6, label='X_true')\n", - "\n", - "for i in range(hxDR.shape[1]):\n", - " x = hxDR[0, i]\n", - " y = hxDR[1, i]\n", - " yaw = hxDR[2, i]\n", - " plt.plot([x, x + graphics_radius * np.cos(yaw)],\n", - " [y, y + graphics_radius * np.sin(yaw)], 'r')\n", - " d = hz[i][:, 0]\n", - " angle = hz[i][:, 1]\n", - " plt.plot([x + d * np.cos(angle + yaw)], [y + d * np.sin(angle + yaw)], '.',\n", - " markersize=20, alpha=0.7)\n", - " plt.legend()\n", - "plt.grid() \n", - "plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [], - "source": [ - "# Copy the data to have a consistent naming with the .py file\n", - "zlist = copy.deepcopy(hz)\n", - "x_opt = copy.deepcopy(hxDR)\n", - "xlist = copy.deepcopy(hxDR)\n", - "number_of_nodes = x_opt.shape[1]\n", - "n = number_of_nodes * STATE_SIZE" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "After the data has been saved, the graph will be constructed by looking at each pair for nodes. The virtual measurement is only created if two nodes have observed the same landmark at different points in time. The next cells are a walk through for a single iteration of graph construction -> optimization -> estimate update. " - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [ - { - "name": "stdout", - "text": [ - "Node combinations: \n", - " [(0, 1)]\n", - "Node 0 observed landmark with ID 0.0\n", - "Node 1 observed landmark with ID 0.0\n" - ], - "output_type": "stream" - } - ], - "source": [ - "# get all the possible combination of the different node\n", - "zids = list(itertools.combinations(range(len(zlist)), 2))\n", - "print(\"Node combinations: \\n\", zids)\n", - "\n", - "for i in range(xlist.shape[1]):\n", - " print(\"Node {} observed landmark with ID {}\".format(i, zlist[i][0, 3]))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "In the following code snippet the error based on the virtual measurement between node 0 and 1 will be created.\n", - "The equations for the error is as follows: \n", - "$e_{ij}^x = x_j + d_j cos(\\psi_j + \\theta_j) - x_i - d_i cos(\\psi_i + \\theta_i) $\n", - "\n", - "$e_{ij}^y = y_j + d_j sin(\\psi_j + \\theta_j) - y_i - d_i sin(\\psi_i + \\theta_i) $\n", - "\n", - "$e_{ij}^x = \\psi_j + \\theta_j - \\psi_i - \\theta_i $\n", - "\n", - "Where $[x_i, y_i, \\psi_i]$ is the pose for node $i$ and similarly for node $j$, $d$ is the measured distance at nodes $i$ and $j$, and $\\theta$ is the measured bearing to the landmark. The difference is visualized with the figure in the next cell.\n", - "\n", - "In case of perfect motion and perfect measurement the error shall be zero since $ x_j + d_j cos(\\psi_j + \\theta_j)$ should equal $x_i + d_i cos(\\psi_i + \\theta_i)$" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [ - { - "name": "stdout", - "text": [ - "0.0 1.427649841628278 -2.0126109674819155 -3.524048014922737\n", - "For nodes (0, 1)\n", - "Added edge with errors: \n", - " [[-0.02 ]\n", - " [-0.084]\n", - " [ 0. ]]\n" - ], - "output_type": "stream" - }, - { - "data": { - "text/plain": "
", - "image/png": "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\n" - }, - "metadata": { - "needs_background": "light" - }, - "output_type": "display_data" - } - ], - "source": [ - "# Initialize edges list\n", - "edges = []\n", - "\n", - "# Go through all the different combinations\n", - "for (t1, t2) in zids:\n", - " x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1]\n", - " x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2]\n", - " \n", - " # All nodes have valid observation with ID=0, therefore, no data association condition\n", - " iz1 = 0\n", - " iz2 = 0\n", - " \n", - " d1 = zlist[t1][iz1, 0]\n", - " angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2]\n", - " d2 = zlist[t2][iz2, 0]\n", - " angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2]\n", - " \n", - " # find angle between observation and horizontal \n", - " tangle1 = pi_2_pi(yaw1 + angle1)\n", - " tangle2 = pi_2_pi(yaw2 + angle2)\n", - " \n", - " # project the observations \n", - " tmp1 = d1 * math.cos(tangle1)\n", - " tmp2 = d2 * math.cos(tangle2)\n", - " tmp3 = d1 * math.sin(tangle1)\n", - " tmp4 = d2 * math.sin(tangle2)\n", - " \n", - " edge = Edge()\n", - " print(y1,y2, tmp3, tmp4)\n", - " # calculate the error of the virtual measurement\n", - " # node 1 as seen from node 2 throught the observations 1,2\n", - " edge.e[0, 0] = x2 - x1 - tmp1 + tmp2\n", - " edge.e[1, 0] = y2 - y1 - tmp3 + tmp4\n", - " edge.e[2, 0] = pi_2_pi(yaw2 - yaw1 - tangle1 + tangle2)\n", - "\n", - " edge.d1, edge.d2 = d1, d2\n", - " edge.yaw1, edge.yaw2 = yaw1, yaw2\n", - " edge.angle1, edge.angle2 = angle1, angle2\n", - " edge.id1, edge.id2 = t1, t2\n", - " \n", - " edges.append(edge)\n", - " \n", - " print(\"For nodes\",(t1,t2))\n", - " print(\"Added edge with errors: \\n\", edge.e)\n", - " \n", - " # Visualize measurement projections\n", - " plt.plot(RFID[0, 0], RFID[0, 1], \"*k\", markersize=20)\n", - " plt.plot([x1,x2],[y1,y2], '.', markersize=50, alpha=0.8)\n", - " plt.plot([x1, x1 + graphics_radius * np.cos(yaw1)],\n", - " [y1, y1 + graphics_radius * np.sin(yaw1)], 'r')\n", - " plt.plot([x2, x2 + graphics_radius * np.cos(yaw2)],\n", - " [y2, y2 + graphics_radius * np.sin(yaw2)], 'r')\n", - " \n", - " plt.plot([x1,x1+tmp1], [y1,y1], label=\"obs 1 x\")\n", - " plt.plot([x2,x2+tmp2], [y2,y2], label=\"obs 2 x\")\n", - " plt.plot([x1,x1], [y1,y1+tmp3], label=\"obs 1 y\")\n", - " plt.plot([x2,x2], [y2,y2+tmp4], label=\"obs 2 y\")\n", - " plt.plot(x1+tmp1, y1+tmp3, 'o')\n", - " plt.plot(x2+tmp2, y2+tmp4, 'o')\n", - "plt.legend()\n", - "plt.grid()\n", - "plt.show()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Since the constraints equations derived before are non-linear, linearization is needed before we can insert them into the information matrix and information vector. Two jacobians \n", - "\n", - "$A = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_i}$ as $\\boldsymbol{x}_i$ holds the three variabls x, y, and theta.\n", - "Similarly, \n", - "$B = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_j}$ " - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [ - { - "name": "stdout", - "text": [ - "The determinant of H: 0.0\n", - "The determinant of H after origin constraint: 716.1972439134893\n" - ], - "output_type": "stream" - }, - { - "data": { - "text/plain": "
", - "image/png": "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\n" - }, - "metadata": { - "needs_background": "light" - }, - "output_type": "display_data" - }, - { - "data": { - "text/plain": "
", - "image/png": "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\n" - }, - "metadata": { - "needs_background": "light" - }, - "output_type": "display_data" - } - ], - "source": [ - "# Initialize the system information matrix and information vector\n", - "H = np.zeros((n, n))\n", - "b = np.zeros((n, 1))\n", - "x_opt = copy.deepcopy(hxDR)\n", - "\n", - "for edge in edges:\n", - " id1 = edge.id1 * STATE_SIZE\n", - " id2 = edge.id2 * STATE_SIZE\n", - " \n", - " t1 = edge.yaw1 + edge.angle1\n", - " A = np.array([[-1.0, 0, edge.d1 * math.sin(t1)],\n", - " [0, -1.0, -edge.d1 * math.cos(t1)],\n", - " [0, 0, -1.0]])\n", - "\n", - " t2 = edge.yaw2 + edge.angle2\n", - " B = np.array([[1.0, 0, -edge.d2 * math.sin(t2)],\n", - " [0, 1.0, edge.d2 * math.cos(t2)],\n", - " [0, 0, 1.0]])\n", - " \n", - " # TODO: use Qsim instead of sigma\n", - " sigma = np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3])\n", - " Rt1 = calc_rotational_matrix(tangle1)\n", - " Rt2 = calc_rotational_matrix(tangle2)\n", - " edge.omega = np.linalg.inv(Rt1 @ sigma @ Rt1.T + Rt2 @ sigma @ Rt2.T)\n", - "\n", - " # Fill in entries in H and b\n", - " H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ A\n", - " H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ B\n", - " H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ A\n", - " H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B\n", - "\n", - " b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e)\n", - " b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e)\n", - " \n", - "\n", - "print(\"The determinant of H: \", np.linalg.det(H))\n", - "plt.figure()\n", - "plt.subplot(1,2,1)\n", - "plt.imshow(H, extent=[0, n, 0, n])\n", - "plt.subplot(1,2,2)\n", - "plt.imshow(b, extent=[0, 1, 0, n])\n", - "plt.show() \n", - "\n", - "# Fix the origin, multiply by large number gives same results but better visualization\n", - "H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)\n", - "print(\"The determinant of H after origin constraint: \", np.linalg.det(H)) \n", - "plt.figure()\n", - "plt.subplot(1,2,1)\n", - "plt.imshow(H, extent=[0, n, 0, n])\n", - "plt.subplot(1,2,2)\n", - "plt.imshow(b, extent=[0, 1, 0, n])\n", - "plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [ - { - "name": "stdout", - "text": [ - "dx: \n", - " [[-0. ]\n", - " [-0. ]\n", - " [ 0. ]\n", - " [ 0.02 ]\n", - " [ 0.084]\n", - " [-0. ]]\n", - "ground truth: \n", - " [[0. 1.414]\n", - " [0. 1.414]\n", - " [0.785 0.985]]\n", - "Odom: \n", - " [[0. 1.428]\n", - " [0. 1.428]\n", - " [0.785 0.976]]\n", - "SLAM: \n", - " [[-0. 1.448]\n", - " [-0. 1.512]\n", - " [ 0.785 0.976]]\n", - "\n", - "graphSLAM localization error: 0.010729072751057656\n", - "Odom localization error: 0.0004460978857535104\n" - ], - "output_type": "stream" - } - ], - "source": [ - "# Find the solution (first iteration)\n", - "dx = - np.linalg.inv(H) @ b\n", - "for i in range(number_of_nodes):\n", - " x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]\n", - "print(\"dx: \\n\",dx)\n", - "print(\"ground truth: \\n \",hxTrue)\n", - "print(\"Odom: \\n\", hxDR)\n", - "print(\"SLAM: \\n\", x_opt)\n", - "\n", - "# performance will improve with more iterations, nodes and landmarks.\n", - "print(\"\\ngraphSLAM localization error: \", np.sum((x_opt - hxTrue) ** 2))\n", - "print(\"Odom localization error: \", np.sum((hxDR - hxTrue) ** 2))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### The references:\n", - "\n", - "\n", - "* http://robots.stanford.edu/papers/thrun.graphslam.pdf\n", - "\n", - "* http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/16-graph-slam.pdf\n", - "\n", - "* http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf\n", - "\n", - "N.B.\n", - "An additional step is required that uses the estimated path to update the belief regarding the map." - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": { - "pycharm": { - "is_executing": false - } - }, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.5" - }, - "pycharm": { - "stem_cell": { - "cell_type": "raw", - "source": [], - "metadata": { - "collapsed": false - } - } - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} \ No newline at end of file diff --git a/docs/how_to_contribute_main.rst b/docs/how_to_contribute_main.rst index c761fd99..a4ac3c81 100644 --- a/docs/how_to_contribute_main.rst +++ b/docs/how_to_contribute_main.rst @@ -1,4 +1,4 @@ -How to contribute +How To Contribute ================= This document describes how to contribute this project. diff --git a/docs/modules/path_planning/Bezier_path_planning/Figure_1.png b/docs/modules/path_planning/bezier_path_planning/Figure_1.png similarity index 100% rename from docs/modules/path_planning/Bezier_path_planning/Figure_1.png rename to docs/modules/path_planning/bezier_path_planning/Figure_1.png diff --git a/docs/modules/path_planning/Bezier_path_planning/Figure_2.png b/docs/modules/path_planning/bezier_path_planning/Figure_2.png similarity index 100% rename from docs/modules/path_planning/Bezier_path_planning/Figure_2.png rename to docs/modules/path_planning/bezier_path_planning/Figure_2.png diff --git a/PathPlanning/BSplinePath/Figure_1.png b/docs/modules/path_planning/bspline_path/Figure_1.png similarity index 100% rename from PathPlanning/BSplinePath/Figure_1.png rename to docs/modules/path_planning/bspline_path/Figure_1.png diff --git a/PathPlanning/ClosedLoopRRTStar/Figure_1.png b/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png similarity index 100% rename from PathPlanning/ClosedLoopRRTStar/Figure_1.png rename to docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png diff --git a/PathPlanning/ClosedLoopRRTStar/Figure_3.png b/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png similarity index 100% rename from PathPlanning/ClosedLoopRRTStar/Figure_3.png rename to docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png diff --git a/PathPlanning/ClosedLoopRRTStar/Figure_4.png b/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png similarity index 100% rename from PathPlanning/ClosedLoopRRTStar/Figure_4.png rename to docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png diff --git a/PathPlanning/ClosedLoopRRTStar/Figure_5.png b/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png similarity index 100% rename from PathPlanning/ClosedLoopRRTStar/Figure_5.png rename to docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png diff --git a/PathPlanning/CubicSpline/Figure_1.png b/docs/modules/path_planning/cubic_spline/Figure_1.png similarity index 100% rename from PathPlanning/CubicSpline/Figure_1.png rename to docs/modules/path_planning/cubic_spline/Figure_1.png diff --git a/PathPlanning/CubicSpline/Figure_2.png b/docs/modules/path_planning/cubic_spline/Figure_2.png similarity index 100% rename from PathPlanning/CubicSpline/Figure_2.png rename to docs/modules/path_planning/cubic_spline/Figure_2.png diff --git a/PathPlanning/CubicSpline/Figure_3.png b/docs/modules/path_planning/cubic_spline/Figure_3.png similarity index 100% rename from PathPlanning/CubicSpline/Figure_3.png rename to docs/modules/path_planning/cubic_spline/Figure_3.png diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png b/docs/modules/path_planning/model_predictive_trajectry_generator/lookuptable.png similarity index 100% rename from PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png rename to docs/modules/path_planning/model_predictive_trajectry_generator/lookuptable.png diff --git a/docs/modules/path_planning/path_planning_main.rst b/docs/modules/path_planning/path_planning_main.rst index d284c4e0..8033e1a8 100644 --- a/docs/modules/path_planning/path_planning_main.rst +++ b/docs/modules/path_planning/path_planning_main.rst @@ -11,7 +11,7 @@ This is a 2D navigation sample code with Dynamic Window Approach. - `The Dynamic Window Approach to Collision Avoidance `__ -|DWA| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif Grid based search ----------------- @@ -22,7 +22,7 @@ Dijkstra algorithm This is a 2D grid based shortest path planning with Dijkstra's algorithm. -|Dijkstra| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/Dijkstra/animation.gif In the animation, cyan points are searched nodes. @@ -33,7 +33,7 @@ A\* algorithm This is a 2D grid based shortest path planning with A star algorithm. -|astar| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/AStar/animation.gif In the animation, cyan points are searched nodes. @@ -46,7 +46,7 @@ D\* algorithm This is a 2D grid based shortest path planning with D star algorithm. -|dstar| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStar/animation.gif The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm. @@ -59,7 +59,7 @@ Potential Field algorithm This is a 2D grid based path planning with Potential Field algorithm. -|PotentialField| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/PotentialFieldPlanning/animation.gif In the animation, the blue heat map shows potential value on each grid. @@ -79,12 +79,12 @@ This algorithm is used for state lattice planner. Path optimization sample ~~~~~~~~~~~~~~~~~~~~~~~~ -|4| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/kn05animation.gif Lookup table generation sample ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -|5| +.. image:: model_predictive_trajectry_generator/lookuptable.png Ref: @@ -92,8 +92,6 @@ Ref: robots `__ - - State Lattice Planning ---------------------- @@ -114,24 +112,24 @@ Ref: Uniform polar sampling ~~~~~~~~~~~~~~~~~~~~~~ -|6| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif Biased polar sampling ~~~~~~~~~~~~~~~~~~~~~ -|7| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/BiasedPolarSampling.gif Lane sampling ~~~~~~~~~~~~~ -|8| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif .. _probabilistic-road-map-(prm)-planning: Probabilistic Road-Map (PRM) planning ------------------------------------- -|PRM| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ProbabilisticRoadMap/animation.gif This PRM planner uses Dijkstra method for graph search. @@ -151,7 +149,7 @@ Ref: Voronoi Road-Map planning ------------------------- -|VRM| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/VoronoiRoadMap/animation.gif This Voronoi road-map planner uses Dijkstra method for graph search. @@ -174,7 +172,7 @@ Rapidly-Exploring Random Trees (RRT) Basic RRT ~~~~~~~~~ -|9| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRT/animation.gif This is a simple path planning code with Rapidly-Exploring Random Trees (RRT) @@ -188,7 +186,7 @@ are start and goal positions. RRT with dubins path ~~~~~~~~~~~~~~~~~~~~ -|PythonRobotics| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTDubins/animation.gif Path planning for a car robot with RRT and dubins path planner. @@ -197,7 +195,7 @@ Path planning for a car robot with RRT and dubins path planner. RRT\* with dubins path ~~~~~~~~~~~~~~~~~~~~~~ -|AtsushiSakai/PythonRobotics| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTStarDubins/animation.gif Path planning for a car robot with RRT\* and dubins path planner. @@ -206,7 +204,7 @@ Path planning for a car robot with RRT\* and dubins path planner. RRT\* with reeds-sheep path ~~~~~~~~~~~~~~~~~~~~~~~~~~~ -|11| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTStarReedsShepp/animation.gif Path planning for a car robot with RRT\* and reeds sheep path planner. @@ -215,7 +213,7 @@ Path planning for a car robot with RRT\* and reeds sheep path planner. Informed RRT\* ~~~~~~~~~~~~~~ -|irrt| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/InformedRRTStar/animation.gif This is a path planning code with Informed RRT*. @@ -232,7 +230,7 @@ Ref: Batch Informed RRT\* ~~~~~~~~~~~~~~~~~~~~ -|irrt2| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BatchInformedRRTStar/animation.gif This is a path planning code with Batch Informed RRT*. @@ -249,7 +247,7 @@ Closed Loop RRT\* A vehicle model based path planning with closed loop RRT*. -|CLRRT| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClosedLoopRRTStar/animation.gif In this code, pure-pursuit algorithm is used for steering control, @@ -275,7 +273,7 @@ This is a path planning simulation with LQR-RRT*. A double integrator motion model is used for LQR local planner. -|LQRRRT| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif Ref: @@ -297,14 +295,14 @@ with cubic spline. Heading angle of each point can be also calculated analytically. -|12| -|13| -|14| +.. image:: cubic_spline/Figure_1.png +.. image:: cubic_spline/Figure_2.png +.. image:: cubic_spline/Figure_3.png B-Spline planning ----------------- -|B-Spline| +.. image:: bspline_path/Figure_1.png This is a path planning with B-Spline curse. @@ -321,7 +319,7 @@ Ref: Eta^3 Spline path planning -------------------------- -|eta3| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/Eta3SplinePath/animation.gif This is a path planning with Eta^3 spline. @@ -360,7 +358,7 @@ Dubins path planning A sample code for Dubins path planning. -|dubins| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DubinsPath/animation.gif?raw=True Ref: @@ -372,7 +370,7 @@ Reeds Shepp planning A sample code with Reeds Shepp path planning. -|RSPlanning| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true Ref: @@ -390,12 +388,12 @@ LQR based path planning A sample code using LQR based path planning for double integrator model. -|RSPlanning2| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true Optimal Trajectory in a Frenet Frame ------------------------------------ -|15| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif This is optimal trajectory generation in a Frenet Frame. @@ -412,35 +410,3 @@ Ref: - `Optimal trajectory generation for dynamic street scenarios in a Frenet Frame `__ -.. |DWA| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif -.. |Dijkstra| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/Dijkstra/animation.gif -.. |astar| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/AStar/animation.gif -.. |dstar| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStar/animation.gif -.. |PotentialField| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/PotentialFieldPlanning/animation.gif -.. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/kn05animation.gif -.. |5| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png?raw=True -.. |6| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif -.. |7| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/BiasedPolarSampling.gif -.. |8| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif -.. |PRM| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ProbabilisticRoadMap/animation.gif -.. |VRM| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/VoronoiRoadMap/animation.gif -.. |9| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRT/animation.gif -.. |10| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTstar/animation.gif -.. |PythonRobotics| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTDubins/animation.gif -.. |AtsushiSakai/PythonRobotics| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTStarDubins/animation.gif -.. |11| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTStarReedsShepp/animation.gif -.. |irrt| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/InformedRRTStar/animation.gif -.. |irrt2| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BatchInformedRRTStar/animation.gif -.. |CLRRT| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClosedLoopRRTStar/animation.gif -.. |LQRRRT| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif -.. |12| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_1.png?raw=True -.. |13| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_2.png?raw=True -.. |14| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_3.png?raw=True -.. |B-Spline| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BSplinePath/Figure_1.png?raw=True -.. |eta3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/Eta3SplinePath/animation.gif -.. |Bezier1| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BezierPath/Figure_1.png?raw=True -.. |2| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/QuinticPolynomialsPlanner/animation.gif -.. |dubins| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DubinsPath/animation.gif?raw=True -.. |RSPlanning| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true -.. |RSPlanning2| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true -.. |15| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif diff --git a/docs/modules/path_planning/quintic_polynomials_planner.rst b/docs/modules/path_planning/quintic_polynomials_planner.rst index 2fab8a51..2bced070 100644 --- a/docs/modules/path_planning/quintic_polynomials_planner.rst +++ b/docs/modules/path_planning/quintic_polynomials_planner.rst @@ -4,7 +4,7 @@ Quintic polynomials planning Motion planning with quintic polynomials. -|2| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/QuinticPolynomialsPlanner/animation.gif It can calculate 2D path, velocity, and acceleration profile based on quintic polynomials. diff --git a/PathPlanning/RRT/figure_1.png b/docs/modules/path_planning/rrt/figure_1.png similarity index 100% rename from PathPlanning/RRT/figure_1.png rename to docs/modules/path_planning/rrt/figure_1.png diff --git a/PathPlanning/RRTStarReedsShepp/figure_1.png b/docs/modules/path_planning/rrt_star_reeds_shepp/figure_1.png similarity index 100% rename from PathPlanning/RRTStarReedsShepp/figure_1.png rename to docs/modules/path_planning/rrt_star_reeds_shepp/figure_1.png diff --git a/PathPlanning/StateLatticePlanner/Figure_1.png b/docs/modules/path_planning/state_lattice_pathplanner/Figure_1.png similarity index 100% rename from PathPlanning/StateLatticePlanner/Figure_1.png rename to docs/modules/path_planning/state_lattice_pathplanner/Figure_1.png diff --git a/PathPlanning/StateLatticePlanner/Figure_2.png b/docs/modules/path_planning/state_lattice_pathplanner/Figure_2.png similarity index 100% rename from PathPlanning/StateLatticePlanner/Figure_2.png rename to docs/modules/path_planning/state_lattice_pathplanner/Figure_2.png diff --git a/PathPlanning/StateLatticePlanner/Figure_3.png b/docs/modules/path_planning/state_lattice_pathplanner/Figure_3.png similarity index 100% rename from PathPlanning/StateLatticePlanner/Figure_3.png rename to docs/modules/path_planning/state_lattice_pathplanner/Figure_3.png diff --git a/PathPlanning/StateLatticePlanner/Figure_4.png b/docs/modules/path_planning/state_lattice_pathplanner/Figure_4.png similarity index 100% rename from PathPlanning/StateLatticePlanner/Figure_4.png rename to docs/modules/path_planning/state_lattice_pathplanner/Figure_4.png diff --git a/PathPlanning/StateLatticePlanner/Figure_5.png b/docs/modules/path_planning/state_lattice_pathplanner/Figure_5.png similarity index 100% rename from PathPlanning/StateLatticePlanner/Figure_5.png rename to docs/modules/path_planning/state_lattice_pathplanner/Figure_5.png diff --git a/PathPlanning/StateLatticePlanner/Figure_6.png b/docs/modules/path_planning/state_lattice_pathplanner/Figure_6.png similarity index 100% rename from PathPlanning/StateLatticePlanner/Figure_6.png rename to docs/modules/path_planning/state_lattice_pathplanner/Figure_6.png diff --git a/docs/modules/path_tracking/path_tracking_main.rst b/docs/modules/path_tracking/path_tracking_main.rst index 3f86da0b..725e4f04 100644 --- a/docs/modules/path_tracking/path_tracking_main.rst +++ b/docs/modules/path_tracking/path_tracking_main.rst @@ -1,6 +1,6 @@ .. _path_tracking: -Path tracking +Path Tracking ============= move to a pose control