+"""
+
+Dubins path planner sample code
+
+author Atsushi Sakai(@Atsushi_twi)
+
+"""
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
+from math import sin, cos, atan2, sqrt, acos, pi, hypot
+import numpy as np
+from utils.angle import angle_mod, rot_mat_2d
+
+show_animation = True
+
+
+[docs]def plan_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature,
+
step_size=0.1, selected_types=None):
+
"""
+
Plan dubins path
+
+
Parameters
+
----------
+
s_x : float
+
x position of the start point [m]
+
s_y : float
+
y position of the start point [m]
+
s_yaw : float
+
yaw angle of the start point [rad]
+
g_x : float
+
x position of the goal point [m]
+
g_y : float
+
y position of the end point [m]
+
g_yaw : float
+
yaw angle of the end point [rad]
+
curvature : float
+
curvature for curve [1/m]
+
step_size : float (optional)
+
step size between two path points [m]. Default is 0.1
+
selected_types : a list of string or None
+
selected path planning types. If None, all types are used for
+
path planning, and minimum path length result is returned.
+
You can select used path plannings types by a string list.
+
e.g.: ["RSL", "RSR"]
+
+
Returns
+
-------
+
x_list: array
+
x positions of the path
+
y_list: array
+
y positions of the path
+
yaw_list: array
+
yaw angles of the path
+
modes: array
+
mode list of the path
+
lengths: array
+
arrow_length list of the path segments.
+
+
Examples
+
--------
+
You can generate a dubins path.
+
+
>>> start_x = 1.0 # [m]
+
>>> start_y = 1.0 # [m]
+
>>> start_yaw = np.deg2rad(45.0) # [rad]
+
>>> end_x = -3.0 # [m]
+
>>> end_y = -3.0 # [m]
+
>>> end_yaw = np.deg2rad(-45.0) # [rad]
+
>>> curvature = 1.0
+
>>> path_x, path_y, path_yaw, mode, _ = plan_dubins_path(
+
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
+
>>> plt.plot(path_x, path_y, label="final course " + "".join(mode))
+
>>> plot_arrow(start_x, start_y, start_yaw)
+
>>> plot_arrow(end_x, end_y, end_yaw)
+
>>> plt.legend()
+
>>> plt.grid(True)
+
>>> plt.axis("equal")
+
>>> plt.show()
+
+
.. image:: dubins_path.jpg
+
"""
+
if selected_types is None:
+
planning_funcs = _PATH_TYPE_MAP.values()
+
else:
+
planning_funcs = [_PATH_TYPE_MAP[ptype] for ptype in selected_types]
+
+
# calculate local goal x, y, yaw
+
l_rot = rot_mat_2d(s_yaw)
+
le_xy = np.stack([g_x - s_x, g_y - s_y]).T @ l_rot
+
local_goal_x = le_xy[0]
+
local_goal_y = le_xy[1]
+
local_goal_yaw = g_yaw - s_yaw
+
+
lp_x, lp_y, lp_yaw, modes, lengths = _dubins_path_planning_from_origin(
+
local_goal_x, local_goal_y, local_goal_yaw, curvature, step_size,
+
planning_funcs)
+
+
# Convert a local coordinate path to the global coordinate
+
rot = rot_mat_2d(-s_yaw)
+
converted_xy = np.stack([lp_x, lp_y]).T @ rot
+
x_list = converted_xy[:, 0] + s_x
+
y_list = converted_xy[:, 1] + s_y
+
yaw_list = angle_mod(np.array(lp_yaw) + s_yaw)
+
+
return x_list, y_list, yaw_list, modes, lengths
+
+
+def _mod2pi(theta):
+ return angle_mod(theta, zero_2_2pi=True)
+
+
+def _calc_trig_funcs(alpha, beta):
+ sin_a = sin(alpha)
+ sin_b = sin(beta)
+ cos_a = cos(alpha)
+ cos_b = cos(beta)
+ cos_ab = cos(alpha - beta)
+ return sin_a, sin_b, cos_a, cos_b, cos_ab
+
+
+def _LSL(alpha, beta, d):
+ sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta)
+ mode = ["L", "S", "L"]
+ p_squared = 2 + d ** 2 - (2 * cos_ab) + (2 * d * (sin_a - sin_b))
+ if p_squared < 0: # invalid configuration
+ return None, None, None, mode
+ tmp = atan2((cos_b - cos_a), d + sin_a - sin_b)
+ d1 = _mod2pi(-alpha + tmp)
+ d2 = sqrt(p_squared)
+ d3 = _mod2pi(beta - tmp)
+ return d1, d2, d3, mode
+
+
+def _RSR(alpha, beta, d):
+ sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta)
+ mode = ["R", "S", "R"]
+ p_squared = 2 + d ** 2 - (2 * cos_ab) + (2 * d * (sin_b - sin_a))
+ if p_squared < 0:
+ return None, None, None, mode
+ tmp = atan2((cos_a - cos_b), d - sin_a + sin_b)
+ d1 = _mod2pi(alpha - tmp)
+ d2 = sqrt(p_squared)
+ d3 = _mod2pi(-beta + tmp)
+ return d1, d2, d3, mode
+
+
+def _LSR(alpha, beta, d):
+ sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta)
+ p_squared = -2 + d ** 2 + (2 * cos_ab) + (2 * d * (sin_a + sin_b))
+ mode = ["L", "S", "R"]
+ if p_squared < 0:
+ return None, None, None, mode
+ d1 = sqrt(p_squared)
+ tmp = atan2((-cos_a - cos_b), (d + sin_a + sin_b)) - atan2(-2.0, d1)
+ d2 = _mod2pi(-alpha + tmp)
+ d3 = _mod2pi(-_mod2pi(beta) + tmp)
+ return d2, d1, d3, mode
+
+
+def _RSL(alpha, beta, d):
+ sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta)
+ p_squared = d ** 2 - 2 + (2 * cos_ab) - (2 * d * (sin_a + sin_b))
+ mode = ["R", "S", "L"]
+ if p_squared < 0:
+ return None, None, None, mode
+ d1 = sqrt(p_squared)
+ tmp = atan2((cos_a + cos_b), (d - sin_a - sin_b)) - atan2(2.0, d1)
+ d2 = _mod2pi(alpha - tmp)
+ d3 = _mod2pi(beta - tmp)
+ return d2, d1, d3, mode
+
+
+def _RLR(alpha, beta, d):
+ sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta)
+ mode = ["R", "L", "R"]
+ tmp = (6.0 - d ** 2 + 2.0 * cos_ab + 2.0 * d * (sin_a - sin_b)) / 8.0
+ if abs(tmp) > 1.0:
+ return None, None, None, mode
+ d2 = _mod2pi(2 * pi - acos(tmp))
+ d1 = _mod2pi(alpha - atan2(cos_a - cos_b, d - sin_a + sin_b) + d2 / 2.0)
+ d3 = _mod2pi(alpha - beta - d1 + d2)
+ return d1, d2, d3, mode
+
+
+def _LRL(alpha, beta, d):
+ sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta)
+ mode = ["L", "R", "L"]
+ tmp = (6.0 - d ** 2 + 2.0 * cos_ab + 2.0 * d * (- sin_a + sin_b)) / 8.0
+ if abs(tmp) > 1.0:
+ return None, None, None, mode
+ d2 = _mod2pi(2 * pi - acos(tmp))
+ d1 = _mod2pi(-alpha - atan2(cos_a - cos_b, d + sin_a - sin_b) + d2 / 2.0)
+ d3 = _mod2pi(_mod2pi(beta) - alpha - d1 + _mod2pi(d2))
+ return d1, d2, d3, mode
+
+
+_PATH_TYPE_MAP = {"LSL": _LSL, "RSR": _RSR, "LSR": _LSR, "RSL": _RSL,
+ "RLR": _RLR, "LRL": _LRL, }
+
+
+def _dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
+ step_size, planning_funcs):
+ dx = end_x
+ dy = end_y
+ d = hypot(dx, dy) * curvature
+
+ theta = _mod2pi(atan2(dy, dx))
+ alpha = _mod2pi(-theta)
+ beta = _mod2pi(end_yaw - theta)
+
+ best_cost = float("inf")
+ b_d1, b_d2, b_d3, b_mode = None, None, None, None
+
+ for planner in planning_funcs:
+ d1, d2, d3, mode = planner(alpha, beta, d)
+ if d1 is None:
+ continue
+
+ cost = (abs(d1) + abs(d2) + abs(d3))
+ if best_cost > cost: # Select minimum length one.
+ b_d1, b_d2, b_d3, b_mode, best_cost = d1, d2, d3, mode, cost
+
+ lengths = [b_d1, b_d2, b_d3]
+ x_list, y_list, yaw_list = _generate_local_course(lengths, b_mode,
+ curvature, step_size)
+
+ lengths = [length / curvature for length in lengths]
+
+ return x_list, y_list, yaw_list, b_mode, lengths
+
+
+def _interpolate(length, mode, max_curvature, origin_x, origin_y,
+ origin_yaw, path_x, path_y, path_yaw):
+ if mode == "S":
+ path_x.append(origin_x + length / max_curvature * cos(origin_yaw))
+ path_y.append(origin_y + length / max_curvature * sin(origin_yaw))
+ path_yaw.append(origin_yaw)
+ else: # curve
+ ldx = sin(length) / max_curvature
+ ldy = 0.0
+ if mode == "L": # left turn
+ ldy = (1.0 - cos(length)) / max_curvature
+ elif mode == "R": # right turn
+ ldy = (1.0 - cos(length)) / -max_curvature
+ gdx = cos(-origin_yaw) * ldx + sin(-origin_yaw) * ldy
+ gdy = -sin(-origin_yaw) * ldx + cos(-origin_yaw) * ldy
+ path_x.append(origin_x + gdx)
+ path_y.append(origin_y + gdy)
+
+ if mode == "L": # left turn
+ path_yaw.append(origin_yaw + length)
+ elif mode == "R": # right turn
+ path_yaw.append(origin_yaw - length)
+
+ return path_x, path_y, path_yaw
+
+
+def _generate_local_course(lengths, modes, max_curvature, step_size):
+ p_x, p_y, p_yaw = [0.0], [0.0], [0.0]
+
+ for (mode, length) in zip(modes, lengths):
+ if length == 0.0:
+ continue
+
+ # set origin state
+ origin_x, origin_y, origin_yaw = p_x[-1], p_y[-1], p_yaw[-1]
+
+ current_length = step_size
+ while abs(current_length + step_size) <= abs(length):
+ p_x, p_y, p_yaw = _interpolate(current_length, mode, max_curvature,
+ origin_x, origin_y, origin_yaw,
+ p_x, p_y, p_yaw)
+ current_length += step_size
+
+ p_x, p_y, p_yaw = _interpolate(length, mode, max_curvature, origin_x,
+ origin_y, origin_yaw, p_x, p_y, p_yaw)
+
+ return p_x, p_y, p_yaw
+
+
+def main():
+ print("Dubins path planner sample start!!")
+ import matplotlib.pyplot as plt
+ from utils.plot import plot_arrow
+
+ start_x = 1.0 # [m]
+ start_y = 1.0 # [m]
+ start_yaw = np.deg2rad(45.0) # [rad]
+
+ end_x = -3.0 # [m]
+ end_y = -3.0 # [m]
+ end_yaw = np.deg2rad(-45.0) # [rad]
+
+ curvature = 1.0
+
+ path_x, path_y, path_yaw, mode, lengths = plan_dubins_path(start_x,
+ start_y,
+ start_yaw,
+ end_x,
+ end_y,
+ end_yaw,
+ curvature)
+
+ if show_animation:
+ plt.plot(path_x, path_y, label="".join(mode))
+ plot_arrow(start_x, start_y, start_yaw)
+ plot_arrow(end_x, end_y, end_yaw)
+ plt.legend()
+ plt.grid(True)
+ plt.axis("equal")
+ plt.show()
+
+
+if __name__ == '__main__':
+ main()
+
-
-# PythonRobotics
-
-
-
-[](https://ci.appveyor.com/project/AtsushiSakai/pythonrobotics)
-
-Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index.html) for robotics algorithm.
-
-
-# Table of Contents
- * [What is this?](#what-is-this)
- * [Requirements](#requirements)
- * [Documentation](#documentation)
- * [How to use](#how-to-use)
- * [Localization](#localization)
- * [Extended Kalman Filter localization](#extended-kalman-filter-localization)
- * [Particle filter localization](#particle-filter-localization)
- * [Histogram filter localization](#histogram-filter-localization)
- * [Mapping](#mapping)
- * [Gaussian grid map](#gaussian-grid-map)
- * [Ray casting grid map](#ray-casting-grid-map)
- * [Lidar to grid map](#lidar-to-grid-map)
- * [k-means object clustering](#k-means-object-clustering)
- * [Rectangle fitting](#rectangle-fitting)
- * [SLAM](#slam)
- * [Iterative Closest Point (ICP) Matching](#iterative-closest-point-icp-matching)
- * [FastSLAM 1.0](#fastslam-10)
- * [Path Planning](#path-planning)
- * [Dynamic Window Approach](#dynamic-window-approach)
- * [Grid based search](#grid-based-search)
- * [Dijkstra algorithm](#dijkstra-algorithm)
- * [A* algorithm](#a-algorithm)
- * [D* algorithm](#d-algorithm)
- * [D* Lite algorithm](#d-lite-algorithm)
- * [Potential Field algorithm](#potential-field-algorithm)
- * [Grid based coverage path planning](#grid-based-coverage-path-planning)
- * [State Lattice Planning](#state-lattice-planning)
- * [Biased polar sampling](#biased-polar-sampling)
- * [Lane sampling](#lane-sampling)
- * [Probabilistic Road-Map (PRM) planning](#probabilistic-road-map-prm-planning)
- * [Rapidly-Exploring Random Trees (RRT)](#rapidly-exploring-random-trees-rrt)
- * [RRT*](#rrt)
- * [RRT* with reeds-shepp path](#rrt-with-reeds-shepp-path)
- * [LQR-RRT*](#lqr-rrt)
- * [Quintic polynomials planning](#quintic-polynomials-planning)
- * [Reeds Shepp planning](#reeds-shepp-planning)
- * [LQR based path planning](#lqr-based-path-planning)
- * [Optimal Trajectory in a Frenet Frame](#optimal-trajectory-in-a-frenet-frame)
- * [Path Tracking](#path-tracking)
- * [move to a pose control](#move-to-a-pose-control)
- * [Stanley control](#stanley-control)
- * [Rear wheel feedback control](#rear-wheel-feedback-control)
- * [Linear–quadratic regulator (LQR) speed and steering control](#linearquadratic-regulator-lqr-speed-and-steering-control)
- * [Model predictive speed and steering control](#model-predictive-speed-and-steering-control)
- * [Nonlinear Model predictive control with C-GMRES](#nonlinear-model-predictive-control-with-c-gmres)
- * [Arm Navigation](#arm-navigation)
- * [N joint arm to point control](#n-joint-arm-to-point-control)
- * [Arm navigation with obstacle avoidance](#arm-navigation-with-obstacle-avoidance)
- * [Aerial Navigation](#aerial-navigation)
- * [drone 3d trajectory following](#drone-3d-trajectory-following)
- * [rocket powered landing](#rocket-powered-landing)
- * [Bipedal](#bipedal)
- * [bipedal planner with inverted pendulum](#bipedal-planner-with-inverted-pendulum)
- * [License](#license)
- * [Use-case](#use-case)
- * [Contribution](#contribution)
- * [Citing](#citing)
- * [Support](#support)
- * [Sponsors](#sponsors)
- * [JetBrains](#JetBrains)
- * [1Password](#1password)
- * [Authors](#authors)
-
-# What is PythonRobotics?
-
-PythonRobotics is a Python code collection and a [textbook](https://atsushisakai.github.io/PythonRobotics/index.html) of robotics algorithms.
-
-Features:
-
-1. Easy to read for understanding each algorithm's basic idea.
-
-2. Widely used and practical algorithms are selected.
-
-3. Minimum dependency.
-
-See this documentation
-
-- [Getting Started — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/1_what_is_python_robotics.html)
-
-or this Youtube video:
-
-- [PythonRobotics project audio overview](https://www.youtube.com/watch?v=uMeRnNoJAfU)
-
-or this paper for more details:
-
-- [\[1808\.10703\] PythonRobotics: a Python code collection of robotics algorithms](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib))
-
-
-# Requirements to run the code
-
-For running each sample code:
-
-- [Python 3.13.x](https://www.python.org/)
-
-- [NumPy](https://numpy.org/)
-
-- [SciPy](https://scipy.org/)
-
-- [Matplotlib](https://matplotlib.org/)
-
-- [cvxpy](https://www.cvxpy.org/)
-
-For development:
-
-- [pytest](https://pytest.org/) (for unit tests)
-
-- [pytest-xdist](https://pypi.org/project/pytest-xdist/) (for parallel unit tests)
-
-- [mypy](https://mypy-lang.org/) (for type check)
-
-- [sphinx](https://www.sphinx-doc.org/) (for document generation)
-
-- [pycodestyle](https://pypi.org/project/pycodestyle/) (for code style check)
-
-# Documentation (Textbook)
-
-This README only shows some examples of this project.
-
-If you are interested in other examples or mathematical backgrounds of each algorithm,
-
-You can check the full documentation (textbook) online: [Welcome to PythonRobotics’s documentation\! — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/index.html)
-
-All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation gifs of PythonRobotics](https://github.com/AtsushiSakai/PythonRoboticsGifs)
-
-# How to use
-
-1. Clone this repo.
-
- ```terminal
- git clone https://github.com/AtsushiSakai/PythonRobotics.git
- ```
-
-
-2. Install the required libraries.
-
-- using conda :
-
- ```terminal
- conda env create -f requirements/environment.yml
- ```
-
-- using pip :
-
- ```terminal
- pip install -r requirements/requirements.txt
- ```
-
-
-3. Execute python script in each directory.
-
-4. Add star to this repo if you like it :smiley:.
-
-# Localization
-
-## Extended Kalman Filter localization
-
-
-
-Reference
-
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
-
-## Particle filter localization
-
-
-
-This is a sensor fusion localization with Particle Filter(PF).
-
-The blue line is true trajectory, the black line is dead reckoning trajectory,
-
-and the red line is an estimated trajectory with PF.
-
-It is assumed that the robot can measure a distance from landmarks (RFID).
-
-These measurements are used for PF localization.
-
-Reference
-
-- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
-
-
-## Histogram filter localization
-
-
-
-This is a 2D localization example with Histogram filter.
-
-The red cross is true position, black points are RFID positions.
-
-The blue grid shows a position probability of histogram filter.
-
-In this simulation, x,y are unknown, yaw is known.
-
-The filter integrates speed input and range observations from RFID for localization.
-
-Initial position is not needed.
-
-Reference
-
-- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
-
-# Mapping
-
-## Gaussian grid map
-
-This is a 2D Gaussian grid mapping example.
-
-
-
-## Ray casting grid map
-
-This is a 2D ray casting grid mapping example.
-
-
-
-## Lidar to grid map
-
-This example shows how to convert a 2D range measurement to a grid map.
-
-
-
-## k-means object clustering
-
-This is a 2D object clustering with k-means algorithm.
-
-
-
-## Rectangle fitting
-
-This is a 2D rectangle fitting for vehicle detection.
-
-
-
-
-# SLAM
-
-Simultaneous Localization and Mapping(SLAM) examples
-
-## Iterative Closest Point (ICP) Matching
-
-This is a 2D ICP matching example with singular value decomposition.
-
-It can calculate a rotation matrix, and a translation vector between points and points.
-
-
-
-Reference
-
-- [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
-
-
-## FastSLAM 1.0
-
-This is a feature based SLAM example using FastSLAM 1.0.
-
-The blue line is ground truth, the black line is dead reckoning, the red line is the estimated trajectory with FastSLAM.
-
-The red points are particles of FastSLAM.
-
-Black points are landmarks, blue crosses are estimated landmark positions by FastSLAM.
-
-
-
-
-
-Reference
-
-- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
-
-- [SLAM simulations by Tim Bailey](http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm)
-
-
-# Path Planning
-
-## Dynamic Window Approach
-
-This is a 2D navigation sample code with Dynamic Window Approach.
-
-- [The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf)
-
-
-
-
-## Grid based search
-
-### Dijkstra algorithm
-
-This is a 2D grid based the shortest path planning with Dijkstra's algorithm.
-
-
-
-In the animation, cyan points are searched nodes.
-
-### A\* algorithm
-
-This is a 2D grid based the shortest path planning with A star algorithm.
-
-
-
-In the animation, cyan points are searched nodes.
-
-Its heuristic is 2D Euclid distance.
-
-### D\* algorithm
-
-This is a 2D grid based the shortest path planning with D star algorithm.
-
-
-
-The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm.
-
-Reference
-
-- [D* Algorithm Wikipedia](https://en.wikipedia.org/wiki/D*)
-
-### D\* Lite algorithm
-
-This algorithm finds the shortest path between two points while rerouting when obstacles are discovered. It has been implemented here for a 2D grid.
-
-
-
-The animation shows a robot finding its path and rerouting to avoid obstacles as they are discovered using the D* Lite search algorithm.
-
-Refs:
-
-- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf)
-- [Improved Fast Replanning for Robot Navigation in Unknown Terrain](http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf)
-
-### Potential Field algorithm
-
-This is a 2D grid based path planning with Potential Field algorithm.
-
-
-
-In the animation, the blue heat map shows potential value on each grid.
-
-Reference
-
-- [Robotic Motion Planning:Potential Functions](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf)
-
-### Grid based coverage path planning
-
-This is a 2D grid based coverage path planning simulation.
-
-
-
-## State Lattice Planning
-
-This script is a path planning code with state lattice planning.
-
-This code uses the model predictive trajectory generator to solve boundary problem.
-
-Reference
-
-- [Optimal rough terrain trajectory generation for wheeled mobile robots](https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
-
-- [State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments](https://www.cs.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
-
-
-### Biased polar sampling
-
-
-
-
-### Lane sampling
-
-
-
-## Probabilistic Road-Map (PRM) planning
-
-
-
-This PRM planner uses Dijkstra method for graph search.
-
-In the animation, blue points are sampled points,
-
-Cyan crosses means searched points with Dijkstra method,
-
-The red line is the final path of PRM.
-
-Reference
-
-- [Probabilistic roadmap \- Wikipedia](https://en.wikipedia.org/wiki/Probabilistic_roadmap)
-
-
-
-## Rapidly-Exploring Random Trees (RRT)
-
-### RRT\*
-
-
-
-This is a path planning code with RRT\*
-
-Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.
-
-Reference
-
-- [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416)
-
-- [Sampling-based Algorithms for Optimal Motion Planning](https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf&doi=bddbc99f97173430aa49a0ada53ab5bade5902fa)
-
-### RRT\* with reeds-shepp path
-
-
-
-Path planning for a car robot with RRT\* and reeds shepp path planner.
-
-### LQR-RRT\*
-
-This is a path planning simulation with LQR-RRT\*.
-
-A double integrator motion model is used for LQR local planner.
-
-
-
-Reference
-
-- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](https://lis.csail.mit.edu/pubs/perez-icra12.pdf)
-
-- [MahanFathi/LQR\-RRTstar: LQR\-RRT\* method is used for random motion planning of a simple pendulum in its phase plot](https://github.com/MahanFathi/LQR-RRTstar)
-
-
-## Quintic polynomials planning
-
-Motion planning with quintic polynomials.
-
-
-
-It can calculate a 2D path, velocity, and acceleration profile based on quintic polynomials.
-
-Reference
-
-- [Local Path Planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
-
-## Reeds Shepp planning
-
-A sample code with Reeds Shepp path planning.
-
-
-
-Reference
-
-- [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html)
-
-- [optimal paths for a car that goes both forwards and backwards](https://pdfs.semanticscholar.org/932e/c495b1d0018fd59dee12a0bf74434fac7af4.pdf)
-
-- [ghliu/pyReedsShepp: Implementation of Reeds Shepp curve\.](https://github.com/ghliu/pyReedsShepp)
-
-
-## LQR based path planning
-
-A sample code using LQR based path planning for double integrator model.
-
-
-
-
-## Optimal Trajectory in a Frenet Frame
-
-
-
-This is optimal trajectory generation in a Frenet Frame.
-
-The cyan line is the target course and black crosses are obstacles.
-
-The red line is the predicted path.
-
-Reference
-
-- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
-
-- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame](https://www.youtube.com/watch?v=Cj6tAQe7UCY)
-
-
-# Path Tracking
-
-## move to a pose control
-
-This is a simulation of moving to a pose control
-
-
-
-Reference
-
-- [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
-
-
-## Stanley control
-
-Path tracking simulation with Stanley steering control and PID speed control.
-
-
-
-Reference
-
-- [Stanley: The robot that won the DARPA grand challenge](http://robots.stanford.edu/papers/thrun.stanley05.pdf)
-
-- [Automatic Steering Methods for Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf)
-
-
-
-## Rear wheel feedback control
-
-Path tracking simulation with rear wheel feedback steering control and PID speed control.
-
-
-
-Reference
-
-- [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
-
-
-## Linear–quadratic regulator (LQR) speed and steering control
-
-Path tracking simulation with LQR speed and steering control.
-
-
-
-Reference
-
-- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](https://ieeexplore.ieee.org/document/5940562/)
-
-
-## Model predictive speed and steering control
-
-Path tracking simulation with iterative linear model predictive speed and steering control.
-
-
-
-Reference
-
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)
-
-- [Real\-time Model Predictive Control \(MPC\), ACADO, Python \| Work\-is\-Playing](http://grauonline.de/wordpress/?page_id=3244)
-
-## Nonlinear Model predictive control with C-GMRES
-
-A motion planning and path tracking simulation with NMPC of C-GMRES
-
-
-
-Reference
-
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc.html)
-
-
-# Arm Navigation
-
-## N joint arm to point control
-
-N joint arm to a point control simulation.
-
-This is an interactive simulation.
-
-You can set the goal position of the end effector with left-click on the plotting area.
-
-
-
-In this simulation N = 10, however, you can change it.
-
-## Arm navigation with obstacle avoidance
-
-Arm navigation with obstacle avoidance simulation.
-
-
-
-
-# Aerial Navigation
-
-## drone 3d trajectory following
-
-This is a 3d trajectory following simulation for a quadrotor.
-
-
-
-## rocket powered landing
-
-This is a 3d trajectory generation simulation for a rocket powered landing.
-
-
-
-Reference
-
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
-
-# Bipedal
-
-## bipedal planner with inverted pendulum
-
-This is a bipedal planner for modifying footsteps for an inverted pendulum.
-
-You can set the footsteps, and the planner will modify those automatically.
-
-
-
-# License
-
-MIT
-
-# Use-case
-
-If this project helps your robotics project, please let me know with creating an issue.
-
-Your robot's video, which is using PythonRobotics, is very welcome!!
-
-This is a list of user's comment and references:[users\_comments](https://github.com/AtsushiSakai/PythonRobotics/blob/master/users_comments.md)
-
-# Contribution
-
-Any contribution is welcome!!
-
-Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html)
-
-# Citing
-
-If you use this project's code for your academic work, we encourage you to cite [our papers](https://arxiv.org/abs/1808.10703)
-
-If you use this project's code in industry, we'd love to hear from you as well; feel free to reach out to the developers directly.
-
-#