diff --git a/.circleci/config.yml b/.circleci/config.yml
index 075a613be0..f6eff674de 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -6,7 +6,7 @@ orbs:
jobs:
build_doc:
docker:
- - image: cimg/python:3.10
+ - image: cimg/python:3.13
steps:
- checkout
- run:
diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml
index 24f0926299..0d943696cc 100644
--- a/.github/FUNDING.yml
+++ b/.github/FUNDING.yml
@@ -1,4 +1,4 @@
# These are supported funding model platforms
github: AtsushiSakai
patreon: myenigma
-custom: https://www.paypal.me/myenigmapay/
+custom: https://www.paypal.com/paypalme/myenigmapay/
diff --git a/.github/dependabot.yml b/.github/dependabot.yml
index 07f1bc80e0..6ffedf6f3b 100644
--- a/.github/dependabot.yml
+++ b/.github/dependabot.yml
@@ -6,3 +6,8 @@ updates:
interval: weekly
time: "20:00"
open-pull-requests-limit: 10
+
+- package-ecosystem: github-actions
+ directory: "/"
+ schedule:
+ interval: weekly
diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md
index c8790ad9ec..c0d9f7eab2 100644
--- a/.github/pull_request_template.md
+++ b/.github/pull_request_template.md
@@ -1,7 +1,7 @@
#### CheckList
--[] Did you add an unittest for your new example or defect fix?
--[] Did you add documents for your new example?
--[] All CIs are green? (You can check it after submitting)
+- [ ] Did you add an unittest for your new example or defect fix?
+- [ ] Did you add documents for your new example?
+- [ ] All CIs are green? (You can check it after submitting)
diff --git a/.github/workflows/Linux_CI.yml b/.github/workflows/Linux_CI.yml
index 73f8aa5c0d..152a3b0682 100644
--- a/.github/workflows/Linux_CI.yml
+++ b/.github/workflows/Linux_CI.yml
@@ -12,16 +12,16 @@ jobs:
runs-on: ubuntu-latest
strategy:
matrix:
- python-version: ['3.10']
+ python-version: [ '3.13' ]
name: Python ${{ matrix.python-version }} CI
steps:
- - uses: actions/checkout@v2
+ - uses: actions/checkout@v5
- run: git fetch --prune --unshallow
- name: Setup python
- uses: actions/setup-python@v2
+ uses: actions/setup-python@v6
with:
python-version: ${{ matrix.python-version }}
- name: Install dependencies
diff --git a/.github/workflows/MacOS_CI.yml b/.github/workflows/MacOS_CI.yml
index 5c796120a5..ab04dc01dc 100644
--- a/.github/workflows/MacOS_CI.yml
+++ b/.github/workflows/MacOS_CI.yml
@@ -16,17 +16,17 @@ jobs:
runs-on: macos-latest
strategy:
matrix:
- python-version: [ '3.10' ]
+ python-version: [ '3.13' ]
name: Python ${{ matrix.python-version }} CI
steps:
- - uses: actions/checkout@v2
+ - uses: actions/checkout@v5
- run: git fetch --prune --unshallow
- name: Update bash
run: brew install bash
- name: Setup python
- uses: actions/setup-python@v2
+ uses: actions/setup-python@v6
with:
python-version: ${{ matrix.python-version }}
@@ -36,4 +36,4 @@ jobs:
python -m pip install --upgrade pip
pip install -r requirements/requirements.txt
- name: do all unit tests
- run: bash runtests.sh
\ No newline at end of file
+ run: bash runtests.sh
diff --git a/.github/workflows/Windows_CI.yml b/.github/workflows/Windows_CI.yml
new file mode 100644
index 0000000000..a4385e595b
--- /dev/null
+++ b/.github/workflows/Windows_CI.yml
@@ -0,0 +1,36 @@
+# This is a basic workflow to help you get started with Actions
+
+name: Windows_CI
+
+# Controls when the action will run. Triggers the workflow on push or pull request
+# events but only for the master branch
+on:
+ push:
+ branches:
+ - master
+ pull_request:
+
+
+jobs:
+ build:
+ runs-on: windows-latest
+ strategy:
+ matrix:
+ python-version: [ '3.13' ]
+ name: Python ${{ matrix.python-version }} CI
+ steps:
+ - uses: actions/checkout@v5
+ - run: git fetch --prune --unshallow
+
+ - name: Setup python
+ uses: actions/setup-python@v6
+ with:
+ python-version: ${{ matrix.python-version }}
+
+ - name: Install dependencies
+ run: |
+ python --version
+ python -m pip install --upgrade pip
+ pip install -r requirements/requirements.txt
+ - name: do all unit tests
+ run: bash runtests.sh
diff --git a/.github/workflows/circle_artifacts.yml b/.github/workflows/circle_artifacts.yml
deleted file mode 100644
index 56ac78ed15..0000000000
--- a/.github/workflows/circle_artifacts.yml
+++ /dev/null
@@ -1,13 +0,0 @@
-on: [status]
-jobs:
- circleci_artifacts_redirector_job:
- runs-on: ubuntu-20.04
- name: Run CircleCI artifacts redirector
- steps:
- - name: GitHub Action step
- uses: larsoner/circleci-artifacts-redirector-action@master
- with:
- repo-token: ${{ secrets.GITHUB_TOKEN }}
- artifact-path: 0/html-scipyorg/index.html
- circleci-jobs: build_docs
- job-title: build_doc artifact
\ No newline at end of file
diff --git a/.github/workflows/circleci-artifacts-redirector.yml b/.github/workflows/circleci-artifacts-redirector.yml
index d988d2c38e..0e64bab96c 100644
--- a/.github/workflows/circleci-artifacts-redirector.yml
+++ b/.github/workflows/circleci-artifacts-redirector.yml
@@ -3,11 +3,12 @@ on: [status]
jobs:
circleci_artifacts_redirector_job:
runs-on: ubuntu-latest
- name: Run CircleCI artifacts redirector
+ name: Run CircleCI artifacts redirector!!
steps:
- name: run-circleci-artifacts-redirector
- uses: larsoner/circleci-artifacts-redirector-action@0.3.1
+ uses: larsoner/circleci-artifacts-redirector-action@v1.3.1
with:
repo-token: ${{ secrets.GITHUB_TOKEN }}
+ api-token: ${{ secrets.CIRCLECI_TOKEN }}
artifact-path: 0/html/index.html
circleci-jobs: build_doc
diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml
index 148260a835..0ed1d7e90d 100644
--- a/.github/workflows/codeql.yml
+++ b/.github/workflows/codeql.yml
@@ -16,7 +16,7 @@ jobs:
steps:
- name: Checkout repository
- uses: actions/checkout@v2
+ uses: actions/checkout@v5
with:
# We must fetch at least the immediate parents so that if this is
# a pull request then we can checkout the head.
@@ -24,7 +24,7 @@ jobs:
# Initializes the CodeQL tools for scanning.
- name: Initialize CodeQL
- uses: github/codeql-action/init@v1
+ uses: github/codeql-action/init@v3
with:
config-file: ./.github/codeql/codeql-config.yml
# Override language selection by uncommenting this and choosing your languages
@@ -34,7 +34,7 @@ jobs:
# Autobuild attempts to build any compiled languages (C/C++, C#, or Java).
# If this step fails, then you should remove it and run the build manually (see below)
- name: Autobuild
- uses: github/codeql-action/autobuild@v1
+ uses: github/codeql-action/autobuild@v3
# ℹ️ Command-line programs to run using the OS shell.
# 📚 https://git.io/JvXDl
@@ -48,4 +48,4 @@ jobs:
# make release
- name: Perform CodeQL Analysis
- uses: github/codeql-action/analyze@v1
+ uses: github/codeql-action/analyze@v3
diff --git a/.github/workflows/gh-pages.yml b/.github/workflows/gh-pages.yml
index e51197cb37..84165b9cd2 100644
--- a/.github/workflows/gh-pages.yml
+++ b/.github/workflows/gh-pages.yml
@@ -1,4 +1,4 @@
-name: Pages
+name: GitHub Pages site update
on:
push:
branches:
@@ -6,9 +6,15 @@ on:
jobs:
build:
runs-on: ubuntu-latest
+ environment:
+ name: github-pages
+ url: ${{ steps.deployment.outputs.page_url }}
+ permissions:
+ id-token: write
+ pages: write
steps:
- name: Setup python
- uses: actions/setup-python@v2
+ uses: actions/setup-python@v6
- name: Checkout
uses: actions/checkout@master
with:
@@ -18,12 +24,7 @@ jobs:
python --version
python -m pip install --upgrade pip
python -m pip install -r requirements/requirements.txt
- - name: Build and Commit
- uses: sphinx-notes/pages@v2
+ - name: Build and Deploy
+ uses: sphinx-notes/pages@v3
with:
- requirements_path: ./docs/doc_requirements.txt
- - name: Push changes
- uses: ad-m/github-push-action@master
- with:
- github_token: ${{ secrets.GITHUB_TOKEN }}
- branch: gh-pages
\ No newline at end of file
+ requirements_path: ./docs/doc_requirements.txt
diff --git a/.lgtm.yml b/.lgtm.yml
deleted file mode 100644
index b06edf3510..0000000000
--- a/.lgtm.yml
+++ /dev/null
@@ -1,4 +0,0 @@
-extraction:
- python:
- python_setup:
- version: 3
diff --git a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py
index 413a8625a5..c379e5eda0 100644
--- a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py
+++ b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py
@@ -56,7 +56,7 @@ def transformation_matrix(self):
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch)
* sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
- [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
+ [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(roll), z]
])
def plot(self): # pragma: no cover
diff --git a/AerialNavigation/drone_3d_trajectory_following/__init__.py b/AerialNavigation/drone_3d_trajectory_following/__init__.py
index 087dab646e..2194d4c303 100644
--- a/AerialNavigation/drone_3d_trajectory_following/__init__.py
+++ b/AerialNavigation/drone_3d_trajectory_following/__init__.py
@@ -1,4 +1,3 @@
-import os
import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent))
diff --git a/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py b/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py
index e00b3fff48..029e82be62 100644
--- a/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py
+++ b/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py
@@ -8,7 +8,6 @@
import numpy as np
from Quadrotor import Quadrotor
from TrajectoryGenerator import TrajectoryGenerator
-from mpl_toolkits.mplot3d import Axes3D
show_animation = True
@@ -128,7 +127,7 @@ def calculate_position(c, t):
Calculates a position given a set of quintic coefficients and a time.
Args
- c: List of coefficients generated by a quintic polynomial
+ c: List of coefficients generated by a quintic polynomial
trajectory generator.
t: Time at which to calculate the position
@@ -143,7 +142,7 @@ def calculate_velocity(c, t):
Calculates a velocity given a set of quintic coefficients and a time.
Args
- c: List of coefficients generated by a quintic polynomial
+ c: List of coefficients generated by a quintic polynomial
trajectory generator.
t: Time at which to calculate the velocity
@@ -158,7 +157,7 @@ def calculate_acceleration(c, t):
Calculates an acceleration given a set of quintic coefficients and a time.
Args
- c: List of coefficients generated by a quintic polynomial
+ c: List of coefficients generated by a quintic polynomial
trajectory generator.
t: Time at which to calculate the acceleration
@@ -168,7 +167,7 @@ def calculate_acceleration(c, t):
return 20 * c[0] * t**3 + 12 * c[1] * t**2 + 6 * c[2] * t + 2 * c[3]
-def rotation_matrix(roll, pitch, yaw):
+def rotation_matrix(roll_array, pitch_array, yaw):
"""
Calculates the ZYX rotation matrix.
@@ -180,6 +179,8 @@ def rotation_matrix(roll, pitch, yaw):
Returns
3x3 rotation matrix as NumPy array
"""
+ roll = roll_array[0]
+ pitch = pitch_array[0]
return np.array(
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll)],
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
@@ -190,7 +191,7 @@ def rotation_matrix(roll, pitch, yaw):
def main():
"""
- Calculates the x, y, z coefficients for the four segments
+ Calculates the x, y, z coefficients for the four segments
of the trajectory
"""
x_coeffs = [[], [], [], []]
diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
index 37dca15b98..e8ba8fa220 100644
--- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
+++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
@@ -5,20 +5,19 @@
author: Sven Niederberger
Atsushi Sakai
-Ref:
+Reference:
- Python implementation of 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper
by Michael Szmuk and Behcet Acıkmese.
- EmbersArc/SuccessiveConvexificationFreeFinalTime: Implementation of "Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time" https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime
"""
-
+import warnings
from time import time
import numpy as np
from scipy.integrate import odeint
import cvxpy
import matplotlib.pyplot as plt
-from mpl_toolkits import mplot3d
# Trajectory points
K = 50
@@ -32,6 +31,7 @@
W_DELTA_SIGMA = 1e-1 # difference in flight time
W_NU = 1e5 # virtual control
+print(cvxpy.installed_solvers())
solver = 'ECOS'
verbose_solver = False
@@ -123,7 +123,7 @@ def set_random_initial_state(self, rng):
rng.uniform(-20, 20)))
def f_func(self, x, u):
- m, rx, ry, rz, vx, vy, vz, q0, q1, q2, q3, wx, wy, wz = x[0], x[1], x[
+ m, _, _, _, vx, vy, vz, q0, q1, q2, q3, wx, wy, wz = x[0], x[1], x[
2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
ux, uy, uz = u[0], u[1], u[2]
@@ -148,7 +148,7 @@ def f_func(self, x, u):
])
def A_func(self, x, u):
- m, rx, ry, rz, vx, vy, vz, q0, q1, q2, q3, wx, wy, wz = x[0], x[1], x[
+ m, _, _, _, _, _, _, q0, q1, q2, q3, wx, wy, wz = x[0], x[1], x[
2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
ux, uy, uz = u[0], u[1], u[2]
@@ -176,7 +176,7 @@ def A_func(self, x, u):
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
def B_func(self, x, u):
- m, rx, ry, rz, vx, vy, vz, q0, q1, q2, q3, wx, wy, wz = x[0], x[1], x[
+ m, _, _, _, _, _, _, q0, q1, q2, q3, _, _, _ = x[0], x[1], x[
2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
ux, uy, uz = u[0], u[1], u[2]
@@ -463,11 +463,11 @@ def __init__(self, m, K):
# x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu
constraints += [
self.var['X'][:, k + 1] ==
- cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) @
+ cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x), order='F') @
self.var['X'][:, k] +
- cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) @
+ cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u), order='F') @
self.var['U'][:, k] +
- cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) @
+ cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u), order='F') @
self.var['U'][:, k + 1] +
self.par['S_bar'][:, k] * self.var['sigma'] +
self.par['z_bar'][:, k] +
@@ -534,8 +534,10 @@ def get_variable(self, name):
def solve(self, **kwargs):
error = False
try:
- self.prob.solve(verbose=verbose_solver,
- solver=solver)
+ with warnings.catch_warnings(): # For User warning from solver
+ warnings.simplefilter('ignore')
+ self.prob.solve(verbose=verbose_solver,
+ solver=solver)
except cvxpy.SolverError:
error = True
@@ -569,10 +571,10 @@ def axis3d_equal(X, Y, Z, ax):
def plot_animation(X, U): # pragma: no cover
fig = plt.figure()
- ax = fig.gca(projection='3d')
+ ax = fig.add_subplot(projection='3d')
# for stopping simulation with the esc key.
fig.canvas.mpl_connect('key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ lambda event: [exit(0) if event.key == 'escape' else None])
for k in range(K):
plt.cla()
diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
index 75b1f9d2c4..9047c13851 100644
--- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
+++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
@@ -34,7 +34,7 @@ def detect_collision(line_seg, circle):
"""
Determines whether a line segment (arm link) is in contact
with a circle (obstacle).
- Credit to: http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
+ Credit to: https://web.archive.org/web/20200130224918/http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
Args:
line_seg: List of coordinates of line segment endpoints e.g. [[1, 1], [2, 2]]
circle: List of circle coordinates and radius e.g. [0, 0, 0.5] is a circle centered
@@ -158,7 +158,7 @@ def astar_torus(grid, start_node, goal_node):
while parent_map[route[0][0]][route[0][1]] != ():
route.insert(0, parent_map[route[0][0]][route[0][1]])
- print("The route found covers %d grid cells." % len(route))
+ print(f"The route found covers {len(route)} grid cells.")
for i in range(1, len(route)):
grid[route[i]] = 6
plt.cla()
@@ -203,16 +203,16 @@ def calc_heuristic_map(M, goal_node):
for i in range(heuristic_map.shape[0]):
for j in range(heuristic_map.shape[1]):
heuristic_map[i, j] = min(heuristic_map[i, j],
- i + 1 + heuristic_map[M - 1, j],
- M - i + heuristic_map[0, j],
- j + 1 + heuristic_map[i, M - 1],
- M - j + heuristic_map[i, 0]
+ M - i - 1 + heuristic_map[M - 1, j],
+ i + heuristic_map[0, j],
+ M - j - 1 + heuristic_map[i, M - 1],
+ j + heuristic_map[i, 0]
)
return heuristic_map
-class NLinkArm(object):
+class NLinkArm:
"""
Class for controlling and plotting a planar arm with an arbitrary number of links.
"""
diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
index 40e6746bfc..f5d435082a 100644
--- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
+++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
@@ -34,7 +34,7 @@ def main():
goal = (58, 56)
grid = get_occupancy_grid(arm, obstacles)
route = astar_torus(grid, start, goal)
- if len(route) >= 0:
+ if route:
animate(grid, arm, route)
@@ -66,7 +66,7 @@ def detect_collision(line_seg, circle):
"""
Determines whether a line segment (arm link) is in contact
with a circle (obstacle).
- Credit to: http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
+ Credit to: https://web.archive.org/web/20200130224918/http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
Args:
line_seg: List of coordinates of line segment endpoints e.g. [[1, 1], [2, 2]]
circle: List of circle coordinates and radius e.g. [0, 0, 0.5] is a circle centered
@@ -105,7 +105,7 @@ def get_occupancy_grid(arm, obstacles):
Args:
arm: An instance of NLinkArm
obstacles: A list of obstacles, with each obstacle defined as a list
- of xy coordinates and a radius.
+ of xy coordinates and a radius.
Returns:
Occupancy grid in joint space
@@ -189,7 +189,7 @@ def astar_torus(grid, start_node, goal_node):
while parent_map[route[0][0]][route[0][1]] != ():
route.insert(0, parent_map[route[0][0]][route[0][1]])
- print("The route found covers %d grid cells." % len(route))
+ print(f"The route found covers {len(route)} grid cells.")
for i in range(1, len(route)):
grid[route[i]] = 6
plt.cla()
@@ -234,16 +234,16 @@ def calc_heuristic_map(M, goal_node):
for i in range(heuristic_map.shape[0]):
for j in range(heuristic_map.shape[1]):
heuristic_map[i, j] = min(heuristic_map[i, j],
- i + 1 + heuristic_map[M - 1, j],
- M - i + heuristic_map[0, j],
- j + 1 + heuristic_map[i, M - 1],
- M - j + heuristic_map[i, 0]
+ M - i - 1 + heuristic_map[M - 1, j],
+ i + heuristic_map[0, j],
+ M - j - 1 + heuristic_map[i, M - 1],
+ j + heuristic_map[i, 0]
)
return heuristic_map
-class NLinkArm(object):
+class NLinkArm:
"""
Class for controlling and plotting a planar arm with an arbitrary number of links.
"""
diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py b/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py
index 0cea963168..0459e234b2 100644
--- a/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py
@@ -127,7 +127,8 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
if plot:
self.fig = plt.figure()
- self.ax = Axes3D(self.fig)
+ self.ax = Axes3D(self.fig, auto_add_to_figure=False)
+ self.fig.add_axes(self.ax)
x_list = []
y_list = []
diff --git a/ArmNavigation/n_joint_arm_3d/__init__.py b/ArmNavigation/n_joint_arm_3d/__init__.py
index ba5dac65f6..2194d4c303 100644
--- a/ArmNavigation/n_joint_arm_3d/__init__.py
+++ b/ArmNavigation/n_joint_arm_3d/__init__.py
@@ -1,3 +1,3 @@
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent))
diff --git a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
index 36093df08d..a237523336 100644
--- a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
+++ b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
@@ -5,12 +5,13 @@
Atsushi Sakai (@Atsushi_twi)
"""
-import numpy as np
import sys
from pathlib import Path
sys.path.append(str(Path(__file__).parent.parent.parent))
+import numpy as np
from ArmNavigation.n_joint_arm_to_point_control.NLinkArm import NLinkArm
+from utils.angle import angle_mod
# Simulation parameters
Kp = 2
@@ -159,9 +160,9 @@ def ang_diff(theta1, theta2):
"""
Returns the difference between two angles in the range -pi to +pi
"""
- return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
+ return angle_mod(theta1 - theta2)
if __name__ == '__main__':
# main()
- animation()
\ No newline at end of file
+ animation()
diff --git a/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py b/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py
index 0de4c7edb8..3bc2a5ec1d 100755
--- a/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py
+++ b/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py
@@ -3,18 +3,14 @@
Author: Mahyar Abdeetedal (mahyaret)
"""
import math
-import os
-import sys
import random
import numpy as np
-from mpl_toolkits import mplot3d
import matplotlib.pyplot as plt
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../n_joint_arm_3d/")
-try:
- from NLinkArm3d import NLinkArm
-except ImportError:
- raise
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
+
+from n_joint_arm_3d.NLinkArm3d import NLinkArm
show_animation = True
verbose = False
@@ -82,8 +78,9 @@ def __init__(self, start, goal, robot, obstacle_list, rand_area,
self.obstacle_list = obstacle_list
self.connect_circle_dist = connect_circle_dist
self.goal_node = self.Node(goal)
- self.ax = plt.axes(projection='3d')
self.node_list = []
+ if show_animation:
+ self.ax = plt.axes(projection='3d')
def planning(self, animation=False, search_until_max_iter=False):
"""
@@ -179,7 +176,7 @@ def search_best_goal_node(self):
def find_near_nodes(self, new_node):
nnode = len(self.node_list) + 1
- r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
+ r = self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode)
# if expand_dist exists, search vertices in
# a range no more than expand_dist
if hasattr(self, 'expand_dis'):
@@ -268,7 +265,7 @@ def steer(self, from_node, to_node, extend_length=float("inf")):
def draw_graph(self, rnd=None):
plt.cla()
- self.ax.axis([-1, 1, -1, 1])
+ self.ax.axis([-1, 1, -1, 1, -1, 1])
self.ax.set_zlim(0, 1)
self.ax.grid(True)
for (ox, oy, oz, size) in self.obstacle_list:
@@ -378,9 +375,9 @@ def main():
search_until_max_iter=False)
if path is None:
- print("Cannot find path")
+ print("Cannot find path.")
else:
- print("found path!!")
+ print("Found path!")
# Draw final path
if show_animation:
@@ -399,7 +396,7 @@ def main():
[y for y in y_points],
[z for z in z_points],
"o-", color="grey", ms=4, mew=0.5)
- plt.pause(0.01)
+ plt.pause(0.1)
plt.show()
diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
index 9f835c8b32..09969c30be 100644
--- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
+++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
@@ -5,7 +5,7 @@
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)
-Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
+Reference: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
ISBN 978-3-319-54413-7 p102
- [Robotics, Vision and Control]
(https://link.springer.com/book/10.1007/978-3-642-20144-8)
@@ -14,9 +14,11 @@
import matplotlib.pyplot as plt
import numpy as np
+import math
+from utils.angle import angle_mod
-# Similation parameters
+# Simulation parameters
Kp = 15
dt = 0.01
@@ -45,20 +47,20 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
if x is not None and y is not None:
x_prev = x
y_prev = y
- if np.sqrt(x**2 + y**2) > (l1 + l2):
+ if np.hypot(x, y) > (l1 + l2):
theta2_goal = 0
else:
theta2_goal = np.arccos(
(x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
- tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+ tmp = math.atan2(l2 * np.sin(theta2_goal),
(l1 + l2 * np.cos(theta2_goal)))
- theta1_goal = np.math.atan2(y, x) - tmp
+ theta1_goal = math.atan2(y, x) - tmp
if theta1_goal < 0:
theta2_goal = -theta2_goal
- tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+ tmp = math.atan2(l2 * np.sin(theta2_goal),
(l1 + l2 * np.cos(theta2_goal)))
- theta1_goal = np.math.atan2(y, x) - tmp
+ theta1_goal = math.atan2(y, x) - tmp
theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt
theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
@@ -109,7 +111,7 @@ def plot_arm(theta1, theta2, target_x, target_y): # pragma: no cover
def ang_diff(theta1, theta2):
# Returns the difference between two angles in the range -pi to +pi
- return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
+ return angle_mod(theta1 - theta2)
def click(event): # pragma: no cover
diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py
index 9757fc42d6..c34357df67 100644
--- a/Bipedal/bipedal_planner/bipedal_planner.py
+++ b/Bipedal/bipedal_planner/bipedal_planner.py
@@ -47,10 +47,10 @@ def walk(self, t_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
return
# set up plotter
- com_trajectory_for_plot, ax = None, None
if plot:
fig = plt.figure()
ax = Axes3D(fig)
+ fig.add_axes(ax)
com_trajectory_for_plot = []
px, py = 0.0, 0.0 # reference footstep position
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 91f6dfa822..3bcc499e6a 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -1,23 +1,5 @@
-# Contributing to Python
+# Contributing
-:+1::tada: First of off, thanks very much for taking the time to contribute! :tada::+1:
+:+1::tada: First of all, thank you very much for taking the time to contribute! :tada::+1:
-The following is a set of guidelines for contributing to PythonRobotics.
-
-These are mostly guidelines, not rules.
-
-Use your best judgment, and feel free to propose changes to this document in a pull request.
-
-# Before contributing
-
-## Taking a look at the paper.
-
-Please check this paper to understand the philosophy of this project.
-
-- [\[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))
-
-## Check your Python version.
-
-We only accept a PR for Python 3.8.x or higher.
-
-We will not accept a PR for Python 2.x.
+Please check this document for contribution: [How to contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html)
diff --git a/Control/inverted_pendulum/inverted_pendulum_lqr_control.py b/InvertedPendulum/inverted_pendulum_lqr_control.py
similarity index 96%
rename from Control/inverted_pendulum/inverted_pendulum_lqr_control.py
rename to InvertedPendulum/inverted_pendulum_lqr_control.py
index fb68c91c45..c4380530b8 100644
--- a/Control/inverted_pendulum/inverted_pendulum_lqr_control.py
+++ b/InvertedPendulum/inverted_pendulum_lqr_control.py
@@ -50,14 +50,14 @@ def main():
if show_animation:
plt.clf()
- px = float(x[0])
- theta = float(x[2])
+ px = float(x[0, 0])
+ theta = float(x[2, 0])
plot_cart(px, theta)
plt.xlim([-5.0, 2.0])
plt.pause(0.001)
print("Finish")
- print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]")
+ print(f"x={float(x[0, 0]):.2f} [m] , theta={math.degrees(x[2, 0]):.2f} [deg]")
if show_animation:
plt.show()
diff --git a/Control/inverted_pendulum/inverted_pendulum_mpc_control.py b/InvertedPendulum/inverted_pendulum_mpc_control.py
similarity index 95%
rename from Control/inverted_pendulum/inverted_pendulum_mpc_control.py
rename to InvertedPendulum/inverted_pendulum_mpc_control.py
index 6b966b13b4..c45dde8acc 100644
--- a/Control/inverted_pendulum/inverted_pendulum_mpc_control.py
+++ b/InvertedPendulum/inverted_pendulum_mpc_control.py
@@ -55,14 +55,14 @@ def main():
if show_animation:
plt.clf()
- px = float(x[0])
- theta = float(x[2])
+ px = float(x[0, 0])
+ theta = float(x[2, 0])
plot_cart(px, theta)
plt.xlim([-5.0, 2.0])
plt.pause(0.001)
print("Finish")
- print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]")
+ print(f"x={float(x[0, 0]):.2f} [m] , theta={math.degrees(x[2, 0]):.2f} [deg]")
if show_animation:
plt.show()
@@ -91,7 +91,7 @@ def mpc_control(x0):
prob = cvxpy.Problem(cvxpy.Minimize(cost), constr)
start = time.time()
- prob.solve(verbose=False)
+ prob.solve(verbose=False, solver=cvxpy.CLARABEL)
elapsed_time = time.time() - start
print(f"calc time:{elapsed_time:.6f} [sec]")
diff --git a/LICENSE b/LICENSE
index c0c52a4321..1c9b928b54 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1,6 +1,6 @@
The MIT License (MIT)
-Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors:
+Copyright (c) 2016 - now Atsushi Sakai and other contributors:
https://github.com/AtsushiSakai/PythonRobotics/contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
index 1e8d1fc6d6..e8a988a270 100644
--- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
+++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
@@ -4,19 +4,19 @@
author: Ryohei Sasaki(rsasaki0109)
-Ref:
+Reference:
Ensemble Kalman filtering
(https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
"""
-
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import math
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
from utils.angle import rot_mat_2d
@@ -180,7 +180,7 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
diff --git a/Localization/extended_kalman_filter/ekf_with_velocity_correction.py b/Localization/extended_kalman_filter/ekf_with_velocity_correction.py
new file mode 100644
index 0000000000..5dd97830fc
--- /dev/null
+++ b/Localization/extended_kalman_filter/ekf_with_velocity_correction.py
@@ -0,0 +1,198 @@
+"""
+
+Extended kalman filter (EKF) localization with velocity correction sample
+
+author: Atsushi Sakai (@Atsushi_twi)
+modified by: Ryohei Sasaki (@rsasaki0109)
+
+"""
+import sys
+import pathlib
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
+import math
+import matplotlib.pyplot as plt
+import numpy as np
+
+from utils.plot import plot_covariance_ellipse
+
+# Covariance for EKF simulation
+Q = np.diag([
+ 0.1, # variance of location on x-axis
+ 0.1, # variance of location on y-axis
+ np.deg2rad(1.0), # variance of yaw angle
+ 0.4, # variance of velocity
+ 0.1 # variance of scale factor
+]) ** 2 # predict state covariance
+R = np.diag([0.1, 0.1]) ** 2 # Observation x,y position covariance
+
+# Simulation parameter
+INPUT_NOISE = np.diag([0.1, np.deg2rad(5.0)]) ** 2
+GPS_NOISE = np.diag([0.05, 0.05]) ** 2
+
+DT = 0.1 # time tick [s]
+SIM_TIME = 50.0 # simulation time [s]
+
+show_animation = True
+
+
+def calc_input():
+ v = 1.0 # [m/s]
+ yawrate = 0.1 # [rad/s]
+ u = np.array([[v], [yawrate]])
+ return u
+
+
+def observation(xTrue, xd, u):
+ xTrue = motion_model(xTrue, u)
+
+ # add noise to gps x-y
+ z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
+
+ # add noise to input
+ ud = u + INPUT_NOISE @ np.random.randn(2, 1)
+
+ xd = motion_model(xd, ud)
+
+ return xTrue, z, xd, ud
+
+
+def motion_model(x, u):
+ F = np.array([[1.0, 0, 0, 0, 0],
+ [0, 1.0, 0, 0, 0],
+ [0, 0, 1.0, 0, 0],
+ [0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 1.0]])
+
+ B = np.array([[DT * math.cos(x[2, 0]) * x[4, 0], 0],
+ [DT * math.sin(x[2, 0]) * x[4, 0], 0],
+ [0.0, DT],
+ [1.0, 0.0],
+ [0.0, 0.0]])
+
+ x = F @ x + B @ u
+
+ return x
+
+
+def observation_model(x):
+ H = np.array([
+ [1, 0, 0, 0, 0],
+ [0, 1, 0, 0, 0]
+ ])
+ z = H @ x
+
+ return z
+
+
+def jacob_f(x, u):
+ """
+ Jacobian of Motion Model
+
+ motion model
+ x_{t+1} = x_t+v*s*dt*cos(yaw)
+ y_{t+1} = y_t+v*s*dt*sin(yaw)
+ yaw_{t+1} = yaw_t+omega*dt
+ v_{t+1} = v{t}
+ s_{t+1} = s{t}
+ so
+ dx/dyaw = -v*s*dt*sin(yaw)
+ dx/dv = dt*s*cos(yaw)
+ dx/ds = dt*v*cos(yaw)
+ dy/dyaw = v*s*dt*cos(yaw)
+ dy/dv = dt*s*sin(yaw)
+ dy/ds = dt*v*sin(yaw)
+ """
+ yaw = x[2, 0]
+ v = u[0, 0]
+ s = x[4, 0]
+ jF = np.array([
+ [1.0, 0.0, -DT * v * s * math.sin(yaw), DT * s * math.cos(yaw), DT * v * math.cos(yaw)],
+ [0.0, 1.0, DT * v * s * math.cos(yaw), DT * s * math.sin(yaw), DT * v * math.sin(yaw)],
+ [0.0, 0.0, 1.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 1.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0]])
+ return jF
+
+
+def jacob_h():
+ jH = np.array([[1, 0, 0, 0, 0],
+ [0, 1, 0, 0, 0]])
+ return jH
+
+
+def ekf_estimation(xEst, PEst, z, u):
+ # Predict
+ xPred = motion_model(xEst, u)
+ jF = jacob_f(xEst, u)
+ PPred = jF @ PEst @ jF.T + Q
+
+ # Update
+ jH = jacob_h()
+ zPred = observation_model(xPred)
+ y = z - zPred
+ S = jH @ PPred @ jH.T + R
+ K = PPred @ jH.T @ np.linalg.inv(S)
+ xEst = xPred + K @ y
+ PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
+ return xEst, PEst
+
+
+def main():
+ print(__file__ + " start!!")
+
+ time = 0.0
+
+ # State Vector [x y yaw v s]'
+ xEst = np.zeros((5, 1))
+ xEst[4, 0] = 1.0 # Initial scale factor
+ xTrue = np.zeros((5, 1))
+ true_scale_factor = 0.9 # True scale factor
+ xTrue[4, 0] = true_scale_factor
+ PEst = np.eye(5)
+
+ xDR = np.zeros((5, 1)) # Dead reckoning
+
+ # history
+ hxEst = xEst
+ hxTrue = xTrue
+ hxDR = xTrue
+ hz = np.zeros((2, 1))
+
+ while SIM_TIME >= time:
+ time += DT
+ u = calc_input()
+
+ xTrue, z, xDR, ud = observation(xTrue, xDR, u)
+
+ xEst, PEst = ekf_estimation(xEst, PEst, z, ud)
+
+ # store data history
+ hxEst = np.hstack((hxEst, xEst))
+ hxDR = np.hstack((hxDR, xDR))
+ hxTrue = np.hstack((hxTrue, xTrue))
+ hz = np.hstack((hz, z))
+ estimated_scale_factor = hxEst[4, -1]
+ if show_animation:
+ plt.cla()
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect('key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.plot(hz[0, :], hz[1, :], ".g")
+ plt.plot(hxTrue[0, :].flatten(),
+ hxTrue[1, :].flatten(), "-b")
+ plt.plot(hxDR[0, :].flatten(),
+ hxDR[1, :].flatten(), "-k")
+ plt.plot(hxEst[0, :].flatten(),
+ hxEst[1, :].flatten(), "-r")
+ plt.text(0.45, 0.85, f"True Velocity Scale Factor: {true_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
+ plt.text(0.45, 0.95, f"Estimated Velocity Scale Factor: {estimated_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
+ plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst)
+ plt.axis("equal")
+ plt.grid(True)
+ plt.pause(0.001)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py
index a5dea19fd8..d9ece6c6f3 100644
--- a/Localization/extended_kalman_filter/extended_kalman_filter.py
+++ b/Localization/extended_kalman_filter/extended_kalman_filter.py
@@ -5,16 +5,16 @@
author: Atsushi Sakai (@Atsushi_twi)
"""
-
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import math
import matplotlib.pyplot as plt
import numpy as np
-from utils.angle import rot_mat_2d
+from utils.plot import plot_covariance_ellipse
# Covariance for EKF simulation
Q = np.diag([
@@ -136,29 +136,6 @@ def ekf_estimation(xEst, PEst, z, u):
return xEst, PEst
-def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
- Pxy = PEst[0:2, 0:2]
- eigval, eigvec = np.linalg.eig(Pxy)
-
- if eigval[0] >= eigval[1]:
- bigind = 0
- smallind = 1
- else:
- bigind = 1
- smallind = 0
-
- t = np.arange(0, 2 * math.pi + 0.1, 0.1)
- a = math.sqrt(eigval[bigind])
- b = math.sqrt(eigval[smallind])
- x = [a * math.cos(it) for it in t]
- y = [b * math.sin(it) for it in t]
- angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
- fx = rot_mat_2d(angle) @ (np.array([x, y]))
- px = np.array(fx[0, :] + xEst[0, 0]).flatten()
- py = np.array(fx[1, :] + xEst[1, 0]).flatten()
- plt.plot(px, py, "--r")
-
-
def main():
print(__file__ + " start!!")
@@ -203,7 +180,7 @@ def main():
hxDR[1, :].flatten(), "-k")
plt.plot(hxEst[0, :].flatten(),
hxEst[1, :].flatten(), "-r")
- plot_covariance_ellipse(xEst, PEst)
+ plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py
index a4aeb419cf..17cfc2e14c 100644
--- a/Localization/histogram_filter/histogram_filter.py
+++ b/Localization/histogram_filter/histogram_filter.py
@@ -15,6 +15,7 @@
import math
import matplotlib.pyplot as plt
+import matplotlib as mpl
import numpy as np
from scipy.ndimage import gaussian_filter
from scipy.stats import norm
@@ -114,7 +115,7 @@ def motion_model(x, u):
def draw_heat_map(data, mx, my):
max_value = max([max(i_data) for i_data in data])
plt.grid(False)
- plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues"))
+ plt.pcolor(mx, my, data, vmax=max_value, cmap=mpl.colormaps["Blues"])
plt.axis("equal")
@@ -194,7 +195,7 @@ def motion_update(grid_map, u, yaw):
y_shift = grid_map.dy // grid_map.xy_resolution
if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted
- grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
+ grid_map = map_shift(grid_map, int(x_shift[0]), int(y_shift[0]))
grid_map.dx -= x_shift * grid_map.xy_resolution
grid_map.dy -= y_shift * grid_map.xy_resolution
diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py
index 1c45687ed1..ba54a3d12b 100644
--- a/Localization/particle_filter/particle_filter.py
+++ b/Localization/particle_filter/particle_filter.py
@@ -6,8 +6,8 @@
"""
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import math
@@ -96,10 +96,10 @@ def calc_covariance(x_est, px, pw):
calculate covariance matrix
see ipynb doc
"""
- cov = np.zeros((3, 3))
+ cov = np.zeros((4, 4))
n_particle = px.shape[1]
for i in range(n_particle):
- dx = (px[:, i:i + 1] - x_est)[0:3]
+ dx = (px[:, i:i + 1] - x_est)
cov += pw[0, i] * dx @ dx.T
cov *= 1.0 / (1.0 - pw @ pw.T)
diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py
index 4f42cfd5d1..4af748ec71 100644
--- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py
+++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py
@@ -7,8 +7,8 @@
"""
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import math
@@ -69,8 +69,8 @@ def motion_model(x, u):
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
- B = np.array([[DT * math.cos(x[2]), 0],
- [DT * math.sin(x[2]), 0],
+ B = np.array([[DT * math.cos(x[2, 0]), 0],
+ [DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py
new file mode 100644
index 0000000000..0f96c9e8c6
--- /dev/null
+++ b/Mapping/DistanceMap/distance_map.py
@@ -0,0 +1,202 @@
+"""
+Distance Map
+
+author: Wang Zheng (@Aglargil)
+
+Reference:
+
+- [Distance Map]
+(https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf)
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy
+
+INF = 1e20
+ENABLE_PLOT = True
+
+
+def compute_sdf_scipy(obstacles):
+ """
+ Compute the signed distance field (SDF) from a boolean field using scipy.
+ This function has the same functionality as compute_sdf.
+ However, by using scipy.ndimage.distance_transform_edt, it can compute much faster.
+
+ Example: 500×500 map
+ • compute_sdf: 3 sec
+ • compute_sdf_scipy: 0.05 sec
+
+ Parameters
+ ----------
+ obstacles : array_like
+ A 2D boolean array where '1' represents obstacles and '0' represents free space.
+
+ Returns
+ -------
+ array_like
+ A 2D array representing the signed distance field, where positive values indicate distance
+ to the nearest obstacle, and negative values indicate distance to the nearest free space.
+ """
+ # distance_transform_edt use '0' as obstacles, so we need to convert the obstacles to '0'
+ a = scipy.ndimage.distance_transform_edt(obstacles == 0)
+ b = scipy.ndimage.distance_transform_edt(obstacles == 1)
+ return a - b
+
+
+def compute_udf_scipy(obstacles):
+ """
+ Compute the unsigned distance field (UDF) from a boolean field using scipy.
+ This function has the same functionality as compute_udf.
+ However, by using scipy.ndimage.distance_transform_edt, it can compute much faster.
+
+ Example: 500×500 map
+ • compute_udf: 1.5 sec
+ • compute_udf_scipy: 0.02 sec
+
+ Parameters
+ ----------
+ obstacles : array_like
+ A 2D boolean array where '1' represents obstacles and '0' represents free space.
+
+ Returns
+ -------
+ array_like
+ A 2D array of distances from the nearest obstacle, with the same dimensions as `bool_field`.
+ """
+ return scipy.ndimage.distance_transform_edt(obstacles == 0)
+
+
+def compute_sdf(obstacles):
+ """
+ Compute the signed distance field (SDF) from a boolean field.
+
+ Parameters
+ ----------
+ obstacles : array_like
+ A 2D boolean array where '1' represents obstacles and '0' represents free space.
+
+ Returns
+ -------
+ array_like
+ A 2D array representing the signed distance field, where positive values indicate distance
+ to the nearest obstacle, and negative values indicate distance to the nearest free space.
+ """
+ a = compute_udf(obstacles)
+ b = compute_udf(obstacles == 0)
+ return a - b
+
+
+def compute_udf(obstacles):
+ """
+ Compute the unsigned distance field (UDF) from a boolean field.
+
+ Parameters
+ ----------
+ obstacles : array_like
+ A 2D boolean array where '1' represents obstacles and '0' represents free space.
+
+ Returns
+ -------
+ array_like
+ A 2D array of distances from the nearest obstacle, with the same dimensions as `bool_field`.
+ """
+ edt = obstacles.copy()
+ if not np.all(np.isin(edt, [0, 1])):
+ raise ValueError("Input array should only contain 0 and 1")
+ edt = np.where(edt == 0, INF, edt)
+ edt = np.where(edt == 1, 0, edt)
+ for row in range(len(edt)):
+ dt(edt[row])
+ edt = edt.T
+ for row in range(len(edt)):
+ dt(edt[row])
+ edt = edt.T
+ return np.sqrt(edt)
+
+
+def dt(d):
+ """
+ Compute 1D distance transform under the squared Euclidean distance
+
+ Parameters
+ ----------
+ d : array_like
+ Input array containing the distances.
+
+ Returns:
+ --------
+ d : array_like
+ The transformed array with computed distances.
+ """
+ v = np.zeros(len(d) + 1)
+ z = np.zeros(len(d) + 1)
+ k = 0
+ v[0] = 0
+ z[0] = -INF
+ z[1] = INF
+ for q in range(1, len(d)):
+ s = ((d[q] + q * q) - (d[int(v[k])] + v[k] * v[k])) / (2 * q - 2 * v[k])
+ while s <= z[k]:
+ k = k - 1
+ s = ((d[q] + q * q) - (d[int(v[k])] + v[k] * v[k])) / (2 * q - 2 * v[k])
+ k = k + 1
+ v[k] = q
+ z[k] = s
+ z[k + 1] = INF
+ k = 0
+ for q in range(len(d)):
+ while z[k + 1] < q:
+ k = k + 1
+ dx = q - v[k]
+ d[q] = dx * dx + d[int(v[k])]
+
+
+def main():
+ obstacles = np.array(
+ [
+ [1, 0, 0, 0, 0],
+ [0, 1, 1, 1, 0],
+ [0, 1, 1, 1, 0],
+ [0, 0, 1, 1, 0],
+ [0, 0, 1, 0, 0],
+ [0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0],
+ [0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0],
+ ]
+ )
+
+ # Compute the signed distance field
+ sdf = compute_sdf(obstacles)
+ udf = compute_udf(obstacles)
+
+ if ENABLE_PLOT:
+ _, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(15, 5))
+
+ obstacles_plot = ax1.imshow(obstacles, cmap="binary")
+ ax1.set_title("Obstacles")
+ ax1.set_xlabel("x")
+ ax1.set_ylabel("y")
+ plt.colorbar(obstacles_plot, ax=ax1)
+
+ udf_plot = ax2.imshow(udf, cmap="viridis")
+ ax2.set_title("Unsigned Distance Field")
+ ax2.set_xlabel("x")
+ ax2.set_ylabel("y")
+ plt.colorbar(udf_plot, ax=ax2)
+
+ sdf_plot = ax3.imshow(sdf, cmap="RdBu")
+ ax3.set_title("Signed Distance Field")
+ ax3.set_xlabel("x")
+ ax3.set_ylabel("y")
+ plt.colorbar(sdf_plot, ax=ax3)
+
+ plt.tight_layout()
+ plt.show()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/Mapping/circle_fitting/circle_fitting.py b/Mapping/circle_fitting/circle_fitting.py
index c331d56796..b5714b507c 100644
--- a/Mapping/circle_fitting/circle_fitting.py
+++ b/Mapping/circle_fitting/circle_fitting.py
@@ -16,12 +16,33 @@
def circle_fitting(x, y):
"""
- Circle Fitting with least squared
- input: point x-y positions
- output cxe x center position
- cye y center position
- re radius of circle
- error: prediction error
+ Fits a circle to a given set of points using a least-squares approach.
+
+ This function calculates the center (x, y) and radius of a circle that best fits
+ the given set of points in a two-dimensional plane. It minimizes the squared
+ errors between the circle and the provided points and returns the calculated
+ center coordinates, radius, and the fitting error.
+
+ Raises
+ ------
+ ValueError
+ If the input lists x and y do not contain the same number of elements.
+
+ Parameters
+ ----------
+ x : list[float]
+ The x-coordinates of the points.
+ y : list[float]
+ The y-coordinates of the points.
+
+ Returns
+ -------
+ tuple[float, float, float, float]
+ A tuple containing:
+ - The x-coordinate of the center of the fitted circle (float).
+ - The y-coordinate of the center of the fitted circle (float).
+ - The radius of the fitted circle (float).
+ - The fitting error as a deviation metric (float).
"""
sumx = sum(x)
@@ -40,9 +61,9 @@ def circle_fitting(x, y):
T = np.linalg.inv(F).dot(G)
- cxe = float(T[0] / -2)
- cye = float(T[1] / -2)
- re = math.sqrt(cxe**2 + cye**2 - T[2])
+ cxe = float(T[0, 0] / -2)
+ cye = float(T[1, 0] / -2)
+ re = math.sqrt(cxe**2 + cye**2 - T[2, 0])
error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])
diff --git a/Mapping/grid_map_lib/grid_map_lib.py b/Mapping/grid_map_lib/grid_map_lib.py
index 85dd76e71d..d08d8ec5ba 100644
--- a/Mapping/grid_map_lib/grid_map_lib.py
+++ b/Mapping/grid_map_lib/grid_map_lib.py
@@ -5,22 +5,42 @@
author: Atsushi Sakai
"""
-
+from functools import total_ordering
import matplotlib.pyplot as plt
import numpy as np
+@total_ordering
+class FloatGrid:
+
+ def __init__(self, init_val=0.0):
+ self.data = init_val
+
+ def get_float_data(self):
+ return self.data
+
+ def __eq__(self, other):
+ if not isinstance(other, FloatGrid):
+ return NotImplemented
+ return self.get_float_data() == other.get_float_data()
+
+ def __lt__(self, other):
+ if not isinstance(other, FloatGrid):
+ return NotImplemented
+ return self.get_float_data() < other.get_float_data()
+
+
class GridMap:
"""
GridMap class
"""
def __init__(self, width, height, resolution,
- center_x, center_y, init_val=0.0):
+ center_x, center_y, init_val=FloatGrid(0.0)):
"""__init__
:param width: number of grid for width
- :param height: number of grid for heigt
+ :param height: number of grid for height
:param resolution: grid resolution [m]
:param center_x: center x position [m]
:param center_y: center y position [m]
@@ -35,8 +55,9 @@ def __init__(self, width, height, resolution,
self.left_lower_x = self.center_x - self.width / 2.0 * self.resolution
self.left_lower_y = self.center_y - self.height / 2.0 * self.resolution
- self.ndata = self.width * self.height
- self.data = [init_val] * self.ndata
+ self.n_data = self.width * self.height
+ self.data = [init_val] * self.n_data
+ self.data_type = type(init_val)
def get_value_from_xy_index(self, x_ind, y_ind):
"""get_value_from_xy_index
@@ -49,7 +70,7 @@ def get_value_from_xy_index(self, x_ind, y_ind):
grid_ind = self.calc_grid_index_from_xy_index(x_ind, y_ind)
- if 0 <= grid_ind < self.ndata:
+ if 0 <= grid_ind < self.n_data:
return self.data[grid_ind]
else:
return None
@@ -101,7 +122,7 @@ def set_value_from_xy_index(self, x_ind, y_ind, val):
grid_ind = int(y_ind * self.width + x_ind)
- if 0 <= grid_ind < self.ndata:
+ if 0 <= grid_ind < self.n_data and isinstance(val, self.data_type):
self.data[grid_ind] = val
return True # OK
else:
@@ -138,6 +159,27 @@ def calc_grid_index_from_xy_index(self, x_ind, y_ind):
grid_ind = int(y_ind * self.width + x_ind)
return grid_ind
+ def calc_xy_index_from_grid_index(self, grid_ind):
+ y_ind, x_ind = divmod(grid_ind, self.width)
+ return x_ind, y_ind
+
+ def calc_grid_index_from_xy_pos(self, x_pos, y_pos):
+ """get_xy_index_from_xy_pos
+
+ :param x_pos: x position [m]
+ :param y_pos: y position [m]
+ """
+ x_ind = self.calc_xy_index_from_position(
+ x_pos, self.left_lower_x, self.width)
+ y_ind = self.calc_xy_index_from_position(
+ y_pos, self.left_lower_y, self.height)
+
+ return self.calc_grid_index_from_xy_index(x_ind, y_ind)
+
+ def calc_grid_central_xy_position_from_grid_index(self, grid_ind):
+ x_ind, y_ind = self.calc_xy_index_from_grid_index(grid_ind)
+ return self.calc_grid_central_xy_position_from_xy_index(x_ind, y_ind)
+
def calc_grid_central_xy_position_from_xy_index(self, x_ind, y_ind):
x_pos = self.calc_grid_central_xy_position_from_index(
x_ind, self.left_lower_x)
@@ -156,39 +198,40 @@ def calc_xy_index_from_position(self, pos, lower_pos, max_index):
else:
return None
- def check_occupied_from_xy_index(self, xind, yind, occupied_val=1.0):
+ def check_occupied_from_xy_index(self, x_ind, y_ind, occupied_val):
- val = self.get_value_from_xy_index(xind, yind)
+ val = self.get_value_from_xy_index(x_ind, y_ind)
if val is None or val >= occupied_val:
return True
else:
return False
- def expand_grid(self):
- xinds, yinds = [], []
+ def expand_grid(self, occupied_val=FloatGrid(1.0)):
+ x_inds, y_inds, values = [], [], []
for ix in range(self.width):
for iy in range(self.height):
- if self.check_occupied_from_xy_index(ix, iy):
- xinds.append(ix)
- yinds.append(iy)
-
- for (ix, iy) in zip(xinds, yinds):
- self.set_value_from_xy_index(ix + 1, iy, val=1.0)
- self.set_value_from_xy_index(ix, iy + 1, val=1.0)
- self.set_value_from_xy_index(ix + 1, iy + 1, val=1.0)
- self.set_value_from_xy_index(ix - 1, iy, val=1.0)
- self.set_value_from_xy_index(ix, iy - 1, val=1.0)
- self.set_value_from_xy_index(ix - 1, iy - 1, val=1.0)
+ if self.check_occupied_from_xy_index(ix, iy, occupied_val):
+ x_inds.append(ix)
+ y_inds.append(iy)
+ values.append(self.get_value_from_xy_index(ix, iy))
+
+ for (ix, iy, value) in zip(x_inds, y_inds, values):
+ self.set_value_from_xy_index(ix + 1, iy, val=value)
+ self.set_value_from_xy_index(ix, iy + 1, val=value)
+ self.set_value_from_xy_index(ix + 1, iy + 1, val=value)
+ self.set_value_from_xy_index(ix - 1, iy, val=value)
+ self.set_value_from_xy_index(ix, iy - 1, val=value)
+ self.set_value_from_xy_index(ix - 1, iy - 1, val=value)
@staticmethod
def check_inside_polygon(iox, ioy, x, y):
- npoint = len(x) - 1
+ n_point = len(x) - 1
inside = False
- for i1 in range(npoint):
- i2 = (i1 + 1) % (npoint + 1)
+ for i1 in range(n_point):
+ i2 = (i1 + 1) % (n_point + 1)
if x[i1] >= x[i2]:
min_x, max_x = x[i2], x[i1]
@@ -211,27 +254,26 @@ def print_grid_map_info(self):
print("center_y:", self.center_y)
print("left_lower_x:", self.left_lower_x)
print("left_lower_y:", self.left_lower_y)
- print("ndata:", self.ndata)
+ print("n_data:", self.n_data)
def plot_grid_map(self, ax=None):
-
- grid_data = np.reshape(np.array(self.data), (self.height, self.width))
+ float_data_array = np.array([d.get_float_data() for d in self.data])
+ grid_data = np.reshape(float_data_array, (self.height, self.width))
if not ax:
fig, ax = plt.subplots()
heat_map = ax.pcolor(grid_data, cmap="Blues", vmin=0.0, vmax=1.0)
plt.axis("equal")
- # plt.show()
return heat_map
-def test_polygon_set():
+def polygon_set_demo():
ox = [0.0, 4.35, 20.0, 50.0, 100.0, 130.0, 40.0]
oy = [0.0, -4.15, -20.0, 0.0, 30.0, 60.0, 80.0]
grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
- grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
+ grid_map.set_value_from_polygon(ox, oy, FloatGrid(1.0), inside=False)
grid_map.plot_grid_map()
@@ -239,24 +281,27 @@ def test_polygon_set():
plt.grid(True)
-def test_position_set():
+def position_set_demo():
grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
- grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
- grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
- grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
- grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
- grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
- grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
+ grid_map.set_value_from_xy_pos(10.1, -1.1, FloatGrid(1.0))
+ grid_map.set_value_from_xy_pos(10.1, -0.1, FloatGrid(1.0))
+ grid_map.set_value_from_xy_pos(10.1, 1.1, FloatGrid(1.0))
+ grid_map.set_value_from_xy_pos(11.1, 0.1, FloatGrid(1.0))
+ grid_map.set_value_from_xy_pos(10.1, 0.1, FloatGrid(1.0))
+ grid_map.set_value_from_xy_pos(9.1, 0.1, FloatGrid(1.0))
grid_map.plot_grid_map()
+ plt.axis("equal")
+ plt.grid(True)
+
def main():
print("start!!")
- test_position_set()
- test_polygon_set()
+ position_set_demo()
+ polygon_set_demo()
plt.show()
diff --git a/Mapping/kmeans_clustering/kmeans_clustering.py b/Mapping/kmeans_clustering/kmeans_clustering.py
index e18960e990..cee01e5ad5 100644
--- a/Mapping/kmeans_clustering/kmeans_clustering.py
+++ b/Mapping/kmeans_clustering/kmeans_clustering.py
@@ -17,12 +17,37 @@
def kmeans_clustering(rx, ry, nc):
+ """
+ Performs k-means clustering on the given dataset, iteratively adjusting cluster centroids
+ until convergence within a defined threshold or reaching the maximum number of
+ iterations.
+
+ The implementation initializes clusters, calculates initial centroids, and refines the
+ clusters through iterative updates to optimize the cost function based on minimum
+ distance between datapoints and centroids.
+
+ Arguments:
+ rx: List[float]
+ The x-coordinates of the dataset points to be clustered.
+ ry: List[float]
+ The y-coordinates of the dataset points to be clustered.
+ nc: int
+ The number of clusters to group the data into.
+
+ Returns:
+ Clusters
+ An instance containing the final cluster assignments and centroids after
+ convergence.
+
+ Raises:
+ None
+
+ """
clusters = Clusters(rx, ry, nc)
clusters.calc_centroid()
pre_cost = float("inf")
for loop in range(MAX_LOOP):
- print("loop:", loop)
cost = clusters.update_clusters()
clusters.calc_centroid()
diff --git a/Mapping/ndt_map/ndt_map.py b/Mapping/ndt_map/ndt_map.py
new file mode 100644
index 0000000000..f4f3299662
--- /dev/null
+++ b/Mapping/ndt_map/ndt_map.py
@@ -0,0 +1,135 @@
+"""
+Normal Distribution Transform (NDTGrid) mapping sample
+"""
+import matplotlib.pyplot as plt
+import numpy as np
+from collections import defaultdict
+
+from Mapping.grid_map_lib.grid_map_lib import GridMap
+from utils.plot import plot_covariance_ellipse
+
+
+class NDTMap:
+ """
+ Normal Distribution Transform (NDT) map class
+
+ :param ox: obstacle x position list
+ :param oy: obstacle y position list
+ :param resolution: grid resolution [m]
+ """
+
+ class NDTGrid:
+ """
+ NDT grid
+ """
+
+ def __init__(self):
+ #: Number of points in the NDTGrid grid
+ self.n_points = 0
+ #: Mean x position of points in the NDTGrid cell
+ self.mean_x = None
+ #: Mean y position of points in the NDTGrid cell
+ self.mean_y = None
+ #: Center x position of the NDT grid
+ self.center_grid_x = None
+ #: Center y position of the NDT grid
+ self.center_grid_y = None
+ #: Covariance matrix of the NDT grid
+ self.covariance = None
+ #: Eigen vectors of the NDT grid
+ self.eig_vec = None
+ #: Eigen values of the NDT grid
+ self.eig_values = None
+
+ def __init__(self, ox, oy, resolution):
+ #: Minimum number of points in the NDT grid
+ self.min_n_points = 3
+ #: Resolution of the NDT grid [m]
+ self.resolution = resolution
+ width = int((max(ox) - min(ox))/resolution) + 3 # rounding up + right and left margin
+ height = int((max(oy) - min(oy))/resolution) + 3
+ center_x = np.mean(ox)
+ center_y = np.mean(oy)
+ self.ox = ox
+ self.oy = oy
+ #: NDT grid index map
+ self.grid_index_map = self._create_grid_index_map(ox, oy)
+
+ #: NDT grid map. Each grid contains NDTGrid object
+ self._construct_grid_map(center_x, center_y, height, ox, oy, resolution, width)
+
+ def _construct_grid_map(self, center_x, center_y, height, ox, oy, resolution, width):
+ self.grid_map = GridMap(width, height, resolution, center_x, center_y, self.NDTGrid())
+ for grid_index, inds in self.grid_index_map.items():
+ ndt = self.NDTGrid()
+ ndt.n_points = len(inds)
+ if ndt.n_points >= self.min_n_points:
+ ndt.mean_x = np.mean(ox[inds])
+ ndt.mean_y = np.mean(oy[inds])
+ ndt.center_grid_x, ndt.center_grid_y = \
+ self.grid_map.calc_grid_central_xy_position_from_grid_index(grid_index)
+ ndt.covariance = np.cov(ox[inds], oy[inds])
+ ndt.eig_values, ndt.eig_vec = np.linalg.eig(ndt.covariance)
+ self.grid_map.data[grid_index] = ndt
+
+ def _create_grid_index_map(self, ox, oy):
+ grid_index_map = defaultdict(list)
+ for i in range(len(ox)):
+ grid_index = self.grid_map.calc_grid_index_from_xy_pos(ox[i], oy[i])
+ grid_index_map[grid_index].append(i)
+ return grid_index_map
+
+
+def create_dummy_observation_data():
+ ox = []
+ oy = []
+ # left corridor
+ for y in range(-50, 50):
+ ox.append(-20.0)
+ oy.append(y)
+ # right corridor 1
+ for y in range(-50, 0):
+ ox.append(20.0)
+ oy.append(y)
+ # right corridor 2
+ for x in range(20, 50):
+ ox.append(x)
+ oy.append(0)
+ # right corridor 3
+ for x in range(20, 50):
+ ox.append(x)
+ oy.append(x/2.0+10)
+ # right corridor 4
+ for y in range(20, 50):
+ ox.append(20)
+ oy.append(y)
+ ox = np.array(ox)
+ oy = np.array(oy)
+ # Adding random noize
+ ox += np.random.rand(len(ox)) * 1.0
+ oy += np.random.rand(len(ox)) * 1.0
+ return ox, oy
+
+
+def main():
+ print(__file__ + " start!!")
+
+ ox, oy = create_dummy_observation_data()
+ grid_resolution = 10.0
+ ndt_map = NDTMap(ox, oy, grid_resolution)
+
+ # plot raw observation
+ plt.plot(ox, oy, ".r")
+
+ # plot grid clustering
+ [plt.plot(ox[inds], oy[inds], "x") for inds in ndt_map.grid_index_map.values()]
+
+ # plot ndt grid map
+ [plot_covariance_ellipse(ndt.mean_x, ndt.mean_y, ndt.covariance, color="-k") for ndt in ndt_map.grid_map.data if ndt.n_points > 0]
+
+ plt.axis("equal")
+ plt.show()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/Mapping/normal_vector_estimation/normal_vector_estimation.py b/Mapping/normal_vector_estimation/normal_vector_estimation.py
new file mode 100644
index 0000000000..996ba3ffee
--- /dev/null
+++ b/Mapping/normal_vector_estimation/normal_vector_estimation.py
@@ -0,0 +1,177 @@
+import numpy as np
+from matplotlib import pyplot as plt
+
+from utils.plot import plot_3d_vector_arrow, plot_triangle, set_equal_3d_axis
+
+show_animation = True
+
+
+def calc_normal_vector(p1, p2, p3):
+ """Calculate normal vector of triangle
+
+ Parameters
+ ----------
+ p1 : np.array
+ 3D point
+ p2 : np.array
+ 3D point
+ p3 : np.array
+ 3D point
+
+ Returns
+ -------
+ normal_vector : np.array
+ normal vector (3,)
+
+ """
+ # calculate two vectors of triangle
+ v1 = p2 - p1
+ v2 = p3 - p1
+
+ # calculate normal vector
+ normal_vector = np.cross(v1, v2)
+
+ # normalize vector
+ normal_vector = normal_vector / np.linalg.norm(normal_vector)
+
+ return normal_vector
+
+
+def sample_3d_points_from_a_plane(num_samples, normal):
+ points_2d = np.random.normal(size=(num_samples, 2)) # 2D points on a plane
+ d = 0
+ for i in range(len(points_2d)):
+ point_3d = np.append(points_2d[i], 0)
+ d += normal @ point_3d
+ d /= len(points_2d)
+
+ points_3d = np.zeros((len(points_2d), 3))
+ for i in range(len(points_2d)):
+ point_2d = np.append(points_2d[i], 0)
+ projection_length = (d - normal @ point_2d) / np.linalg.norm(normal)
+ points_3d[i] = point_2d + projection_length * normal
+
+ return points_3d
+
+
+def distance_to_plane(point, normal, origin):
+ dot_product = np.dot(normal, point) - np.dot(normal, origin)
+ if np.isclose(dot_product, 0):
+ return 0.0
+ else:
+ distance = abs(dot_product) / np.linalg.norm(normal)
+ return distance
+
+
+def ransac_normal_vector_estimation(points_3d, inlier_radio_th=0.7,
+ inlier_dist=0.1, p=0.99):
+ """
+ RANSAC based normal vector estimation
+
+ Parameters
+ ----------
+ points_3d : np.array
+ 3D points (N, 3)
+ inlier_radio_th : float
+ Inlier ratio threshold. If inlier ratio is larger than this value,
+ the iteration is stopped. Default is 0.7.
+ inlier_dist : float
+ Inlier distance threshold. If distance between points and estimated
+ plane is smaller than this value, the point is inlier. Default is 0.1.
+ p : float
+ Probability that at least one of the sets of random samples does not
+ include an outlier. If this probability is near 1, the iteration
+ number is large. Default is 0.99.
+
+ Returns
+ -------
+ center_vector : np.array
+ Center of estimated plane. (3,)
+ normal_vector : np.array
+ Normal vector of estimated plane. (3,)
+
+ """
+ center = np.mean(points_3d, axis=0)
+
+ max_iter = int(np.floor(np.log(1.0-p)/np.log(1.0-(1.0-inlier_radio_th)**3)))
+
+ for ite in range(max_iter):
+ # Random sampling
+ sampled_ids = np.random.choice(points_3d.shape[0], size=3,
+ replace=False)
+ sampled_points = points_3d[sampled_ids, :]
+ p1 = sampled_points[0, :]
+ p2 = sampled_points[1, :]
+ p3 = sampled_points[2, :]
+ normal_vector = calc_normal_vector(p1, p2, p3)
+
+ # calc inlier ratio
+ n_inliner = 0
+ for i in range(points_3d.shape[0]):
+ p = points_3d[i, :]
+ if distance_to_plane(p, normal_vector, center) <= inlier_dist:
+ n_inliner += 1
+ inlier_ratio = n_inliner / points_3d.shape[0]
+ print(f"Iter:{ite}, {inlier_ratio=}")
+ if inlier_ratio > inlier_radio_th:
+ return center, normal_vector
+
+ return center, None
+
+
+def main1():
+ p1 = np.array([0.0, 0.0, 1.0])
+ p2 = np.array([1.0, 1.0, 0.0])
+ p3 = np.array([0.0, 1.0, 0.0])
+
+ center = np.mean([p1, p2, p3], axis=0)
+ normal_vector = calc_normal_vector(p1, p2, p3)
+ print(f"{center=}")
+ print(f"{normal_vector=}")
+
+ if show_animation:
+ fig = plt.figure()
+ ax = fig.add_subplot(projection='3d')
+ set_equal_3d_axis(ax, [0.0, 2.5], [0.0, 2.5], [0.0, 3.0])
+ plot_triangle(p1, p2, p3, ax)
+ ax.plot(center[0], center[1], center[2], "ro")
+ plot_3d_vector_arrow(ax, center, center + normal_vector)
+ plt.show()
+
+
+def main2(rng=None):
+ true_normal = np.array([0, 1, 1])
+ true_normal = true_normal / np.linalg.norm(true_normal)
+ num_samples = 100
+ noise_scale = 0.1
+
+ points_3d = sample_3d_points_from_a_plane(num_samples, true_normal)
+ # add random noise
+ points_3d += np.random.normal(size=points_3d.shape, scale=noise_scale)
+
+ print(f"{points_3d.shape=}")
+
+ center, estimated_normal = ransac_normal_vector_estimation(
+ points_3d, inlier_dist=noise_scale)
+
+ if estimated_normal is None:
+ print("Failed to estimate normal vector")
+ return
+
+ print(f"{true_normal=}")
+ print(f"{estimated_normal=}")
+
+ if show_animation:
+ fig = plt.figure()
+ ax = fig.add_subplot(projection='3d')
+ ax.plot(points_3d[:, 0], points_3d[:, 1], points_3d[:, 2], ".r")
+ plot_3d_vector_arrow(ax, center, center + true_normal)
+ plot_3d_vector_arrow(ax, center, center + estimated_normal)
+ set_equal_3d_axis(ax, [-3.0, 3.0], [-3.0, 3.0], [-3.0, 3.0])
+ plt.title("RANSAC based Normal vector estimation")
+ plt.show()
+
+
+if __name__ == '__main__':
+ # main1()
+ main2()
diff --git a/Mapping/point_cloud_sampling/point_cloud_sampling.py b/Mapping/point_cloud_sampling/point_cloud_sampling.py
new file mode 100644
index 0000000000..df7cde41c0
--- /dev/null
+++ b/Mapping/point_cloud_sampling/point_cloud_sampling.py
@@ -0,0 +1,168 @@
+"""
+Point cloud sampling example codes. This code supports
+- Voxel point sampling
+- Farthest point sampling
+- Poisson disk sampling
+
+"""
+import matplotlib.pyplot as plt
+import numpy as np
+import numpy.typing as npt
+from collections import defaultdict
+
+do_plot = True
+
+
+def voxel_point_sampling(original_points: npt.NDArray, voxel_size: float):
+ """
+ Voxel Point Sampling function.
+ This function sample N-dimensional points with voxel grid.
+ Points in a same voxel grid will be merged by mean operation for sampling.
+
+ Parameters
+ ----------
+ original_points : (M, N) N-dimensional points for sampling.
+ The number of points is M.
+ voxel_size : voxel grid size
+
+ Returns
+ -------
+ sampled points (M', N)
+ """
+ voxel_dict = defaultdict(list)
+ for i in range(original_points.shape[0]):
+ xyz = original_points[i, :]
+ xyz_index = tuple(xyz // voxel_size)
+ voxel_dict[xyz_index].append(xyz)
+ points = np.vstack([np.mean(v, axis=0) for v in voxel_dict.values()])
+ return points
+
+
+def farthest_point_sampling(orig_points: npt.NDArray,
+ n_points: int, seed: int):
+ """
+ Farthest point sampling function
+ This function sample N-dimensional points with the farthest point policy.
+
+ Parameters
+ ----------
+ orig_points : (M, N) N-dimensional points for sampling.
+ The number of points is M.
+ n_points : number of points for sampling
+ seed : random seed number
+
+ Returns
+ -------
+ sampled points (n_points, N)
+
+ """
+ rng = np.random.default_rng(seed)
+ n_orig_points = orig_points.shape[0]
+ first_point_id = rng.choice(range(n_orig_points))
+ min_distances = np.ones(n_orig_points) * float("inf")
+ selected_ids = [first_point_id]
+ while len(selected_ids) < n_points:
+ base_point = orig_points[selected_ids[-1], :]
+ distances = np.linalg.norm(orig_points[np.newaxis, :] - base_point,
+ axis=2).flatten()
+ min_distances = np.minimum(min_distances, distances)
+ distances_rank = np.argsort(-min_distances) # Farthest order
+ for i in distances_rank: # From the farthest point
+ if i not in selected_ids: # if not selected yes, select it
+ selected_ids.append(i)
+ break
+ return orig_points[selected_ids, :]
+
+
+def poisson_disk_sampling(orig_points: npt.NDArray, n_points: int,
+ min_distance: float, seed: int, MAX_ITER=1000):
+ """
+ Poisson disk sampling function
+ This function sample N-dimensional points randomly until the number of
+ points keeping minimum distance between selected points.
+
+ Parameters
+ ----------
+ orig_points : (M, N) N-dimensional points for sampling.
+ The number of points is M.
+ n_points : number of points for sampling
+ min_distance : minimum distance between selected points.
+ seed : random seed number
+ MAX_ITER : Maximum number of iteration. Default is 1000.
+
+ Returns
+ -------
+ sampled points (n_points or less, N)
+ """
+ rng = np.random.default_rng(seed)
+ selected_id = rng.choice(range(orig_points.shape[0]))
+ selected_ids = [selected_id]
+ loop = 0
+ while len(selected_ids) < n_points and loop <= MAX_ITER:
+ selected_id = rng.choice(range(orig_points.shape[0]))
+ base_point = orig_points[selected_id, :]
+ distances = np.linalg.norm(
+ orig_points[np.newaxis, selected_ids] - base_point,
+ axis=2).flatten()
+ if min(distances) >= min_distance:
+ selected_ids.append(selected_id)
+ loop += 1
+ if len(selected_ids) != n_points:
+ print("Could not find the specified number of points...")
+
+ return orig_points[selected_ids, :]
+
+
+def plot_sampled_points(original_points, sampled_points, method_name):
+ fig = plt.figure()
+ ax = fig.add_subplot(projection='3d')
+ ax.scatter(original_points[:, 0], original_points[:, 1],
+ original_points[:, 2], marker=".", label="Original points")
+ ax.scatter(sampled_points[:, 0], sampled_points[:, 1],
+ sampled_points[:, 2], marker="o",
+ label="Filtered points")
+ plt.legend()
+ plt.title(method_name)
+ plt.axis("equal")
+
+
+def main():
+ n_points = 1000
+ seed = 1234
+ rng = np.random.default_rng(seed)
+
+ x = rng.normal(0.0, 10.0, n_points)
+ y = rng.normal(0.0, 1.0, n_points)
+ z = rng.normal(0.0, 10.0, n_points)
+ original_points = np.vstack((x, y, z)).T
+ print(f"{original_points.shape=}")
+ print("Voxel point sampling")
+ voxel_size = 20.0
+ voxel_sampling_points = voxel_point_sampling(original_points, voxel_size)
+ print(f"{voxel_sampling_points.shape=}")
+
+ print("Farthest point sampling")
+ n_points = 20
+ farthest_sampling_points = farthest_point_sampling(original_points,
+ n_points, seed)
+ print(f"{farthest_sampling_points.shape=}")
+
+ print("Poisson disk sampling")
+ n_points = 20
+ min_distance = 10.0
+ poisson_disk_points = poisson_disk_sampling(original_points, n_points,
+ min_distance, seed)
+ print(f"{poisson_disk_points.shape=}")
+
+ if do_plot:
+ plot_sampled_points(original_points, voxel_sampling_points,
+ "Voxel point sampling")
+ plot_sampled_points(original_points, farthest_sampling_points,
+ "Farthest point sampling")
+ plot_sampled_points(original_points, poisson_disk_points,
+ "poisson disk sampling")
+ plt.show()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/Mapping/raycasting_grid_map/raycasting_grid_map.py b/Mapping/ray_casting_grid_map/ray_casting_grid_map.py
similarity index 96%
rename from Mapping/raycasting_grid_map/raycasting_grid_map.py
rename to Mapping/ray_casting_grid_map/ray_casting_grid_map.py
index 8ce37b925b..c7e73f0630 100644
--- a/Mapping/raycasting_grid_map/raycasting_grid_map.py
+++ b/Mapping/ray_casting_grid_map/ray_casting_grid_map.py
@@ -48,7 +48,7 @@ def atan_zero_to_twopi(y, x):
return angle
-def precasting(minx, miny, xw, yw, xyreso, yawreso):
+def pre_casting(minx, miny, xw, yw, xyreso, yawreso):
precast = [[] for i in range(int(round((math.pi * 2.0) / yawreso)) + 1)]
@@ -81,7 +81,7 @@ def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso):
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
- precast = precasting(minx, miny, xw, yw, xyreso, yawreso)
+ precast = pre_casting(minx, miny, xw, yw, xyreso, yawreso)
for (x, y) in zip(ox, oy):
diff --git a/Mapping/rectangle_fitting/__init_.py b/Mapping/rectangle_fitting/__init_.py
index 087dab646e..2194d4c303 100644
--- a/Mapping/rectangle_fitting/__init_.py
+++ b/Mapping/rectangle_fitting/__init_.py
@@ -1,4 +1,3 @@
-import os
import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent))
diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py
index 6d3e63f0f3..7902619666 100644
--- a/Mapping/rectangle_fitting/rectangle_fitting.py
+++ b/Mapping/rectangle_fitting/rectangle_fitting.py
@@ -4,7 +4,7 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
- Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners -
The Robotics Institute Carnegie Mellon University
https://www.ri.cmu.edu/publications/
@@ -13,9 +13,8 @@
"""
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
-
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import matplotlib.pyplot as plt
import numpy as np
@@ -31,6 +30,11 @@
class LShapeFitting:
+ """
+ LShapeFitting class. You can use this class by initializing the class and
+ changing the parameters, and then calling the fitting method.
+
+ """
class Criteria(Enum):
AREA = 1
@@ -38,15 +42,35 @@ class Criteria(Enum):
VARIANCE = 3
def __init__(self):
- # Parameters
+ """
+ Default parameter settings
+ """
+ #: Fitting criteria parameter
self.criteria = self.Criteria.VARIANCE
- self.min_dist_of_closeness_criteria = 0.01 # [m]
- self.d_theta_deg_for_search = 1.0 # [deg]
- self.R0 = 3.0 # [m] range segmentation param
- self.Rd = 0.001 # [m] range segmentation param
+ #: Minimum distance for closeness criteria parameter [m]
+ self.min_dist_of_closeness_criteria = 0.01
+ #: Angle difference parameter [deg]
+ self.d_theta_deg_for_search = 1.0
+ #: Range segmentation parameter [m]
+ self.R0 = 3.0
+ #: Range segmentation parameter [m]
+ self.Rd = 0.001
def fitting(self, ox, oy):
+ """
+ Fitting L-shape model to object points
+
+ Parameters
+ ----------
+ ox : x positions of range points from an object
+ oy : y positions of range points from an object
+ Returns
+ -------
+ rects: Fitting rectangles
+ id_sets: id sets of each cluster
+
+ """
# step1: Adaptive Range Segmentation
id_sets = self._adoptive_range_segmentation(ox, oy)
@@ -61,56 +85,53 @@ def fitting(self, ox, oy):
@staticmethod
def _calc_area_criterion(c1, c2):
- c1_max = max(c1)
- c2_max = max(c2)
- c1_min = min(c1)
- c2_min = min(c2)
-
+ c1_max, c1_min, c2_max, c2_min = LShapeFitting._find_min_max(c1, c2)
alpha = -(c1_max - c1_min) * (c2_max - c2_min)
-
return alpha
def _calc_closeness_criterion(self, c1, c2):
- c1_max = max(c1)
- c2_max = max(c2)
- c1_min = min(c1)
- c2_min = min(c2)
+ c1_max, c1_min, c2_max, c2_min = LShapeFitting._find_min_max(c1, c2)
# Vectorization
- D1 = np.minimum(c1_max - c1, c1 - c1_min)
- D2 = np.minimum(c2_max - c2, c2 - c2_min)
- d = np.maximum(np.minimum(D1, D2), self.min_dist_of_closeness_criteria)
+ d1 = np.minimum(c1_max - c1, c1 - c1_min)
+ d2 = np.minimum(c2_max - c2, c2 - c2_min)
+ d = np.maximum(np.minimum(d1, d2), self.min_dist_of_closeness_criteria)
beta = (1.0 / d).sum()
return beta
@staticmethod
def _calc_variance_criterion(c1, c2):
- c1_max = max(c1)
- c2_max = max(c2)
- c1_min = min(c1)
- c2_min = min(c2)
+ c1_max, c1_min, c2_max, c2_min = LShapeFitting._find_min_max(c1, c2)
# Vectorization
- D1 = np.minimum(c1_max - c1, c1 - c1_min)
- D2 = np.minimum(c2_max - c2, c2 - c2_min)
- E1 = D1[D1 < D2]
- E2 = D2[D1 >= D2]
- V1 = - np.var(E1) if len(E1) > 0 else 0.
- V2 = - np.var(E2) if len(E2) > 0 else 0.
- gamma = V1 + V2
+ d1 = np.minimum(c1_max - c1, c1 - c1_min)
+ d2 = np.minimum(c2_max - c2, c2 - c2_min)
+ e1 = d1[d1 < d2]
+ e2 = d2[d1 >= d2]
+ v1 = - np.var(e1) if len(e1) > 0 else 0.
+ v2 = - np.var(e2) if len(e2) > 0 else 0.
+ gamma = v1 + v2
return gamma
+ @staticmethod
+ def _find_min_max(c1, c2):
+ c1_max = max(c1)
+ c2_max = max(c2)
+ c1_min = min(c1)
+ c2_min = min(c2)
+ return c1_max, c1_min, c2_max, c2_min
+
def _rectangle_search(self, x, y):
- X = np.array([x, y]).T
+ xy = np.array([x, y]).T
d_theta = np.deg2rad(self.d_theta_deg_for_search)
min_cost = (-float('inf'), None)
for theta in np.arange(0.0, np.pi / 2.0 - d_theta, d_theta):
- c = X @ rot_mat_2d(theta)
+ c = xy @ rot_mat_2d(theta)
c1 = c[:, 0]
c2 = c[:, 1]
@@ -130,8 +151,8 @@ def _rectangle_search(self, x, y):
sin_s = np.sin(min_cost[1])
cos_s = np.cos(min_cost[1])
- c1_s = X @ np.array([cos_s, sin_s]).T
- c2_s = X @ np.array([-sin_s, cos_s]).T
+ c1_s = xy @ np.array([cos_s, sin_s]).T
+ c2_s = xy @ np.array([-sin_s, cos_s]).T
rect = RectangleData()
rect.a[0] = cos_s
@@ -152,28 +173,28 @@ def _rectangle_search(self, x, y):
def _adoptive_range_segmentation(self, ox, oy):
# Setup initial cluster
- S = []
+ segment_list = []
for i, _ in enumerate(ox):
- C = set()
- R = self.R0 + self.Rd * np.linalg.norm([ox[i], oy[i]])
+ c = set()
+ r = self.R0 + self.Rd * np.linalg.norm([ox[i], oy[i]])
for j, _ in enumerate(ox):
d = np.hypot(ox[i] - ox[j], oy[i] - oy[j])
- if d <= R:
- C.add(j)
- S.append(C)
+ if d <= r:
+ c.add(j)
+ segment_list.append(c)
# Merge cluster
- while 1:
+ while True:
no_change = True
- for (c1, c2) in list(itertools.permutations(range(len(S)), 2)):
- if S[c1] & S[c2]:
- S[c1] = (S[c1] | S.pop(c2))
+ for (c1, c2) in list(itertools.permutations(range(len(segment_list)), 2)):
+ if segment_list[c1] & segment_list[c2]:
+ segment_list[c1] = (segment_list[c1] | segment_list.pop(c2))
no_change = False
break
if no_change:
break
- return S
+ return segment_list
class RectangleData:
diff --git a/Mapping/rectangle_fitting/simulator.py b/Mapping/rectangle_fitting/simulator.py
index e75738c3b1..aa32ae1b1f 100644
--- a/Mapping/rectangle_fitting/simulator.py
+++ b/Mapping/rectangle_fitting/simulator.py
@@ -6,8 +6,8 @@
"""
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import numpy as np
import matplotlib.pyplot as plt
@@ -138,7 +138,3 @@ def ray_casting_filter(theta_l, range_l, angle_resolution):
ry.append(range_db[i] * np.sin(t))
return rx, ry
-
-
-if __name__ == '__main__':
- main()
diff --git a/MissionPlanning/BehaviorTree/behavior_tree.py b/MissionPlanning/BehaviorTree/behavior_tree.py
new file mode 100644
index 0000000000..9ad886aafb
--- /dev/null
+++ b/MissionPlanning/BehaviorTree/behavior_tree.py
@@ -0,0 +1,690 @@
+"""
+Behavior Tree
+
+author: Wang Zheng (@Aglargil)
+
+Reference:
+
+- [Behavior Tree](https://en.wikipedia.org/wiki/Behavior_tree_(artificial_intelligence,_robotics_and_control))
+"""
+
+import time
+import xml.etree.ElementTree as ET
+from enum import Enum
+
+
+class Status(Enum):
+ SUCCESS = "success"
+ FAILURE = "failure"
+ RUNNING = "running"
+
+
+class NodeType(Enum):
+ CONTROL_NODE = "ControlNode"
+ ACTION_NODE = "ActionNode"
+ DECORATOR_NODE = "DecoratorNode"
+
+
+class Node:
+ """
+ Base class for all nodes in a behavior tree.
+ """
+
+ def __init__(self, name):
+ self.name = name
+ self.status = None
+
+ def tick(self) -> Status:
+ """
+ Tick the node.
+
+ Returns:
+ Status: The status of the node.
+ """
+ raise ValueError("Node is not implemented")
+
+ def tick_and_set_status(self) -> Status:
+ """
+ Tick the node and set the status.
+
+ Returns:
+ Status: The status of the node.
+ """
+ self.status = self.tick()
+ return self.status
+
+ def reset(self):
+ """
+ Reset the node.
+ """
+ self.status = None
+
+ def reset_children(self):
+ """
+ Reset the children of the node.
+ """
+ pass
+
+
+class ControlNode(Node):
+ """
+ Base class for all control nodes in a behavior tree.
+
+ Control nodes manage the execution flow of their child nodes according to specific rules.
+ They typically have multiple children and determine which children to execute and in what order.
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.children = []
+ self.type = NodeType.CONTROL_NODE
+
+ def not_set_children_raise_error(self):
+ if len(self.children) == 0:
+ raise ValueError("Children are not set")
+
+ def reset_children(self):
+ for child in self.children:
+ child.reset()
+
+
+class SequenceNode(ControlNode):
+ """
+ Executes child nodes in sequence until one fails or all succeed.
+
+ Returns:
+ - Returns FAILURE if any child returns FAILURE
+ - Returns SUCCESS when all children have succeeded
+ - Returns RUNNING when a child is still running or when moving to the next child
+
+ Example:
+ .. code-block:: xml
+
+
+
+
+
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.current_child_index = 0
+
+ def tick(self) -> Status:
+ self.not_set_children_raise_error()
+
+ if self.current_child_index >= len(self.children):
+ self.reset_children()
+ return Status.SUCCESS
+ status = self.children[self.current_child_index].tick_and_set_status()
+ if status == Status.FAILURE:
+ self.reset_children()
+ return Status.FAILURE
+ elif status == Status.SUCCESS:
+ self.current_child_index += 1
+ return Status.RUNNING
+ elif status == Status.RUNNING:
+ return Status.RUNNING
+ else:
+ raise ValueError("Unknown status")
+
+
+class SelectorNode(ControlNode):
+ """
+ Executes child nodes in sequence until one succeeds or all fail.
+
+ Returns:
+ - Returns SUCCESS if any child returns SUCCESS
+ - Returns FAILURE when all children have failed
+ - Returns RUNNING when a child is still running or when moving to the next child
+
+ Examples:
+ .. code-block:: xml
+
+
+
+
+
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.current_child_index = 0
+
+ def tick(self) -> Status:
+ self.not_set_children_raise_error()
+
+ if self.current_child_index >= len(self.children):
+ self.reset_children()
+ return Status.FAILURE
+ status = self.children[self.current_child_index].tick_and_set_status()
+ if status == Status.SUCCESS:
+ self.reset_children()
+ return Status.SUCCESS
+ elif status == Status.FAILURE:
+ self.current_child_index += 1
+ return Status.RUNNING
+ elif status == Status.RUNNING:
+ return Status.RUNNING
+ else:
+ raise ValueError("Unknown status")
+
+
+class WhileDoElseNode(ControlNode):
+ """
+ Conditional execution node with three parts: condition, do, and optional else.
+
+ Returns:
+ First executes the condition node (child[0])
+ If condition succeeds, executes do node (child[1]) and returns RUNNING
+ If condition fails, executes else node (child[2]) if present and returns result of else node
+ If condition fails and there is no else node, returns SUCCESS
+
+ Example:
+ .. code-block:: xml
+
+
+
+
+
+
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ if len(self.children) != 3 and len(self.children) != 2:
+ raise ValueError("WhileDoElseNode must have exactly 3 or 2 children")
+
+ condition_node = self.children[0]
+ do_node = self.children[1]
+ else_node = self.children[2] if len(self.children) == 3 else None
+
+ condition_status = condition_node.tick_and_set_status()
+ if condition_status == Status.SUCCESS:
+ do_node.tick_and_set_status()
+ return Status.RUNNING
+ elif condition_status == Status.FAILURE:
+ if else_node is not None:
+ else_status = else_node.tick_and_set_status()
+ if else_status == Status.SUCCESS:
+ self.reset_children()
+ return Status.SUCCESS
+ elif else_status == Status.FAILURE:
+ self.reset_children()
+ return Status.FAILURE
+ elif else_status == Status.RUNNING:
+ return Status.RUNNING
+ else:
+ raise ValueError("Unknown status")
+ else:
+ self.reset_children()
+ return Status.SUCCESS
+ else:
+ raise ValueError("Unknown status")
+
+
+class ActionNode(Node):
+ """
+ Base class for all action nodes in a behavior tree.
+
+ Action nodes are responsible for performing specific tasks or actions.
+ They do not have children and are typically used to execute logic or operations.
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.type = NodeType.ACTION_NODE
+
+
+class SleepNode(ActionNode):
+ """
+ Sleep node that sleeps for a specified duration.
+
+ Returns:
+ Returns SUCCESS after the specified duration has passed
+ Returns RUNNING if the duration has not yet passed
+
+ Example:
+ .. code-block:: xml
+
+
+ """
+
+ def __init__(self, name, duration):
+ super().__init__(name)
+ self.duration = duration
+ self.start_time = None
+
+ def tick(self) -> Status:
+ if self.start_time is None:
+ self.start_time = time.time()
+ if time.time() - self.start_time > self.duration:
+ return Status.SUCCESS
+ return Status.RUNNING
+
+
+class EchoNode(ActionNode):
+ """
+ Echo node that prints a message to the console.
+
+ Returns:
+ Returns SUCCESS after the message has been printed
+
+ Example:
+ .. code-block:: xml
+
+
+ """
+
+ def __init__(self, name, message):
+ super().__init__(name)
+ self.message = message
+
+ def tick(self) -> Status:
+ print(self.name, self.message)
+ return Status.SUCCESS
+
+
+class DecoratorNode(Node):
+ """
+ Base class for all decorator nodes in a behavior tree.
+
+ Decorator nodes modify the behavior of their child node.
+ They must have a single child and can alter the status of the child node.
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.type = NodeType.DECORATOR_NODE
+ self.child = None
+
+ def not_set_child_raise_error(self):
+ if self.child is None:
+ raise ValueError("Child is not set")
+
+ def reset_children(self):
+ self.child.reset()
+
+
+class InverterNode(DecoratorNode):
+ """
+ Inverter node that inverts the status of its child node.
+
+ Returns:
+ - Returns SUCCESS if the child returns FAILURE
+ - Returns FAILURE if the child returns SUCCESS
+ - Returns RUNNING if the child returns RUNNING
+
+ Examples:
+ .. code-block:: xml
+
+
+
+
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ status = self.child.tick_and_set_status()
+ return Status.SUCCESS if status == Status.FAILURE else Status.FAILURE
+
+
+class TimeoutNode(DecoratorNode):
+ """
+ Timeout node that fails if the child node takes too long to execute
+
+ Returns:
+ - FAILURE: If the timeout duration has been exceeded
+ - Child's status: Otherwise, passes through the status of the child node
+
+ Example:
+ .. code-block:: xml
+
+
+
+
+ """
+
+ def __init__(self, name, timeout):
+ super().__init__(name)
+ self.timeout = timeout
+ self.start_time = None
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ if self.start_time is None:
+ self.start_time = time.time()
+ if time.time() - self.start_time > self.timeout:
+ return Status.FAILURE
+ print(f"{self.name} is running")
+ return self.child.tick_and_set_status()
+
+
+class DelayNode(DecoratorNode):
+ """
+ Delay node that delays the execution of its child node for a specified duration.
+
+ Returns:
+ - Returns RUNNING if the duration has not yet passed
+ - Returns child's status after the duration has passed
+
+ Example:
+ .. code-block:: xml
+
+
+
+
+ """
+
+ def __init__(self, name, delay):
+ super().__init__(name)
+ self.delay = delay
+ self.start_time = None
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ if self.start_time is None:
+ self.start_time = time.time()
+ if time.time() - self.start_time > self.delay:
+ return self.child.tick_and_set_status()
+ return Status.RUNNING
+
+
+class ForceSuccessNode(DecoratorNode):
+ """
+ ForceSuccess node that always returns SUCCESS.
+
+ Returns:
+ - Returns RUNNING if the child returns RUNNING
+ - Returns SUCCESS if the child returns SUCCESS or FAILURE
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ status = self.child.tick_and_set_status()
+ if status == Status.FAILURE:
+ return Status.SUCCESS
+ return status
+
+
+class ForceFailureNode(DecoratorNode):
+ """
+ ForceFailure node that always returns FAILURE.
+
+ Returns:
+ - Returns RUNNING if the child returns RUNNING
+ - Returns FAILURE if the child returns SUCCESS or FAILURE
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ status = self.child.tick_and_set_status()
+ if status == Status.SUCCESS:
+ return Status.FAILURE
+ return status
+
+
+class BehaviorTree:
+ """
+ Behavior tree class that manages the execution of a behavior tree.
+ """
+
+ def __init__(self, root):
+ self.root = root
+
+ def tick(self):
+ """
+ Tick once on the behavior tree.
+ """
+ self.root.tick_and_set_status()
+
+ def reset(self):
+ """
+ Reset the behavior tree.
+ """
+ self.root.reset()
+
+ def tick_while_running(self, interval=None, enable_print=True):
+ """
+ Tick the behavior tree while it is running.
+
+ Args:
+ interval (float, optional): The interval between ticks. Defaults to None.
+ enable_print (bool, optional): Whether to print the behavior tree. Defaults to True.
+ """
+ while self.root.tick_and_set_status() == Status.RUNNING:
+ if enable_print:
+ self.print_tree()
+ if interval is not None:
+ time.sleep(interval)
+ if enable_print:
+ self.print_tree()
+
+ def to_text(self, root, indent=0):
+ """
+ Recursively convert the behavior tree to a text representation.
+ """
+ current_text = ""
+ if root.status == Status.RUNNING:
+ # yellow
+ current_text = "\033[93m" + root.name + "\033[0m"
+ elif root.status == Status.SUCCESS:
+ # green
+ current_text = "\033[92m" + root.name + "\033[0m"
+ elif root.status == Status.FAILURE:
+ # red
+ current_text = "\033[91m" + root.name + "\033[0m"
+ else:
+ current_text = root.name
+ if root.type == NodeType.CONTROL_NODE:
+ current_text = " " * indent + "[" + current_text + "]\n"
+ for child in root.children:
+ current_text += self.to_text(child, indent + 2)
+ elif root.type == NodeType.DECORATOR_NODE:
+ current_text = " " * indent + "(" + current_text + ")\n"
+ current_text += self.to_text(root.child, indent + 2)
+ elif root.type == NodeType.ACTION_NODE:
+ current_text = " " * indent + "<" + current_text + ">\n"
+ return current_text
+
+ def print_tree(self):
+ """
+ Print the behavior tree.
+
+ Node print format:
+ Action:
+ Decorator: (Decorator)
+ Control: [Control]
+
+ Node status colors:
+ Yellow: RUNNING
+ Green: SUCCESS
+ Red: FAILURE
+ """
+ text = self.to_text(self.root)
+ text = text.strip()
+ print("\033[94m" + "Behavior Tree" + "\033[0m")
+ print(text)
+ print("\033[94m" + "Behavior Tree" + "\033[0m")
+
+
+class BehaviorTreeFactory:
+ """
+ Factory class for creating behavior trees from XML strings.
+ """
+
+ def __init__(self):
+ self.node_builders = {}
+ # Control nodes
+ self.register_node_builder(
+ "Sequence",
+ lambda node: SequenceNode(node.attrib.get("name", SequenceNode.__name__)),
+ )
+ self.register_node_builder(
+ "Selector",
+ lambda node: SelectorNode(node.attrib.get("name", SelectorNode.__name__)),
+ )
+ self.register_node_builder(
+ "WhileDoElse",
+ lambda node: WhileDoElseNode(
+ node.attrib.get("name", WhileDoElseNode.__name__)
+ ),
+ )
+ # Decorator nodes
+ self.register_node_builder(
+ "Inverter",
+ lambda node: InverterNode(node.attrib.get("name", InverterNode.__name__)),
+ )
+ self.register_node_builder(
+ "Timeout",
+ lambda node: TimeoutNode(
+ node.attrib.get("name", SelectorNode.__name__),
+ float(node.attrib["sec"]),
+ ),
+ )
+ self.register_node_builder(
+ "Delay",
+ lambda node: DelayNode(
+ node.attrib.get("name", DelayNode.__name__),
+ float(node.attrib["sec"]),
+ ),
+ )
+ self.register_node_builder(
+ "ForceSuccess",
+ lambda node: ForceSuccessNode(
+ node.attrib.get("name", ForceSuccessNode.__name__)
+ ),
+ )
+ self.register_node_builder(
+ "ForceFailure",
+ lambda node: ForceFailureNode(
+ node.attrib.get("name", ForceFailureNode.__name__)
+ ),
+ )
+ # Action nodes
+ self.register_node_builder(
+ "Sleep",
+ lambda node: SleepNode(
+ node.attrib.get("name", SleepNode.__name__),
+ float(node.attrib["sec"]),
+ ),
+ )
+ self.register_node_builder(
+ "Echo",
+ lambda node: EchoNode(
+ node.attrib.get("name", EchoNode.__name__),
+ node.attrib["message"],
+ ),
+ )
+
+ def register_node_builder(self, node_name, builder):
+ """
+ Register a builder for a node
+
+ Args:
+ node_name (str): The name of the node.
+ builder (function): The builder function.
+
+ Example:
+ .. code-block:: python
+
+ factory = BehaviorTreeFactory()
+ factory.register_node_builder(
+ "MyNode",
+ lambda node: MyNode(
+ node.attrib.get("name", MyNode.__name__),
+ node.attrib["my_param"],
+ ),
+ )
+ """
+ self.node_builders[node_name] = builder
+
+ def build_node(self, node):
+ """
+ Build a node from an XML element.
+
+ Args:
+ node (Element): The XML element to build the node from.
+
+ Returns:
+ BehaviorTree Node: the built node
+ """
+ if node.tag in self.node_builders:
+ root = self.node_builders[node.tag](node)
+ if root.type == NodeType.CONTROL_NODE:
+ if len(node) <= 0:
+ raise ValueError(f"{root.name} Control node must have children")
+ for child in node:
+ root.children.append(self.build_node(child))
+ elif root.type == NodeType.DECORATOR_NODE:
+ if len(node) != 1:
+ raise ValueError(
+ f"{root.name} Decorator node must have exactly one child"
+ )
+ root.child = self.build_node(node[0])
+ elif root.type == NodeType.ACTION_NODE:
+ if len(node) != 0:
+ raise ValueError(f"{root.name} Action node must have no children")
+ return root
+ else:
+ raise ValueError(f"Unknown node type: {node.tag}")
+
+ def build_tree(self, xml_string):
+ """
+ Build a behavior tree from an XML string.
+
+ Args:
+ xml_string (str): The XML string containing the behavior tree.
+
+ Returns:
+ BehaviorTree: The behavior tree.
+ """
+ xml_tree = ET.fromstring(xml_string)
+ root = self.build_node(xml_tree)
+ return BehaviorTree(root)
+
+ def build_tree_from_file(self, file_path):
+ """
+ Build a behavior tree from a file.
+
+ Args:
+ file_path (str): The path to the file containing the behavior tree.
+
+ Returns:
+ BehaviorTree: The behavior tree.
+ """
+ with open(file_path) as file:
+ xml_string = file.read()
+ return self.build_tree(xml_string)
+
+
+xml_string = """
+
+
+
+
+
+
+
+ """
+
+
+def main():
+ factory = BehaviorTreeFactory()
+ tree = factory.build_tree(xml_string)
+ tree.tick_while_running()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/MissionPlanning/BehaviorTree/robot_behavior_case.py b/MissionPlanning/BehaviorTree/robot_behavior_case.py
new file mode 100644
index 0000000000..6c39aa76b2
--- /dev/null
+++ b/MissionPlanning/BehaviorTree/robot_behavior_case.py
@@ -0,0 +1,247 @@
+"""
+Robot Behavior Tree Case
+
+This file demonstrates how to use a behavior tree to control robot behavior.
+"""
+
+from behavior_tree import (
+ BehaviorTreeFactory,
+ Status,
+ ActionNode,
+)
+import time
+import random
+import os
+
+
+class CheckBatteryNode(ActionNode):
+ """
+ Node to check robot battery level
+
+ If battery level is below threshold, returns FAILURE, otherwise returns SUCCESS
+ """
+
+ def __init__(self, name, threshold=20):
+ super().__init__(name)
+ self.threshold = threshold
+ self.battery_level = 100 # Initial battery level is 100%
+
+ def tick(self):
+ # Simulate battery level decreasing
+ self.battery_level -= random.randint(1, 5)
+ print(f"Current battery level: {self.battery_level}%")
+
+ if self.battery_level <= self.threshold:
+ return Status.FAILURE
+ return Status.SUCCESS
+
+
+class ChargeBatteryNode(ActionNode):
+ """
+ Node to charge the robot's battery
+ """
+
+ def __init__(self, name, charge_rate=10):
+ super().__init__(name)
+ self.charge_rate = charge_rate
+ self.charging_time = 0
+
+ def tick(self):
+ # Simulate charging process
+ if self.charging_time == 0:
+ print("Starting to charge...")
+
+ self.charging_time += 1
+ charge_amount = self.charge_rate * self.charging_time
+
+ if charge_amount >= 100:
+ print("Charging complete! Battery level: 100%")
+ self.charging_time = 0
+ return Status.SUCCESS
+ else:
+ print(f"Charging in progress... Battery level: {min(charge_amount, 100)}%")
+ return Status.RUNNING
+
+
+class MoveToPositionNode(ActionNode):
+ """
+ Node to move to a specified position
+ """
+
+ def __init__(self, name, position, move_duration=2):
+ super().__init__(name)
+ self.position = position
+ self.move_duration = move_duration
+ self.start_time = None
+
+ def tick(self):
+ if self.start_time is None:
+ self.start_time = time.time()
+ print(f"Starting movement to position {self.position}")
+
+ elapsed_time = time.time() - self.start_time
+
+ if elapsed_time >= self.move_duration:
+ print(f"Arrived at position {self.position}")
+ self.start_time = None
+ return Status.SUCCESS
+ else:
+ print(
+ f"Moving to position {self.position}... {int(elapsed_time / self.move_duration * 100)}% complete"
+ )
+ return Status.RUNNING
+
+
+class DetectObstacleNode(ActionNode):
+ """
+ Node to detect obstacles
+ """
+
+ def __init__(self, name, obstacle_probability=0.3):
+ super().__init__(name)
+ self.obstacle_probability = obstacle_probability
+
+ def tick(self):
+ # Use random probability to simulate obstacle detection
+ if random.random() < self.obstacle_probability:
+ print("Obstacle detected!")
+ return Status.SUCCESS
+ else:
+ print("No obstacle detected")
+ return Status.FAILURE
+
+
+class AvoidObstacleNode(ActionNode):
+ """
+ Node to avoid obstacles
+ """
+
+ def __init__(self, name, avoid_duration=1.5):
+ super().__init__(name)
+ self.avoid_duration = avoid_duration
+ self.start_time = None
+
+ def tick(self):
+ if self.start_time is None:
+ self.start_time = time.time()
+ print("Starting obstacle avoidance...")
+
+ elapsed_time = time.time() - self.start_time
+
+ if elapsed_time >= self.avoid_duration:
+ print("Obstacle avoidance complete")
+ self.start_time = None
+ return Status.SUCCESS
+ else:
+ print("Avoiding obstacle...")
+ return Status.RUNNING
+
+
+class PerformTaskNode(ActionNode):
+ """
+ Node to perform a specific task
+ """
+
+ def __init__(self, name, task_name, task_duration=3):
+ super().__init__(name)
+ self.task_name = task_name
+ self.task_duration = task_duration
+ self.start_time = None
+
+ def tick(self):
+ if self.start_time is None:
+ self.start_time = time.time()
+ print(f"Starting task: {self.task_name}")
+
+ elapsed_time = time.time() - self.start_time
+
+ if elapsed_time >= self.task_duration:
+ print(f"Task complete: {self.task_name}")
+ self.start_time = None
+ return Status.SUCCESS
+ else:
+ print(
+ f"Performing task: {self.task_name}... {int(elapsed_time / self.task_duration * 100)}% complete"
+ )
+ return Status.RUNNING
+
+
+def create_robot_behavior_tree():
+ """
+ Create robot behavior tree
+ """
+
+ factory = BehaviorTreeFactory()
+
+ # Register custom nodes
+ factory.register_node_builder(
+ "CheckBattery",
+ lambda node: CheckBatteryNode(
+ node.attrib.get("name", "CheckBattery"),
+ int(node.attrib.get("threshold", "20")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "ChargeBattery",
+ lambda node: ChargeBatteryNode(
+ node.attrib.get("name", "ChargeBattery"),
+ int(node.attrib.get("charge_rate", "10")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "MoveToPosition",
+ lambda node: MoveToPositionNode(
+ node.attrib.get("name", "MoveToPosition"),
+ node.attrib.get("position", "Unknown Position"),
+ float(node.attrib.get("move_duration", "2")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "DetectObstacle",
+ lambda node: DetectObstacleNode(
+ node.attrib.get("name", "DetectObstacle"),
+ float(node.attrib.get("obstacle_probability", "0.3")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "AvoidObstacle",
+ lambda node: AvoidObstacleNode(
+ node.attrib.get("name", "AvoidObstacle"),
+ float(node.attrib.get("avoid_duration", "1.5")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "PerformTask",
+ lambda node: PerformTaskNode(
+ node.attrib.get("name", "PerformTask"),
+ node.attrib.get("task_name", "Unknown Task"),
+ float(node.attrib.get("task_duration", "3")),
+ ),
+ )
+ # Read XML from file
+ xml_path = os.path.join(os.path.dirname(__file__), "robot_behavior_tree.xml")
+ return factory.build_tree_from_file(xml_path)
+
+
+def main():
+ """
+ Main function: Create and run the robot behavior tree
+ """
+ print("Creating robot behavior tree...")
+ tree = create_robot_behavior_tree()
+
+ print("\nStarting robot behavior tree execution...\n")
+ # Run for a period of time or until completion
+
+ tree.tick_while_running(interval=0.01)
+
+ print("\nBehavior tree execution complete!")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/MissionPlanning/BehaviorTree/robot_behavior_tree.xml b/MissionPlanning/BehaviorTree/robot_behavior_tree.xml
new file mode 100644
index 0000000000..0bca76a3ff
--- /dev/null
+++ b/MissionPlanning/BehaviorTree/robot_behavior_tree.xml
@@ -0,0 +1,57 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/MissionPlanning/StateMachine/robot_behavior_case.py b/MissionPlanning/StateMachine/robot_behavior_case.py
new file mode 100644
index 0000000000..03ee60ae9f
--- /dev/null
+++ b/MissionPlanning/StateMachine/robot_behavior_case.py
@@ -0,0 +1,111 @@
+"""
+A case study of robot behavior using state machine
+
+author: Wang Zheng (@Aglargil)
+"""
+
+from state_machine import StateMachine
+
+
+class Robot:
+ def __init__(self):
+ self.battery = 100
+ self.task_progress = 0
+
+ # Initialize state machine
+ self.machine = StateMachine("robot_sm", self)
+
+ # Add state transition rules
+ self.machine.add_transition(
+ src_state="patrolling",
+ event="detect_task",
+ dst_state="executing_task",
+ guard=None,
+ action=None,
+ )
+
+ self.machine.add_transition(
+ src_state="executing_task",
+ event="task_complete",
+ dst_state="patrolling",
+ guard=None,
+ action="reset_task",
+ )
+
+ self.machine.add_transition(
+ src_state="executing_task",
+ event="low_battery",
+ dst_state="returning_to_base",
+ guard="is_battery_low",
+ )
+
+ self.machine.add_transition(
+ src_state="returning_to_base",
+ event="reach_base",
+ dst_state="charging",
+ guard=None,
+ action=None,
+ )
+
+ self.machine.add_transition(
+ src_state="charging",
+ event="charge_complete",
+ dst_state="patrolling",
+ guard=None,
+ action="battery_full",
+ )
+
+ # Set initial state
+ self.machine.set_current_state("patrolling")
+
+ def is_battery_low(self):
+ """Battery level check condition"""
+ return self.battery < 30
+
+ def reset_task(self):
+ """Reset task progress"""
+ self.task_progress = 0
+ print("[Action] Task progress has been reset")
+
+ # Modify state entry callback naming convention (add state_ prefix)
+ def on_enter_executing_task(self):
+ print("\n------ Start Executing Task ------")
+ print(f"Current battery: {self.battery}%")
+ while self.machine.get_current_state().name == "executing_task":
+ self.task_progress += 10
+ self.battery -= 25
+ print(
+ f"Task progress: {self.task_progress}%, Remaining battery: {self.battery}%"
+ )
+
+ if self.task_progress >= 100:
+ self.machine.process("task_complete")
+ break
+ elif self.is_battery_low():
+ self.machine.process("low_battery")
+ break
+
+ def on_enter_returning_to_base(self):
+ print("\nLow battery, returning to charging station...")
+ self.machine.process("reach_base")
+
+ def on_enter_charging(self):
+ print("\n------ Charging ------")
+ self.battery = 100
+ print("Charging complete!")
+ self.machine.process("charge_complete")
+
+
+# Keep the test section structure the same, only modify the trigger method
+if __name__ == "__main__":
+ robot = Robot()
+ print(robot.machine.generate_plantuml())
+
+ print(f"Initial state: {robot.machine.get_current_state().name}")
+ print("------------")
+
+ # Trigger task detection event
+ robot.machine.process("detect_task")
+
+ print("\n------------")
+ print(f"Final state: {robot.machine.get_current_state().name}")
diff --git a/MissionPlanning/StateMachine/state_machine.py b/MissionPlanning/StateMachine/state_machine.py
new file mode 100644
index 0000000000..454759236e
--- /dev/null
+++ b/MissionPlanning/StateMachine/state_machine.py
@@ -0,0 +1,294 @@
+"""
+State Machine
+
+author: Wang Zheng (@Aglargil)
+
+Reference:
+
+- [State Machine]
+(https://en.wikipedia.org/wiki/Finite-state_machine)
+"""
+
+import string
+from urllib.request import urlopen, Request
+from base64 import b64encode
+from zlib import compress
+from io import BytesIO
+from collections.abc import Callable
+from matplotlib.image import imread
+from matplotlib import pyplot as plt
+
+
+def deflate_and_encode(plantuml_text):
+ """
+ zlib compress the plantuml text and encode it for the plantuml server.
+
+ Reference https://plantuml.com/en/text-encoding
+ """
+ plantuml_alphabet = (
+ string.digits + string.ascii_uppercase + string.ascii_lowercase + "-_"
+ )
+ base64_alphabet = (
+ string.ascii_uppercase + string.ascii_lowercase + string.digits + "+/"
+ )
+ b64_to_plantuml = bytes.maketrans(
+ base64_alphabet.encode("utf-8"), plantuml_alphabet.encode("utf-8")
+ )
+ zlibbed_str = compress(plantuml_text.encode("utf-8"))
+ compressed_string = zlibbed_str[2:-4]
+ return b64encode(compressed_string).translate(b64_to_plantuml).decode("utf-8")
+
+
+class State:
+ def __init__(self, name, on_enter=None, on_exit=None):
+ self.name = name
+ self.on_enter = on_enter
+ self.on_exit = on_exit
+
+ def enter(self):
+ print(f"entering <{self.name}>")
+ if self.on_enter:
+ self.on_enter()
+
+ def exit(self):
+ print(f"exiting <{self.name}>")
+ if self.on_exit:
+ self.on_exit()
+
+
+class StateMachine:
+ def __init__(self, name: str, model=object):
+ """Initialize the state machine.
+
+ Args:
+ name (str): Name of the state machine.
+ model (object, optional): Model object used to automatically look up callback functions
+ for states and transitions:
+ State callbacks: Automatically searches for 'on_enter_' and 'on_exit_' methods.
+ Transition callbacks: When action or guard parameters are strings, looks up corresponding methods in the model.
+
+ Example:
+ >>> class MyModel:
+ ... def on_enter_idle(self):
+ ... print("Entering idle state")
+ ... def on_exit_idle(self):
+ ... print("Exiting idle state")
+ ... def can_start(self):
+ ... return True
+ ... def on_start(self):
+ ... print("Starting operation")
+ >>> model = MyModel()
+ >>> machine = StateMachine("my_machine", model)
+ """
+ self._name = name
+ self._states = {}
+ self._events = {}
+ self._transition_table = {}
+ self._model = model
+ self._state: State = None
+
+ def _register_event(self, event: str):
+ self._events[event] = event
+
+ def _get_state(self, name):
+ return self._states[name]
+
+ def _get_event(self, name):
+ return self._events[name]
+
+ def _has_event(self, event: str):
+ return event in self._events
+
+ def add_transition(
+ self,
+ src_state: str | State,
+ event: str,
+ dst_state: str | State,
+ guard: str | Callable = None,
+ action: str | Callable = None,
+ ) -> None:
+ """Add a transition to the state machine.
+
+ Args:
+ src_state (str | State): The source state where the transition begins.
+ Can be either a state name or a State object.
+ event (str): The event that triggers this transition.
+ dst_state (str | State): The destination state where the transition ends.
+ Can be either a state name or a State object.
+ guard (str | Callable, optional): Guard condition for the transition.
+ If callable: Function that returns bool.
+ If str: Name of a method in the model class.
+ If returns True: Transition proceeds.
+ If returns False: Transition is skipped.
+ action (str | Callable, optional): Action to execute during transition.
+ If callable: Function to execute.
+ If str: Name of a method in the model class.
+ Executed after guard passes and before entering new state.
+
+ Example:
+ >>> machine.add_transition(
+ ... src_state="idle",
+ ... event="start",
+ ... dst_state="running",
+ ... guard="can_start",
+ ... action="on_start"
+ ... )
+ """
+ # Convert string parameters to objects if necessary
+ self.register_state(src_state)
+ self._register_event(event)
+ self.register_state(dst_state)
+
+ def get_state_obj(state):
+ return state if isinstance(state, State) else self._get_state(state)
+
+ def get_callable(func):
+ return func if callable(func) else getattr(self._model, func, None)
+
+ src_state_obj = get_state_obj(src_state)
+ dst_state_obj = get_state_obj(dst_state)
+
+ guard_func = get_callable(guard) if guard else None
+ action_func = get_callable(action) if action else None
+ self._transition_table[(src_state_obj.name, event)] = (
+ dst_state_obj,
+ guard_func,
+ action_func,
+ )
+
+ def state_transition(self, src_state: State, event: str):
+ if (src_state.name, event) not in self._transition_table:
+ raise ValueError(
+ f"|{self._name}| invalid transition: <{src_state.name}> : [{event}]"
+ )
+
+ dst_state, guard, action = self._transition_table[(src_state.name, event)]
+
+ def call_guard(guard):
+ if callable(guard):
+ return guard()
+ else:
+ return True
+
+ def call_action(action):
+ if callable(action):
+ action()
+
+ if call_guard(guard):
+ call_action(action)
+ if src_state.name != dst_state.name:
+ print(
+ f"|{self._name}| transitioning from <{src_state.name}> to <{dst_state.name}>"
+ )
+ src_state.exit()
+ self._state = dst_state
+ dst_state.enter()
+ else:
+ print(
+ f"|{self._name}| skipping transition from <{src_state.name}> to <{dst_state.name}> because guard failed"
+ )
+
+ def register_state(self, state: str | State, on_enter=None, on_exit=None):
+ """Register a state in the state machine.
+
+ Args:
+ state (str | State): The state to register. Can be either a string (state name)
+ or a State object.
+ on_enter (Callable, optional): Callback function to be executed when entering the state.
+ If state is a string and on_enter is None, it will look for
+ a method named 'on_enter_' in the model.
+ on_exit (Callable, optional): Callback function to be executed when exiting the state.
+ If state is a string and on_exit is None, it will look for
+ a method named 'on_exit_' in the model.
+ Example:
+ >>> machine.register_state("idle", on_enter=on_enter_idle, on_exit=on_exit_idle)
+ >>> machine.register_state(State("running", on_enter=on_enter_running, on_exit=on_exit_running))
+ """
+ if isinstance(state, str):
+ if on_enter is None:
+ on_enter = getattr(self._model, "on_enter_" + state, None)
+ if on_exit is None:
+ on_exit = getattr(self._model, "on_exit_" + state, None)
+ self._states[state] = State(state, on_enter, on_exit)
+ return
+
+ self._states[state.name] = state
+
+ def set_current_state(self, state: State | str):
+ if isinstance(state, str):
+ self._state = self._get_state(state)
+ else:
+ self._state = state
+
+ def get_current_state(self):
+ return self._state
+
+ def process(self, event: str) -> None:
+ """Process an event in the state machine.
+
+ Args:
+ event: Event name.
+
+ Example:
+ >>> machine.process("start")
+ """
+ if self._state is None:
+ raise ValueError("State machine is not initialized")
+
+ if self._has_event(event):
+ self.state_transition(self._state, event)
+ else:
+ raise ValueError(f"Invalid event: {event}")
+
+ def generate_plantuml(self) -> str:
+ """Generate PlantUML state diagram representation of the state machine.
+
+ Returns:
+ str: PlantUML state diagram code.
+ """
+ if self._state is None:
+ raise ValueError("State machine is not initialized")
+
+ plant_uml = ["@startuml"]
+ plant_uml.append("[*] --> " + self._state.name)
+
+ # Generate transitions
+ for (src_state, event), (
+ dst_state,
+ guard,
+ action,
+ ) in self._transition_table.items():
+ transition = f"{src_state} --> {dst_state.name} : {event}"
+
+ # Add guard and action if present
+ conditions = []
+ if guard:
+ guard_name = guard.__name__ if callable(guard) else guard
+ conditions.append(f"[{guard_name}]")
+ if action:
+ action_name = action.__name__ if callable(action) else action
+ conditions.append(f"/ {action_name}")
+
+ if conditions:
+ transition += "\\n" + " ".join(conditions)
+
+ plant_uml.append(transition)
+
+ plant_uml.append("@enduml")
+ plant_uml_text = "\n".join(plant_uml)
+
+ try:
+ url = f"http://www.plantuml.com/plantuml/img/{deflate_and_encode(plant_uml_text)}"
+ headers = {"User-Agent": "Mozilla/5.0"}
+ request = Request(url, headers=headers)
+
+ with urlopen(request) as response:
+ content = response.read()
+
+ plt.imshow(imread(BytesIO(content), format="png"))
+ plt.axis("off")
+ plt.show()
+ except Exception as e:
+ print(f"Error showing PlantUML: {e}")
+
+ return plant_uml_text
diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py
index eb2672a9ce..6d20350432 100644
--- a/PathPlanning/AStar/a_star.py
+++ b/PathPlanning/AStar/a_star.py
@@ -71,7 +71,7 @@ def planning(self, sx, sy, gx, gy):
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(start_node)] = start_node
- while 1:
+ while True:
if len(open_set) == 0:
print("Open set is empty..")
break
diff --git a/PathPlanning/AStar/a_star_searching_from_two_side.py b/PathPlanning/AStar/a_star_searching_from_two_side.py
index 0318970cc8..f43cea71b4 100644
--- a/PathPlanning/AStar/a_star_searching_from_two_side.py
+++ b/PathPlanning/AStar/a_star_searching_from_two_side.py
@@ -78,43 +78,43 @@ def boundary_and_obstacles(start, goal, top_vertex, bottom_vertex, obs_number):
def find_neighbor(node, ob, closed):
- # generate neighbors in certain condition
- ob_list = ob.tolist()
- neighbor: list = []
+ # Convert obstacles to a set for faster lookup
+ ob_set = set(map(tuple, ob.tolist()))
+ neighbor_set = set()
+
+ # Generate neighbors within the 3x3 grid around the node
for x in range(node.coordinate[0] - 1, node.coordinate[0] + 2):
for y in range(node.coordinate[1] - 1, node.coordinate[1] + 2):
- if [x, y] not in ob_list:
- # find all possible neighbor nodes
- neighbor.append([x, y])
- # remove node violate the motion rule
- # 1. remove node.coordinate itself
- neighbor.remove(node.coordinate)
- # 2. remove neighbor nodes who cross through two diagonal
- # positioned obstacles since there is no enough space for
- # robot to go through two diagonal positioned obstacles
-
- # top bottom left right neighbors of node
- top_nei = [node.coordinate[0], node.coordinate[1] + 1]
- bottom_nei = [node.coordinate[0], node.coordinate[1] - 1]
- left_nei = [node.coordinate[0] - 1, node.coordinate[1]]
- right_nei = [node.coordinate[0] + 1, node.coordinate[1]]
- # neighbors in four vertex
- lt_nei = [node.coordinate[0] - 1, node.coordinate[1] + 1]
- rt_nei = [node.coordinate[0] + 1, node.coordinate[1] + 1]
- lb_nei = [node.coordinate[0] - 1, node.coordinate[1] - 1]
- rb_nei = [node.coordinate[0] + 1, node.coordinate[1] - 1]
-
- # remove the unnecessary neighbors
- if top_nei and left_nei in ob_list and lt_nei in neighbor:
- neighbor.remove(lt_nei)
- if top_nei and right_nei in ob_list and rt_nei in neighbor:
- neighbor.remove(rt_nei)
- if bottom_nei and left_nei in ob_list and lb_nei in neighbor:
- neighbor.remove(lb_nei)
- if bottom_nei and right_nei in ob_list and rb_nei in neighbor:
- neighbor.remove(rb_nei)
- neighbor = [x for x in neighbor if x not in closed]
- return neighbor
+ coord = (x, y)
+ if coord not in ob_set and coord != tuple(node.coordinate):
+ neighbor_set.add(coord)
+
+ # Define neighbors in cardinal and diagonal directions
+ top_nei = (node.coordinate[0], node.coordinate[1] + 1)
+ bottom_nei = (node.coordinate[0], node.coordinate[1] - 1)
+ left_nei = (node.coordinate[0] - 1, node.coordinate[1])
+ right_nei = (node.coordinate[0] + 1, node.coordinate[1])
+ lt_nei = (node.coordinate[0] - 1, node.coordinate[1] + 1)
+ rt_nei = (node.coordinate[0] + 1, node.coordinate[1] + 1)
+ lb_nei = (node.coordinate[0] - 1, node.coordinate[1] - 1)
+ rb_nei = (node.coordinate[0] + 1, node.coordinate[1] - 1)
+
+ # Remove neighbors that violate diagonal motion rules
+ if top_nei in ob_set and left_nei in ob_set:
+ neighbor_set.discard(lt_nei)
+ if top_nei in ob_set and right_nei in ob_set:
+ neighbor_set.discard(rt_nei)
+ if bottom_nei in ob_set and left_nei in ob_set:
+ neighbor_set.discard(lb_nei)
+ if bottom_nei in ob_set and right_nei in ob_set:
+ neighbor_set.discard(rb_nei)
+
+ # Filter out neighbors that are in the closed set
+ closed_set = set(map(tuple, closed))
+ neighbor_set -= closed_set
+
+ return list(neighbor_set)
+
def find_node_index(coordinate, node_list):
@@ -267,7 +267,7 @@ def draw_control(org_closed, goal_closed, flag, start, end, bound, obstacle):
if node_intersect: # a path is find
path = get_path(org_closed, goal_closed, node_intersect[0])
stop_loop = 1
- print('Path is find!')
+ print('Path found!')
if show_animation: # draw the path
plt.plot(path[:, 0], path[:, 1], '-r')
plt.title('Robot Arrived', size=20, loc='center')
diff --git a/PathPlanning/BSplinePath/bspline_path.py b/PathPlanning/BSplinePath/bspline_path.py
index d4b24c7645..a2a396efaa 100644
--- a/PathPlanning/BSplinePath/bspline_path.py
+++ b/PathPlanning/BSplinePath/bspline_path.py
@@ -5,57 +5,114 @@
author: Atsushi Sakai (@Atsushi_twi)
"""
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import numpy as np
import matplotlib.pyplot as plt
-import scipy.interpolate as scipy_interpolate
+import scipy.interpolate as interpolate
+from utils.plot import plot_curvature
-def approximate_b_spline_path(x: list, y: list, n_path_points: int,
- degree: int = 3) -> tuple:
- """
- approximate points with a B-Spline path
- :param x: x position list of approximated points
- :param y: y position list of approximated points
- :param n_path_points: number of path points
- :param degree: (Optional) B Spline curve degree
- :return: x and y position list of the result path
+def approximate_b_spline_path(x: list,
+ y: list,
+ n_path_points: int,
+ degree: int = 3,
+ s=None,
+ ) -> tuple:
"""
- t = range(len(x))
- x_tup = scipy_interpolate.splrep(t, x, k=degree)
- y_tup = scipy_interpolate.splrep(t, y, k=degree)
+ Approximate points with a B-Spline path
+
+ Parameters
+ ----------
+ x : array_like
+ x position list of approximated points
+ y : array_like
+ y position list of approximated points
+ n_path_points : int
+ number of path points
+ degree : int, optional
+ B Spline curve degree. Must be 2<= k <= 5. Default: 3.
+ s : int, optional
+ smoothing parameter. If this value is bigger, the path will be
+ smoother, but it will be less accurate. If this value is smaller,
+ the path will be more accurate, but it will be less smooth.
+ When `s` is 0, it is equivalent to the interpolation. Default is None,
+ in this case `s` will be `len(x)`.
+
+ Returns
+ -------
+ x : array
+ x positions of the result path
+ y : array
+ y positions of the result path
+ heading : array
+ heading of the result path
+ curvature : array
+ curvature of the result path
- x_list = list(x_tup)
- x_list[1] = x + [0.0, 0.0, 0.0, 0.0]
-
- y_list = list(y_tup)
- y_list[1] = y + [0.0, 0.0, 0.0, 0.0]
+ """
+ distances = _calc_distance_vector(x, y)
- ipl_t = np.linspace(0.0, len(x) - 1, n_path_points)
- rx = scipy_interpolate.splev(ipl_t, x_list)
- ry = scipy_interpolate.splev(ipl_t, y_list)
+ spl_i_x = interpolate.UnivariateSpline(distances, x, k=degree, s=s)
+ spl_i_y = interpolate.UnivariateSpline(distances, y, k=degree, s=s)
- return rx, ry
+ sampled = np.linspace(0.0, distances[-1], n_path_points)
+ return _evaluate_spline(sampled, spl_i_x, spl_i_y)
-def interpolate_b_spline_path(x: list, y: list, n_path_points: int,
+def interpolate_b_spline_path(x, y,
+ n_path_points: int,
degree: int = 3) -> tuple:
"""
- interpolate points with a B-Spline path
+ Interpolate x-y points with a B-Spline path
+
+ Parameters
+ ----------
+ x : array_like
+ x positions of interpolated points
+ y : array_like
+ y positions of interpolated points
+ n_path_points : int
+ number of path points
+ degree : int, optional
+ B-Spline degree. Must be 2<= k <= 5. Default: 3
+
+ Returns
+ -------
+ x : array
+ x positions of the result path
+ y : array
+ y positions of the result path
+ heading : array
+ heading of the result path
+ curvature : array
+ curvature of the result path
- :param x: x positions of interpolated points
- :param y: y positions of interpolated points
- :param n_path_points: number of path points
- :param degree: B-Spline degree
- :return: x and y position list of the result path
"""
- ipl_t = np.linspace(0.0, len(x) - 1, len(x))
- spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree)
- spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree)
+ return approximate_b_spline_path(x, y, n_path_points, degree, s=0.0)
- travel = np.linspace(0.0, len(x) - 1, n_path_points)
- return spl_i_x(travel), spl_i_y(travel)
+
+def _calc_distance_vector(x, y):
+ dx, dy = np.diff(x), np.diff(y)
+ distances = np.cumsum([np.hypot(idx, idy) for idx, idy in zip(dx, dy)])
+ distances = np.concatenate(([0.0], distances))
+ distances /= distances[-1]
+ return distances
+
+
+def _evaluate_spline(sampled, spl_i_x, spl_i_y):
+ x = spl_i_x(sampled)
+ y = spl_i_y(sampled)
+ dx = spl_i_x.derivative(1)(sampled)
+ dy = spl_i_y.derivative(1)(sampled)
+ heading = np.arctan2(dy, dx)
+ ddx = spl_i_x.derivative(2)(sampled)
+ ddy = spl_i_y.derivative(2)(sampled)
+ curvature = (ddy * dx - ddx * dy) / np.power(dx * dx + dy * dy, 2.0 / 3.0)
+ return np.array(x), y, heading, curvature,
def main():
@@ -63,17 +120,28 @@ def main():
# way points
way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]
way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]
- n_course_point = 100 # sampling number
+ n_course_point = 50 # sampling number
- rax, ray = approximate_b_spline_path(way_point_x, way_point_y,
- n_course_point)
- rix, riy = interpolate_b_spline_path(way_point_x, way_point_y,
- n_course_point)
+ plt.subplots()
+ rax, ray, heading, curvature = approximate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, s=0.5)
+ plt.plot(rax, ray, '-r', label="Approximated B-Spline path")
+ plot_curvature(rax, ray, heading, curvature)
- # show results
+ plt.title("B-Spline approximation")
plt.plot(way_point_x, way_point_y, '-og', label="way points")
- plt.plot(rax, ray, '-r', label="Approximated B-Spline path")
+ plt.grid(True)
+ plt.legend()
+ plt.axis("equal")
+
+ plt.subplots()
+ rix, riy, heading, curvature = interpolate_b_spline_path(
+ way_point_x, way_point_y, n_course_point)
plt.plot(rix, riy, '-b', label="Interpolated B-Spline path")
+ plot_curvature(rix, riy, heading, curvature)
+
+ plt.title("B-Spline interpolation")
+ plt.plot(way_point_x, way_point_y, '-og', label="way points")
plt.grid(True)
plt.legend()
plt.axis("equal")
diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py
similarity index 99%
rename from PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
rename to PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py
index b5eabbb39b..92c3a58761 100644
--- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
+++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py
@@ -23,7 +23,7 @@
show_animation = True
-class RTree(object):
+class RTree:
# Class to represent the explicit tree created
# while sampling through the state space
@@ -132,7 +132,7 @@ def node_id_to_real_world_coord(self, nid):
self.node_id_to_grid_coord(nid))
-class BITStar(object):
+class BITStar:
def __init__(self, start, goal,
obstacleList, randArea, eta=2.0,
@@ -189,7 +189,7 @@ def setup_planning(self):
[(self.start[1] + self.goal[1]) / 2.0], [0]])
a1 = np.array([[(self.goal[0] - self.start[0]) / cMin],
[(self.goal[1] - self.start[1]) / cMin], [0]])
- eTheta = math.atan2(a1[1], a1[0])
+ eTheta = math.atan2(a1[1, 0], a1[0, 0])
# first column of identity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
M = np.dot(a1, id1_t)
diff --git a/PathPlanning/BidirectionalAStar/bidirectional_a_star.py b/PathPlanning/BidirectionalAStar/bidirectional_a_star.py
index ecb875f410..5387cca1ad 100644
--- a/PathPlanning/BidirectionalAStar/bidirectional_a_star.py
+++ b/PathPlanning/BidirectionalAStar/bidirectional_a_star.py
@@ -75,7 +75,7 @@ def planning(self, sx, sy, gx, gy):
current_B = goal_node
meet_point_A, meet_point_B = None, None
- while 1:
+ while True:
if len(open_set_A) == 0:
print("Open set A is empty..")
break
diff --git a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py
index dd4282573e..60a8c5e170 100644
--- a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py
+++ b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py
@@ -76,7 +76,7 @@ def planning(self, sx, sy, gx, gy):
meet_point_A, meet_point_B = None, None
- while 1:
+ while True:
if len(open_set_A) == 0:
print("Open set A is empty..")
break
diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py
index 406e5eb51f..ad994732a5 100644
--- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py
+++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py
@@ -67,7 +67,7 @@ def planning(self, sx, sy, gx, gy):
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart
- while 1:
+ while True:
if len(open_set) == 0:
print("Open set is empty..")
break
@@ -220,20 +220,20 @@ def main():
# set obstacle positions
ox, oy = [], []
for i in range(-10, 60):
- ox.append(i)
+ ox.append(float(i))
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(-10, 61):
ox.append(-10.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
diff --git a/PathPlanning/BugPlanning/bug.py b/PathPlanning/BugPlanning/bug.py
index 62fec7c109..34890cb55a 100644
--- a/PathPlanning/BugPlanning/bug.py
+++ b/PathPlanning/BugPlanning/bug.py
@@ -1,7 +1,7 @@
"""
Bug Planning
author: Sarim Mehdi(muhammadsarim.mehdi@studio.unibo.it)
-Source: https://sites.google.com/site/ece452bugalgorithms/
+Source: https://web.archive.org/web/20201103052224/https://sites.google.com/site/ece452bugalgorithms/
"""
import numpy as np
diff --git a/PathPlanning/Catmull_RomSplinePath/blending_functions.py b/PathPlanning/Catmull_RomSplinePath/blending_functions.py
new file mode 100644
index 0000000000..f3ef5dd323
--- /dev/null
+++ b/PathPlanning/Catmull_RomSplinePath/blending_functions.py
@@ -0,0 +1,34 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+def blending_function_1(t):
+ return -t + 2*t**2 - t**3
+
+def blending_function_2(t):
+ return 2 - 5*t**2 + 3*t**3
+
+def blending_function_3(t):
+ return t + 4*t**2 - 3*t**3
+
+def blending_function_4(t):
+ return -t**2 + t**3
+
+def plot_blending_functions():
+ t = np.linspace(0, 1, 100)
+
+ plt.plot(t, blending_function_1(t), label='b1')
+ plt.plot(t, blending_function_2(t), label='b2')
+ plt.plot(t, blending_function_3(t), label='b3')
+ plt.plot(t, blending_function_4(t), label='b4')
+
+ plt.title("Catmull-Rom Blending Functions")
+ plt.xlabel("t")
+ plt.ylabel("Value")
+ plt.legend()
+ plt.grid(True)
+ plt.axhline(y=0, color='k', linestyle='--')
+ plt.axvline(x=0, color='k', linestyle='--')
+ plt.show()
+
+if __name__ == "__main__":
+ plot_blending_functions()
\ No newline at end of file
diff --git a/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py b/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py
new file mode 100644
index 0000000000..79916330c9
--- /dev/null
+++ b/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py
@@ -0,0 +1,86 @@
+"""
+Path Planner with Catmull-Rom Spline
+Author: Surabhi Gupta (@this_is_surabhi)
+Source: http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf
+"""
+
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+def catmull_rom_point(t, p0, p1, p2, p3):
+ """
+ Parameters
+ ----------
+ t : float
+ Parameter value (0 <= t <= 1) (0 <= t <= 1)
+ p0, p1, p2, p3 : np.ndarray
+ Control points for the spline segment
+
+ Returns
+ -------
+ np.ndarray
+ Calculated point on the spline
+ """
+ return 0.5 * ((2 * p1) +
+ (-p0 + p2) * t +
+ (2 * p0 - 5 * p1 + 4 * p2 - p3) * t**2 +
+ (-p0 + 3 * p1 - 3 * p2 + p3) * t**3)
+
+
+def catmull_rom_spline(control_points, num_points):
+ """
+ Parameters
+ ----------
+ control_points : list
+ List of control points
+ num_points : int
+ Number of points to generate on the spline
+
+ Returns
+ -------
+ tuple
+ x and y coordinates of the spline points
+ """
+ t_vals = np.linspace(0, 1, num_points)
+ spline_points = []
+
+ control_points = np.array(control_points)
+
+ for i in range(len(control_points) - 1):
+ if i == 0:
+ p1, p2, p3 = control_points[i:i+3]
+ p0 = p1
+ elif i == len(control_points) - 2:
+ p0, p1, p2 = control_points[i-1:i+2]
+ p3 = p2
+ else:
+ p0, p1, p2, p3 = control_points[i-1:i+3]
+
+ for t in t_vals:
+ point = catmull_rom_point(t, p0, p1, p2, p3)
+ spline_points.append(point)
+
+ return np.array(spline_points).T
+
+
+def main():
+ print(__file__ + " start!!")
+
+ way_points = [[-1.0, -2.0], [1.0, -1.0], [3.0, -2.0], [4.0, -1.0], [3.0, 1.0], [1.0, 2.0], [0.0, 2.0]]
+ n_course_point = 100
+ spline_x, spline_y = catmull_rom_spline(way_points, n_course_point)
+
+ plt.plot(spline_x,spline_y, '-r', label="Catmull-Rom Spline Path")
+ plt.plot(np.array(way_points).T[0], np.array(way_points).T[1], '-og', label="Way points")
+ plt.title("Catmull-Rom Spline Path")
+ plt.grid(True)
+ plt.legend()
+ plt.axis("equal")
+ plt.show()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
index dc9a575694..01ab8349a9 100644
--- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
+++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
@@ -5,27 +5,17 @@
author: AtsushiSakai(@Atsushi_twi)
"""
-import os
-import sys
-
import matplotlib.pyplot as plt
import numpy as np
-sys.path.append(os.path.dirname(os.path.abspath(__file__)))
-
-import pure_pursuit
-import unicycle_model
-
-sys.path.append(os.path.dirname(
- os.path.abspath(__file__)) + "/../ReedsSheppPath/")
-sys.path.append(os.path.dirname(
- os.path.abspath(__file__)) + "/../RRTStarReedsShepp/")
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-try:
- import reeds_shepp_path_planning
- from rrt_star_reeds_shepp import RRTStarReedsShepp
-except ImportError:
- raise
+from ClosedLoopRRTStar import pure_pursuit
+from ClosedLoopRRTStar import unicycle_model
+from ReedsSheppPath import reeds_shepp_path_planning
+from RRTStarReedsShepp.rrt_star_reeds_shepp import RRTStarReedsShepp
show_animation = True
diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
index ca763f0d4f..982ebeca06 100644
--- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
+++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
@@ -10,7 +10,11 @@
import matplotlib.pyplot as plt
import numpy as np
-import unicycle_model
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
+
+from ClosedLoopRRTStar import unicycle_model
Kp = 2.0 # speed propotional gain
Lf = 0.5 # look-ahead distance
diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
index 5a0fd17a4e..c05f76c84e 100644
--- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
+++ b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
@@ -8,6 +8,7 @@
import math
import numpy as np
+from utils.angle import angle_mod
dt = 0.05 # [s]
L = 0.9 # [m]
@@ -39,7 +40,7 @@ def update(state, a, delta):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
if __name__ == '__main__': # pragma: no cover
diff --git a/PathPlanning/CubicSpline/cubic_spline_planner.py b/PathPlanning/CubicSpline/cubic_spline_planner.py
index 7cc7bedc79..2391f67c39 100644
--- a/PathPlanning/CubicSpline/cubic_spline_planner.py
+++ b/PathPlanning/CubicSpline/cubic_spline_planner.py
@@ -76,6 +76,11 @@ def calc_position(self, x):
if `x` is outside the data point's `x` range, return None.
+ Parameters
+ ----------
+ x : float
+ x position to calculate y.
+
Returns
-------
y : float
@@ -99,6 +104,11 @@ def calc_first_derivative(self, x):
if x is outside the input x, return None
+ Parameters
+ ----------
+ x : float
+ x position to calculate first derivative.
+
Returns
-------
dy : float
@@ -121,6 +131,11 @@ def calc_second_derivative(self, x):
if x is outside the input x, return None
+ Parameters
+ ----------
+ x : float
+ x position to calculate second derivative.
+
Returns
-------
ddy : float
@@ -137,6 +152,31 @@ def calc_second_derivative(self, x):
ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
return ddy
+ def calc_third_derivative(self, x):
+ """
+ Calc third derivative at given x.
+
+ if x is outside the input x, return None
+
+ Parameters
+ ----------
+ x : float
+ x position to calculate third derivative.
+
+ Returns
+ -------
+ dddy : float
+ third derivative for given x.
+ """
+ if x < self.x[0]:
+ return None
+ elif x > self.x[-1]:
+ return None
+
+ i = self.__search_index(x)
+ dddy = 6.0 * self.d[i]
+ return dddy
+
def __search_index(self, x):
"""
search data segment index
@@ -287,6 +327,33 @@ def calc_curvature(self, s):
k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
return k
+ def calc_curvature_rate(self, s):
+ """
+ calc curvature rate
+
+ Parameters
+ ----------
+ s : float
+ distance from the start point. if `s` is outside the data point's
+ range, return None.
+
+ Returns
+ -------
+ k : float
+ curvature rate for given s.
+ """
+ dx = self.sx.calc_first_derivative(s)
+ dy = self.sy.calc_first_derivative(s)
+ ddx = self.sx.calc_second_derivative(s)
+ ddy = self.sy.calc_second_derivative(s)
+ dddx = self.sx.calc_third_derivative(s)
+ dddy = self.sy.calc_third_derivative(s)
+ a = dx * ddy - dy * ddx
+ b = dx * dddy - dy * dddx
+ c = dx * ddx + dy * ddy
+ d = dx * dx + dy * dy
+ return (b * d - 3.0 * a * c) / (d * d * d)
+
def calc_yaw(self, s):
"""
calc yaw
diff --git a/PathPlanning/DStar/dstar.py b/PathPlanning/DStar/dstar.py
index f8954d776c..b62b939f54 100644
--- a/PathPlanning/DStar/dstar.py
+++ b/PathPlanning/DStar/dstar.py
@@ -9,6 +9,7 @@
"""
import math
+
from sys import maxsize
import matplotlib.pyplot as plt
@@ -103,7 +104,7 @@ def process_state(self):
if y.h <= k_old and x.h > y.h + x.cost(y):
x.parent = y
x.h = y.h + x.cost(y)
- elif k_old == x.h:
+ if k_old == x.h:
for y in self.map.get_neighbors(x):
if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \
or y.parent != x and y.h > x.h + x.cost(y):
@@ -116,7 +117,7 @@ def process_state(self):
self.insert(y, x.h + x.cost(y))
else:
if y.parent != x and y.h > x.h + x.cost(y):
- self.insert(y, x.h)
+ self.insert(x, x.h)
else:
if y.parent != x and x.h > y.h + x.cost(y) \
and y.t == "close" and y.h > k_old:
@@ -173,6 +174,8 @@ def run(self, start, end):
s.set_state("e")
tmp = start
+ AddNewObstacle(self.map) # add new obstacle after the first search finished
+
while tmp != end:
tmp.set_state("*")
rx.append(tmp.x)
@@ -195,6 +198,15 @@ def modify(self, state):
if k_min >= state.h:
break
+def AddNewObstacle(map:Map):
+ ox, oy = [], []
+ for i in range(5, 21):
+ ox.append(i)
+ oy.append(40)
+ map.set_obstacle([(i, j) for i, j in zip(ox, oy)])
+ if show_animation:
+ plt.pause(0.001)
+ plt.plot(ox, oy, ".g")
def main():
m = Map(100, 100)
@@ -217,7 +229,6 @@ def main():
for i in range(0, 40):
ox.append(40)
oy.append(60 - i)
- print([(i, j) for i, j in zip(ox, oy)])
m.set_obstacle([(i, j) for i, j in zip(ox, oy)])
start = [10, 10]
diff --git a/PathPlanning/DStarLite/d_star_lite.py b/PathPlanning/DStarLite/d_star_lite.py
index c250e38f91..1a44d84fa5 100644
--- a/PathPlanning/DStarLite/d_star_lite.py
+++ b/PathPlanning/DStarLite/d_star_lite.py
@@ -2,7 +2,7 @@
D* Lite grid planning
author: vss2sn (28676655+vss2sn@users.noreply.github.com)
Link to papers:
-D* Lite (Link: http://idm-lab.org/bib/abstracts/papers/aaai02b.pd)
+D* Lite (Link: http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf)
Improved Fast Replanning for Robot Navigation in Unknown Terrain
(Link: http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf)
Implemented maintaining similarity with the pseudocode for understanding.
@@ -12,6 +12,7 @@
import math
import matplotlib.pyplot as plt
import random
+import numpy as np
show_animation = True
pause_time = 0.001
@@ -62,32 +63,28 @@ def __init__(self, ox: list, oy: list):
self.y_max = int(abs(max(oy) - self.y_min_world))
self.obstacles = [Node(x - self.x_min_world, y - self.y_min_world)
for x, y in zip(ox, oy)]
+ self.obstacles_xy = {(obstacle.x, obstacle.y) for obstacle in self.obstacles}
self.start = Node(0, 0)
self.goal = Node(0, 0)
self.U = list() # type: ignore
self.km = 0.0
self.kold = 0.0
- self.rhs = list() # type: ignore
- self.g = list() # type: ignore
- self.detected_obstacles = list() # type: ignore
+ self.rhs = self.create_grid(float("inf"))
+ self.g = self.create_grid(float("inf"))
+ self.detected_obstacles_xy: set[tuple[int, int]] = set()
+ self.xy = np.empty((0, 2))
if show_animation:
self.detected_obstacles_for_plotting_x = list() # type: ignore
self.detected_obstacles_for_plotting_y = list() # type: ignore
+ self.initialized = False
def create_grid(self, val: float):
- grid = list()
- for _ in range(0, self.x_max):
- grid_row = list()
- for _ in range(0, self.y_max):
- grid_row.append(val)
- grid.append(grid_row)
- return grid
+ return np.full((self.x_max, self.y_max), val)
def is_obstacle(self, node: Node):
- return any([compare_coordinates(node, obstacle)
- for obstacle in self.obstacles]) or \
- any([compare_coordinates(node, obstacle)
- for obstacle in self.detected_obstacles])
+ is_in_obstacles = (node.x, node.y) in self.obstacles_xy
+ is_in_detected_obstacles = (node.x, node.y) in self.detected_obstacles_xy
+ return is_in_obstacles or is_in_detected_obstacles
def c(self, node1: Node, node2: Node):
if self.is_obstacle(node2):
@@ -139,13 +136,16 @@ def initialize(self, start: Node, goal: Node):
self.start.y = start.y - self.y_min_world
self.goal.x = goal.x - self.x_min_world
self.goal.y = goal.y - self.y_min_world
- self.U = list() # Would normally be a priority queue
- self.km = 0.0
- self.rhs = self.create_grid(math.inf)
- self.g = self.create_grid(math.inf)
- self.rhs[self.goal.x][self.goal.y] = 0
- self.U.append((self.goal, self.calculate_key(self.goal)))
- self.detected_obstacles = list()
+ if not self.initialized:
+ self.initialized = True
+ print('Initializing')
+ self.U = list() # Would normally be a priority queue
+ self.km = 0.0
+ self.rhs = self.create_grid(math.inf)
+ self.g = self.create_grid(math.inf)
+ self.rhs[self.goal.x][self.goal.y] = 0
+ self.U.append((self.goal, self.calculate_key(self.goal)))
+ self.detected_obstacles_xy = set()
def update_vertex(self, u: Node):
if not compare_coordinates(u, self.goal):
@@ -167,26 +167,33 @@ def compare_keys(self, key_pair1: tuple[float, float],
def compute_shortest_path(self):
self.U.sort(key=lambda x: x[1])
- while (len(self.U) > 0 and
- self.compare_keys(self.U[0][1],
- self.calculate_key(self.start))) or \
- self.rhs[self.start.x][self.start.y] != \
- self.g[self.start.x][self.start.y]:
+ has_elements = len(self.U) > 0
+ start_key_not_updated = self.compare_keys(
+ self.U[0][1], self.calculate_key(self.start)
+ )
+ rhs_not_equal_to_g = self.rhs[self.start.x][self.start.y] != \
+ self.g[self.start.x][self.start.y]
+ while has_elements and start_key_not_updated or rhs_not_equal_to_g:
self.kold = self.U[0][1]
u = self.U[0][0]
self.U.pop(0)
if self.compare_keys(self.kold, self.calculate_key(u)):
self.U.append((u, self.calculate_key(u)))
self.U.sort(key=lambda x: x[1])
- elif self.g[u.x][u.y] > self.rhs[u.x][u.y]:
- self.g[u.x][u.y] = self.rhs[u.x][u.y]
+ elif (self.g[u.x, u.y] > self.rhs[u.x, u.y]).any():
+ self.g[u.x, u.y] = self.rhs[u.x, u.y]
for s in self.pred(u):
self.update_vertex(s)
else:
- self.g[u.x][u.y] = math.inf
+ self.g[u.x, u.y] = math.inf
for s in self.pred(u) + [u]:
self.update_vertex(s)
self.U.sort(key=lambda x: x[1])
+ start_key_not_updated = self.compare_keys(
+ self.U[0][1], self.calculate_key(self.start)
+ )
+ rhs_not_equal_to_g = self.rhs[self.start.x][self.start.y] != \
+ self.g[self.start.x][self.start.y]
def detect_changes(self):
changed_vertices = list()
@@ -196,7 +203,7 @@ def detect_changes(self):
compare_coordinates(spoofed_obstacle, self.goal):
continue
changed_vertices.append(spoofed_obstacle)
- self.detected_obstacles.append(spoofed_obstacle)
+ self.detected_obstacles_xy.add((spoofed_obstacle.x, spoofed_obstacle.y))
if show_animation:
self.detected_obstacles_for_plotting_x.append(
spoofed_obstacle.x + self.x_min_world)
@@ -210,14 +217,14 @@ def detect_changes(self):
# Allows random generation of obstacles
random.seed()
if random.random() > 1 - p_create_random_obstacle:
- x = random.randint(0, self.x_max)
- y = random.randint(0, self.y_max)
+ x = random.randint(0, self.x_max - 1)
+ y = random.randint(0, self.y_max - 1)
new_obs = Node(x, y)
if compare_coordinates(new_obs, self.start) or \
compare_coordinates(new_obs, self.goal):
return changed_vertices
changed_vertices.append(Node(x, y))
- self.detected_obstacles.append(Node(x, y))
+ self.detected_obstacles_xy.add((x, y))
if show_animation:
self.detected_obstacles_for_plotting_x.append(x +
self.x_min_world)
diff --git a/PathPlanning/DepthFirstSearch/depth_first_search.py b/PathPlanning/DepthFirstSearch/depth_first_search.py
index 523225ee42..6922b8cbad 100644
--- a/PathPlanning/DepthFirstSearch/depth_first_search.py
+++ b/PathPlanning/DepthFirstSearch/depth_first_search.py
@@ -67,7 +67,7 @@ def planning(self, sx, sy, gx, gy):
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart
- while 1:
+ while True:
if len(open_set) == 0:
print("Open set is empty..")
break
diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py
index 9956dff326..f5a4703910 100644
--- a/PathPlanning/Dijkstra/dijkstra.py
+++ b/PathPlanning/Dijkstra/dijkstra.py
@@ -12,7 +12,7 @@
show_animation = True
-class Dijkstra:
+class DijkstraPlanner:
def __init__(self, ox, oy, resolution, robot_radius):
"""
@@ -71,7 +71,7 @@ def planning(self, sx, sy, gx, gy):
open_set, closed_set = dict(), dict()
open_set[self.calc_index(start_node)] = start_node
- while 1:
+ while True:
c_id = min(open_set, key=lambda o: open_set[o].cost)
current = open_set[c_id]
@@ -221,20 +221,20 @@ def main():
# set obstacle positions
ox, oy = [], []
for i in range(-10, 60):
- ox.append(i)
+ ox.append(float(i))
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(-10, 61):
ox.append(-10.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
@@ -246,7 +246,7 @@ def main():
plt.grid(True)
plt.axis("equal")
- dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
+ dijkstra = DijkstraPlanner(ox, oy, grid_size, robot_radius)
rx, ry = dijkstra.planning(sx, sy, gx, gy)
if show_animation: # pragma: no cover
diff --git a/PathPlanning/DubinsPath/dubins_path_planner.py b/PathPlanning/DubinsPath/dubins_path_planner.py
index 4d4ee07291..a7e8a100cc 100644
--- a/PathPlanning/DubinsPath/dubins_path_planner.py
+++ b/PathPlanning/DubinsPath/dubins_path_planner.py
@@ -6,9 +6,8 @@
"""
import sys
-import os
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+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
diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py
index 468d3b9f97..9ccd18b7c2 100644
--- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py
+++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py
@@ -9,7 +9,7 @@
More information on Dynamic Movement Primitives available at:
https://arxiv.org/abs/2102.03861
-https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full
+https://www.frontiersin.org/journals/computational-neuroscience/articles/10.3389/fncom.2013.00138/full
"""
@@ -18,7 +18,7 @@
import numpy as np
-class DMP(object):
+class DMP:
def __init__(self, training_data, data_period, K=156.25, B=25):
"""
@@ -215,21 +215,21 @@ def show_DMP_purpose(self):
T_vals = np.linspace(T_orig, 2*T_orig, 20)
for new_q0_value in q0_vals:
- plot_title = "Initial Position = [%s, %s]" % \
- (round(new_q0_value[0], 2), round(new_q0_value[1], 2))
+ plot_title = (f"Initial Position = [{round(new_q0_value[0], 2)},"
+ f" {round(new_q0_value[1], 2)}]")
_, path = self.recreate_trajectory(new_q0_value, g_orig, T_orig)
self.view_trajectory(path, title=plot_title, demo=True)
for new_g_value in g_vals:
- plot_title = "Goal Position = [%s, %s]" % \
- (round(new_g_value[0], 2), round(new_g_value[1], 2))
+ plot_title = (f"Goal Position = [{round(new_g_value[0], 2)},"
+ f" {round(new_g_value[1], 2)}]")
_, path = self.recreate_trajectory(q0_orig, new_g_value, T_orig)
self.view_trajectory(path, title=plot_title, demo=True)
for new_T_value in T_vals:
- plot_title = "Period = %s [sec]" % round(new_T_value, 2)
+ plot_title = f"Period = {round(new_T_value, 2)} [sec]"
_, path = self.recreate_trajectory(q0_orig, g_orig, new_T_value)
self.view_trajectory(path, title=plot_title, demo=True)
@@ -257,5 +257,4 @@ def example_DMP():
if __name__ == '__main__':
-
example_DMP()
diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index 74ff064e86..8664ec1745 100644
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
@@ -300,8 +300,7 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
if show_animation:
plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")
plt.pause(0.0001)
-
- plt.show()
+ plt.show()
if __name__ == '__main__':
diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py
new file mode 100644
index 0000000000..77d4e6e399
--- /dev/null
+++ b/PathPlanning/ElasticBands/elastic_bands.py
@@ -0,0 +1,300 @@
+"""
+Elastic Bands
+
+author: Wang Zheng (@Aglargil)
+
+Reference:
+
+- [Elastic Bands: Connecting Path Planning and Control]
+(http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf)
+"""
+
+import numpy as np
+import sys
+import pathlib
+import matplotlib.pyplot as plt
+from matplotlib.patches import Circle
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
+from Mapping.DistanceMap.distance_map import compute_sdf_scipy
+
+# Elastic Bands Params
+MAX_BUBBLE_RADIUS = 100
+MIN_BUBBLE_RADIUS = 10
+RHO0 = 20.0 # Maximum distance for applying repulsive force
+KC = 0.05 # Contraction force gain
+KR = -0.1 # Repulsive force gain
+LAMBDA = 0.7 # Overlap constraint factor
+STEP_SIZE = 3.0 # Step size for calculating gradient
+
+# Visualization Params
+ENABLE_PLOT = True
+# ENABLE_INTERACTIVE is True allows user to add obstacles by left clicking
+# and add path points by right clicking and start planning by middle clicking
+ENABLE_INTERACTIVE = False
+# ENABLE_SAVE_DATA is True allows saving the path and obstacles which added
+# by user in interactive mode to file
+ENABLE_SAVE_DATA = False
+MAX_ITER = 50
+
+
+class Bubble:
+ def __init__(self, position, radius):
+ self.pos = np.array(position) # Bubble center coordinates [x, y]
+ self.radius = radius # Safety distance radius ρ(b)
+ if self.radius > MAX_BUBBLE_RADIUS:
+ self.radius = MAX_BUBBLE_RADIUS
+ if self.radius < MIN_BUBBLE_RADIUS:
+ self.radius = MIN_BUBBLE_RADIUS
+
+
+class ElasticBands:
+ def __init__(
+ self,
+ initial_path,
+ obstacles,
+ rho0=RHO0,
+ kc=KC,
+ kr=KR,
+ lambda_=LAMBDA,
+ step_size=STEP_SIZE,
+ ):
+ self.distance_map = compute_sdf_scipy(obstacles)
+ self.bubbles = [
+ Bubble(p, self.compute_rho(p)) for p in initial_path
+ ] # Initialize bubble chain
+ self.kc = kc # Contraction force gain
+ self.kr = kr # Repulsive force gain
+ self.rho0 = rho0 # Maximum distance for applying repulsive force
+ self.lambda_ = lambda_ # Overlap constraint factor
+ self.step_size = step_size # Step size for calculating gradient
+ self._maintain_overlap()
+
+ def compute_rho(self, position):
+ """Compute the distance field value at the position"""
+ return self.distance_map[int(position[0]), int(position[1])]
+
+ def contraction_force(self, i):
+ """Calculate internal contraction force for the i-th bubble"""
+ if i == 0 or i == len(self.bubbles) - 1:
+ return np.zeros(2)
+
+ prev = self.bubbles[i - 1].pos
+ next_ = self.bubbles[i + 1].pos
+ current = self.bubbles[i].pos
+
+ # f_c = kc * ( (prev-current)/|prev-current| + (next-current)/|next-current| )
+ dir_prev = (prev - current) / (np.linalg.norm(prev - current) + 1e-6)
+ dir_next = (next_ - current) / (np.linalg.norm(next_ - current) + 1e-6)
+ return self.kc * (dir_prev + dir_next)
+
+ def repulsive_force(self, i):
+ """Calculate external repulsive force for the i-th bubble"""
+ h = self.step_size # Step size
+ b = self.bubbles[i].pos
+ rho = self.bubbles[i].radius
+
+ if rho >= self.rho0:
+ return np.zeros(2)
+
+ # Finite difference approximation of the gradient ∂ρ/∂b
+ dx = np.array([h, 0])
+ dy = np.array([0, h])
+ grad_x = (self.compute_rho(b - dx) - self.compute_rho(b + dx)) / (2 * h)
+ grad_y = (self.compute_rho(b - dy) - self.compute_rho(b + dy)) / (2 * h)
+ grad = np.array([grad_x, grad_y])
+
+ return self.kr * (self.rho0 - rho) * grad
+
+ def update_bubbles(self):
+ """Update bubble positions"""
+ new_bubbles = []
+ for i in range(len(self.bubbles)):
+ if i == 0 or i == len(self.bubbles) - 1:
+ new_bubbles.append(self.bubbles[i]) # Fixed start and end points
+ continue
+
+ f_total = self.contraction_force(i) + self.repulsive_force(i)
+ v = self.bubbles[i - 1].pos - self.bubbles[i + 1].pos
+
+ # Remove tangential component
+ f_star = f_total - f_total * v * v / (np.linalg.norm(v) ** 2 + 1e-6)
+
+ alpha = self.bubbles[i].radius # Adaptive step size
+ new_pos = self.bubbles[i].pos + alpha * f_star
+ new_pos = np.clip(new_pos, 0, 499)
+ new_radius = self.compute_rho(new_pos)
+
+ # Update bubble and maintain overlap constraint
+ new_bubble = Bubble(new_pos, new_radius)
+ new_bubbles.append(new_bubble)
+
+ self.bubbles = new_bubbles
+ self._maintain_overlap()
+
+ def _maintain_overlap(self):
+ """Maintain bubble chain continuity (simplified insertion/deletion mechanism)"""
+ # Insert bubbles
+ i = 0
+ while i < len(self.bubbles) - 1:
+ bi, bj = self.bubbles[i], self.bubbles[i + 1]
+ dist = np.linalg.norm(bi.pos - bj.pos)
+ if dist > self.lambda_ * (bi.radius + bj.radius):
+ new_pos = (bi.pos + bj.pos) / 2
+ rho = self.compute_rho(
+ new_pos
+ ) # Calculate new radius using environment model
+ self.bubbles.insert(i + 1, Bubble(new_pos, rho))
+ i += 2 # Skip the processed region
+ else:
+ i += 1
+
+ # Delete redundant bubbles
+ i = 1
+ while i < len(self.bubbles) - 1:
+ prev = self.bubbles[i - 1]
+ next_ = self.bubbles[i + 1]
+ dist = np.linalg.norm(prev.pos - next_.pos)
+ if dist <= self.lambda_ * (prev.radius + next_.radius):
+ del self.bubbles[i] # Delete if redundant
+ else:
+ i += 1
+
+
+class ElasticBandsVisualizer:
+ def __init__(self):
+ self.obstacles = np.zeros((500, 500))
+ self.obstacles_points = []
+ self.path_points = []
+ self.elastic_band = None
+ self.running = True
+
+ if ENABLE_PLOT:
+ self.fig, self.ax = plt.subplots(figsize=(8, 8))
+ self.fig.canvas.mpl_connect("close_event", self.on_close)
+ self.ax.set_xlim(0, 500)
+ self.ax.set_ylim(0, 500)
+
+ if ENABLE_INTERACTIVE:
+ self.path_points = [] # Add a list to store path points
+ # Connect mouse events
+ self.fig.canvas.mpl_connect("button_press_event", self.on_click)
+ else:
+ self.path_points = np.load(pathlib.Path(__file__).parent / "path.npy")
+ self.obstacles_points = np.load(
+ pathlib.Path(__file__).parent / "obstacles.npy"
+ )
+ for x, y in self.obstacles_points:
+ self.add_obstacle(x, y)
+ self.plan_path()
+
+ self.plot_background()
+
+ def on_close(self, event):
+ """Handle window close event"""
+ self.running = False
+ plt.close("all") # Close all figure windows
+
+ def plot_background(self):
+ """Plot the background grid"""
+ if not ENABLE_PLOT or not self.running:
+ return
+
+ self.ax.cla()
+ self.ax.set_xlim(0, 500)
+ self.ax.set_ylim(0, 500)
+ self.ax.grid(True)
+
+ if ENABLE_INTERACTIVE:
+ self.ax.set_title(
+ "Elastic Bands Path Planning\n"
+ "Left click: Add obstacles\n"
+ "Right click: Add path points\n"
+ "Middle click: Start planning",
+ pad=20,
+ )
+ else:
+ self.ax.set_title("Elastic Bands Path Planning", pad=20)
+
+ if self.path_points:
+ self.ax.plot(
+ [p[0] for p in self.path_points],
+ [p[1] for p in self.path_points],
+ "yo",
+ markersize=8,
+ )
+
+ self.ax.imshow(self.obstacles.T, origin="lower", cmap="binary", alpha=0.8)
+ self.ax.plot([], [], color="black", label="obstacles")
+ if self.elastic_band is not None:
+ path = [b.pos.tolist() for b in self.elastic_band.bubbles]
+ path = np.array(path)
+ self.ax.plot(path[:, 0], path[:, 1], "b-", linewidth=2, label="path")
+
+ for bubble in self.elastic_band.bubbles:
+ circle = Circle(
+ bubble.pos, bubble.radius, fill=False, color="g", alpha=0.3
+ )
+ self.ax.add_patch(circle)
+ self.ax.plot(bubble.pos[0], bubble.pos[1], "bo", markersize=10)
+ self.ax.plot([], [], color="green", label="bubbles")
+
+ self.ax.legend(loc="upper right")
+ plt.draw()
+ plt.pause(0.01)
+
+ def add_obstacle(self, x, y):
+ """Add an obstacle at the given coordinates"""
+ size = 30 # Side length of the square
+ half_size = size // 2
+ x_start = max(0, x - half_size)
+ x_end = min(self.obstacles.shape[0], x + half_size)
+ y_start = max(0, y - half_size)
+ y_end = min(self.obstacles.shape[1], y + half_size)
+ self.obstacles[x_start:x_end, y_start:y_end] = 1
+
+ def on_click(self, event):
+ """Handle mouse click events"""
+ if event.inaxes != self.ax:
+ return
+
+ x, y = int(event.xdata), int(event.ydata)
+
+ if event.button == 1: # Left click to add obstacles
+ self.add_obstacle(x, y)
+ self.obstacles_points.append([x, y])
+
+ elif event.button == 3: # Right click to add path points
+ self.path_points.append([x, y])
+
+ elif event.button == 2: # Middle click to end path input and start planning
+ if len(self.path_points) >= 2:
+ if ENABLE_SAVE_DATA:
+ np.save(
+ pathlib.Path(__file__).parent / "path.npy", self.path_points
+ )
+ np.save(
+ pathlib.Path(__file__).parent / "obstacles.npy",
+ self.obstacles_points,
+ )
+ self.plan_path()
+
+ self.plot_background()
+
+ def plan_path(self):
+ """Plan the path"""
+
+ initial_path = self.path_points
+ # Create an elastic band object and optimize
+ self.elastic_band = ElasticBands(initial_path, self.obstacles)
+ for _ in range(MAX_ITER):
+ self.elastic_band.update_bubbles()
+ self.path_points = [b.pos for b in self.elastic_band.bubbles]
+ self.plot_background()
+
+
+if __name__ == "__main__":
+ _ = ElasticBandsVisualizer()
+ if ENABLE_PLOT:
+ plt.show(block=True)
diff --git a/PathPlanning/ElasticBands/obstacles.npy b/PathPlanning/ElasticBands/obstacles.npy
new file mode 100644
index 0000000000..af4376afcf
Binary files /dev/null and b/PathPlanning/ElasticBands/obstacles.npy differ
diff --git a/PathPlanning/ElasticBands/path.npy b/PathPlanning/ElasticBands/path.npy
new file mode 100644
index 0000000000..be7c253d65
Binary files /dev/null and b/PathPlanning/ElasticBands/path.npy differ
diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
index dc07d3c84b..3f685e512f 100644
--- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py
+++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
@@ -5,7 +5,7 @@
author: Joe Dinius, Ph.D (https://jwdinius.github.io)
Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
(https://ieeexplore.ieee.org/document/4339545/)
diff --git a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
index bd211046c7..e72d33261e 100644
--- a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
+++ b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
@@ -6,7 +6,7 @@
Atsushi Sakai (@Atsushi_twi)
Refs:
-- https://jwdinius.github.io/blog/2018/eta3traj
+- https://jwdinius.github.io/blog/2018/eta3traj/
- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
(https://ieeexplore.ieee.org/document/4339545/)
@@ -16,25 +16,20 @@
import matplotlib.pyplot as plt
from matplotlib.collections import LineCollection
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../Eta3SplinePath")
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-try:
- from eta3_spline_path import Eta3Path, Eta3PathSegment
-except ImportError:
- raise
+from Eta3SplinePath.eta3_spline_path import Eta3Path, Eta3PathSegment
show_animation = True
class MaxVelocityNotReached(Exception):
def __init__(self, actual_vel, max_vel):
- self.message = 'Actual velocity {} does not equal desired max velocity {}!'.format(
- actual_vel, max_vel)
+ self.message = f'Actual velocity {actual_vel} does not equal desired max velocity {max_vel}!'
-class eta3_trajectory(Eta3Path):
+class Eta3SplineTrajectory(Eta3Path):
"""
eta3_trajectory
@@ -46,7 +41,7 @@ def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.
# ensure that all inputs obey the assumptions of the model
assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \
and a0 <= max_accel and v0 <= max_vel
- super(eta3_trajectory, self).__init__(segments=segments)
+ super(__class__, self).__init__(segments=segments)
self.total_length = sum([s.segment_length for s in self.segments])
self.max_vel = float(max_vel)
self.v0 = float(v0)
@@ -170,7 +165,7 @@ def velocity_profile(self):
try:
assert np.isclose(self.vels[index], 0)
except AssertionError as e:
- print('The final velocity {} is not zero'.format(self.vels[index]))
+ print(f'The final velocity {self.vels[index]} is not zero')
raise e
self.seg_lengths[index] = s_sf
@@ -305,8 +300,8 @@ def test1(max_vel=0.5):
trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
- traj = eta3_trajectory(trajectory_segments,
- max_vel=max_vel, max_accel=0.5)
+ traj = Eta3SplineTrajectory(trajectory_segments,
+ max_vel=max_vel, max_accel=0.5)
# interpolate at several points along the path
times = np.linspace(0, traj.total_time, 101)
@@ -339,8 +334,8 @@ def test2(max_vel=0.5):
trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
- traj = eta3_trajectory(trajectory_segments,
- max_vel=max_vel, max_accel=0.5)
+ traj = Eta3SplineTrajectory(trajectory_segments,
+ max_vel=max_vel, max_accel=0.5)
# interpolate at several points along the path
times = np.linspace(0, traj.total_time, 101)
@@ -405,8 +400,8 @@ def test3(max_vel=2.0):
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# construct the whole path
- traj = eta3_trajectory(trajectory_segments,
- max_vel=max_vel, max_accel=0.5, max_jerk=1)
+ traj = Eta3SplineTrajectory(trajectory_segments,
+ max_vel=max_vel, max_accel=0.5, max_jerk=1)
# interpolate at several points along the path
times = np.linspace(0, traj.total_time, 1001)
diff --git a/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py
new file mode 100644
index 0000000000..482712ceaf
--- /dev/null
+++ b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py
@@ -0,0 +1,144 @@
+"""
+
+A converter between Cartesian and Frenet coordinate systems
+
+author: Wang Zheng (@Aglargil)
+
+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)
+
+"""
+
+import math
+
+
+class CartesianFrenetConverter:
+ """
+ A class for converting states between Cartesian and Frenet coordinate systems
+ """
+
+ @ staticmethod
+ def cartesian_to_frenet(rs, rx, ry, rtheta, rkappa, rdkappa, x, y, v, a, theta, kappa):
+ """
+ Convert state from Cartesian coordinate to Frenet coordinate
+
+ Parameters
+ ----------
+ rs: reference line s-coordinate
+ rx, ry: reference point coordinates
+ rtheta: reference point heading
+ rkappa: reference point curvature
+ rdkappa: reference point curvature rate
+ x, y: current position
+ v: velocity
+ a: acceleration
+ theta: heading angle
+ kappa: curvature
+
+ Returns
+ -------
+ s_condition: [s(t), s'(t), s''(t)]
+ d_condition: [d(s), d'(s), d''(s)]
+ """
+ dx = x - rx
+ dy = y - ry
+
+ cos_theta_r = math.cos(rtheta)
+ sin_theta_r = math.sin(rtheta)
+
+ cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx
+ d = math.copysign(math.hypot(dx, dy), cross_rd_nd)
+
+ delta_theta = theta - rtheta
+ tan_delta_theta = math.tan(delta_theta)
+ cos_delta_theta = math.cos(delta_theta)
+
+ one_minus_kappa_r_d = 1 - rkappa * d
+ d_dot = one_minus_kappa_r_d * tan_delta_theta
+
+ kappa_r_d_prime = rdkappa * d + rkappa * d_dot
+
+ d_ddot = (-kappa_r_d_prime * tan_delta_theta +
+ one_minus_kappa_r_d / (cos_delta_theta * cos_delta_theta) *
+ (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa))
+
+ s = rs
+ s_dot = v * cos_delta_theta / one_minus_kappa_r_d
+
+ delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa
+ s_ddot = (a * cos_delta_theta -
+ s_dot * s_dot *
+ (d_dot * delta_theta_prime - kappa_r_d_prime)) / one_minus_kappa_r_d
+
+ return [s, s_dot, s_ddot], [d, d_dot, d_ddot]
+
+ @ staticmethod
+ def frenet_to_cartesian(rs, rx, ry, rtheta, rkappa, rdkappa, s_condition, d_condition):
+ """
+ Convert state from Frenet coordinate to Cartesian coordinate
+
+ Parameters
+ ----------
+ rs: reference line s-coordinate
+ rx, ry: reference point coordinates
+ rtheta: reference point heading
+ rkappa: reference point curvature
+ rdkappa: reference point curvature rate
+ s_condition: [s(t), s'(t), s''(t)]
+ d_condition: [d(s), d'(s), d''(s)]
+
+ Returns
+ -------
+ x, y: position
+ theta: heading angle
+ kappa: curvature
+ v: velocity
+ a: acceleration
+ """
+ if abs(rs - s_condition[0]) >= 1.0e-6:
+ raise ValueError(
+ "The reference point s and s_condition[0] don't match")
+
+ cos_theta_r = math.cos(rtheta)
+ sin_theta_r = math.sin(rtheta)
+
+ x = rx - sin_theta_r * d_condition[0]
+ y = ry + cos_theta_r * d_condition[0]
+
+ one_minus_kappa_r_d = 1 - rkappa * d_condition[0]
+
+ tan_delta_theta = d_condition[1] / one_minus_kappa_r_d
+ delta_theta = math.atan2(d_condition[1], one_minus_kappa_r_d)
+ cos_delta_theta = math.cos(delta_theta)
+
+ theta = CartesianFrenetConverter.normalize_angle(delta_theta + rtheta)
+
+ kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1]
+
+ kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *
+ cos_delta_theta * cos_delta_theta) / one_minus_kappa_r_d + rkappa) * \
+ cos_delta_theta / one_minus_kappa_r_d
+
+ d_dot = d_condition[1] * s_condition[1]
+ v = math.sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *
+ s_condition[1] * s_condition[1] + d_dot * d_dot)
+
+ delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa
+
+ a = (s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +
+ s_condition[1] * s_condition[1] / cos_delta_theta *
+ (d_condition[1] * delta_theta_prime - kappa_r_d_prime))
+
+ return x, y, theta, kappa, v, a
+
+ @ staticmethod
+ def normalize_angle(angle):
+ """
+ Normalize angle to [-pi, pi]
+ """
+ a = math.fmod(angle + math.pi, 2.0 * math.pi)
+ if a < 0.0:
+ a += 2.0 * math.pi
+ return a - math.pi
diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
index ef20af22f0..4b82fb70fd 100644
--- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
+++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
@@ -4,7 +4,7 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+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)
@@ -17,49 +17,314 @@
import numpy as np
import matplotlib.pyplot as plt
import copy
-import math
import sys
-import os
+import pathlib
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../QuinticPolynomialsPlanner/")
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../CubicSpline/")
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-try:
- from quintic_polynomials_planner import QuinticPolynomial
- import cubic_spline_planner
-except ImportError:
- raise
+from QuinticPolynomialsPlanner.quintic_polynomials_planner import QuinticPolynomial
+from CubicSpline import cubic_spline_planner
-SIM_LOOP = 500
+from enum import Enum, auto
+from FrenetOptimalTrajectory.cartesian_frenet_converter import (
+ CartesianFrenetConverter,
+)
+
+
+class LateralMovement(Enum):
+ HIGH_SPEED = auto()
+ LOW_SPEED = auto()
+
+
+class LongitudinalMovement(Enum):
+ MERGING_AND_STOPPING = auto()
+ VELOCITY_KEEPING = auto()
+
+
+# Default Parameters
+
+LATERAL_MOVEMENT = LateralMovement.HIGH_SPEED
+LONGITUDINAL_MOVEMENT = LongitudinalMovement.VELOCITY_KEEPING
-# Parameter
MAX_SPEED = 50.0 / 3.6 # maximum speed [m/s]
-MAX_ACCEL = 2.0 # maximum acceleration [m/ss]
+MAX_ACCEL = 5.0 # maximum acceleration [m/ss]
MAX_CURVATURE = 1.0 # maximum curvature [1/m]
-MAX_ROAD_WIDTH = 7.0 # maximum road width [m]
-D_ROAD_W = 1.0 # road width sampling length [m]
DT = 0.2 # time tick [s]
MAX_T = 5.0 # max prediction time [m]
MIN_T = 4.0 # min prediction time [m]
-TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]
-D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]
N_S_SAMPLE = 1 # sampling number of target speed
-ROBOT_RADIUS = 2.0 # robot radius [m]
# cost weights
K_J = 0.1
K_T = 0.1
+K_S_DOT = 1.0
K_D = 1.0
+K_S = 1.0
K_LAT = 1.0
K_LON = 1.0
+SIM_LOOP = 500
show_animation = True
-class QuarticPolynomial:
+if LATERAL_MOVEMENT == LateralMovement.LOW_SPEED:
+ MAX_ROAD_WIDTH = 1.0 # maximum road width [m]
+ D_ROAD_W = 0.2 # road width sampling length [m]
+ TARGET_SPEED = 3.0 / 3.6 # maximum speed [m/s]
+ D_T_S = 0.5 / 3.6 # target speed sampling length [m/s]
+ # Waypoints
+ WX = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0]
+ WY = [0.0, 0.0, 1.0, 0.0, -1.0, -2.0]
+ OBSTACLES = np.array([[3.0, 1.0], [5.0, -0.0], [6.0, 0.5], [8.0, -1.5]])
+ ROBOT_RADIUS = 0.5 # robot radius [m]
+
+ # Initial state parameters
+ INITIAL_SPEED = 1.0 / 3.6 # current speed [m/s]
+ INITIAL_ACCEL = 0.0 # current acceleration [m/ss]
+ INITIAL_LAT_POSITION = 0.5 # current lateral position [m]
+ INITIAL_LAT_SPEED = 0.0 # current lateral speed [m/s]
+ INITIAL_LAT_ACCELERATION = 0.0 # current lateral acceleration [m/s]
+ INITIAL_COURSE_POSITION = 0.0 # current course position
+
+ ANIMATION_AREA = 5.0 # Animation area length [m]
+
+ STOP_S = 4.0 # Merge and stop distance [m]
+ D_S = 0.3 # Stop point sampling length [m]
+ N_STOP_S_SAMPLE = 3 # Stop point sampling number
+else:
+ MAX_ROAD_WIDTH = 7.0 # maximum road width [m]
+ D_ROAD_W = 1.0 # road width sampling length [m]
+ TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]
+ D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]
+ # Waypoints
+ WX = [0.0, 10.0, 20.5, 35.0, 70.5]
+ WY = [0.0, -6.0, 5.0, 6.5, 0.0]
+ # Obstacle list
+ OBSTACLES = np.array(
+ [[20.0, 10.0], [30.0, 6.0], [30.0, 8.0], [35.0, 8.0], [50.0, 3.0]]
+ )
+ ROBOT_RADIUS = 2.0 # robot radius [m]
+
+ # Initial state parameters
+ INITIAL_SPEED = 10.0 / 3.6 # current speed [m/s]
+ INITIAL_ACCEL = 0.0 # current acceleration [m/ss]
+ INITIAL_LAT_POSITION = 2.0 # current lateral position [m]
+ INITIAL_LAT_SPEED = 0.0 # current lateral speed [m/s]
+ INITIAL_LAT_ACCELERATION = 0.0 # current lateral acceleration [m/s]
+ INITIAL_COURSE_POSITION = 0.0 # current course position
+
+ ANIMATION_AREA = 20.0 # Animation area length [m]
+ STOP_S = 25.0 # Merge and stop distance [m]
+ D_S = 2 # Stop point sampling length [m]
+ N_STOP_S_SAMPLE = 4 # Stop point sampling number
+
+
+class LateralMovementStrategy:
+ def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti):
+ """
+ Calculate the lateral trajectory
+ """
+ raise NotImplementedError("calc_lateral_trajectory not implemented")
+
+ def calc_cartesian_parameters(self, fp, csp):
+ """
+ Calculate the cartesian parameters (x, y, yaw, curvature, v, a)
+ """
+ raise NotImplementedError("calc_cartesian_parameters not implemented")
+
+
+class HighSpeedLateralMovementStrategy(LateralMovementStrategy):
+ def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti):
+ tp = copy.deepcopy(fp)
+ s0_d = fp.s_d[0]
+ s0_dd = fp.s_dd[0]
+ # d'(t) = d'(s) * s'(t)
+ # d''(t) = d''(s) * s'(t)^2 + d'(s) * s''(t)
+ lat_qp = QuinticPolynomial(
+ c_d, c_d_d * s0_d, c_d_dd * s0_d**2 + c_d_d * s0_dd, di, 0.0, 0.0, Ti
+ )
+
+ tp.d = []
+ tp.d_d = []
+ tp.d_dd = []
+ tp.d_ddd = []
+
+ # Calculate all derivatives in a single loop to reduce iterations
+ for i in range(len(fp.t)):
+ t = fp.t[i]
+ s_d = fp.s_d[i]
+ s_dd = fp.s_dd[i]
+
+ s_d_inv = 1.0 / (s_d + 1e-6) + 1e-6 # Avoid division by zero
+ s_d_inv_sq = s_d_inv * s_d_inv # Square of inverse
+
+ d = lat_qp.calc_point(t)
+ d_d = lat_qp.calc_first_derivative(t)
+ d_dd = lat_qp.calc_second_derivative(t)
+ d_ddd = lat_qp.calc_third_derivative(t)
+
+ tp.d.append(d)
+ # d'(s) = d'(t) / s'(t)
+ tp.d_d.append(d_d * s_d_inv)
+ # d''(s) = (d''(t) - d'(s) * s''(t)) / s'(t)^2
+ tp.d_dd.append((d_dd - tp.d_d[i] * s_dd) * s_d_inv_sq)
+ tp.d_ddd.append(d_ddd)
+
+ return tp
+
+ def calc_cartesian_parameters(self, fp, csp):
+ # calc global positions
+ for i in range(len(fp.s)):
+ ix, iy = csp.calc_position(fp.s[i])
+ if ix is None:
+ break
+ i_yaw = csp.calc_yaw(fp.s[i])
+ i_kappa = csp.calc_curvature(fp.s[i])
+ i_dkappa = csp.calc_curvature_rate(fp.s[i])
+ s_condition = [fp.s[i], fp.s_d[i], fp.s_dd[i]]
+ d_condition = [
+ fp.d[i],
+ fp.d_d[i],
+ fp.d_dd[i],
+ ]
+ x, y, theta, kappa, v, a = CartesianFrenetConverter.frenet_to_cartesian(
+ fp.s[i], ix, iy, i_yaw, i_kappa, i_dkappa, s_condition, d_condition
+ )
+ fp.x.append(x)
+ fp.y.append(y)
+ fp.yaw.append(theta)
+ fp.c.append(kappa)
+ fp.v.append(v)
+ fp.a.append(a)
+ return fp
+
+
+class LowSpeedLateralMovementStrategy(LateralMovementStrategy):
+ def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti):
+ s0 = fp.s[0]
+ s1 = fp.s[-1]
+ tp = copy.deepcopy(fp)
+ # d = d(s), d_d = d'(s), d_dd = d''(s)
+ # * shift s range from [s0, s1] to [0, s1 - s0]
+ lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, s1 - s0)
+
+ tp.d = [lat_qp.calc_point(s - s0) for s in fp.s]
+ tp.d_d = [lat_qp.calc_first_derivative(s - s0) for s in fp.s]
+ tp.d_dd = [lat_qp.calc_second_derivative(s - s0) for s in fp.s]
+ tp.d_ddd = [lat_qp.calc_third_derivative(s - s0) for s in fp.s]
+ return tp
+
+ def calc_cartesian_parameters(self, fp, csp):
+ # calc global positions
+ for i in range(len(fp.s)):
+ ix, iy = csp.calc_position(fp.s[i])
+ if ix is None:
+ break
+ i_yaw = csp.calc_yaw(fp.s[i])
+ i_kappa = csp.calc_curvature(fp.s[i])
+ i_dkappa = csp.calc_curvature_rate(fp.s[i])
+ s_condition = [fp.s[i], fp.s_d[i], fp.s_dd[i]]
+ d_condition = [fp.d[i], fp.d_d[i], fp.d_dd[i]]
+ x, y, theta, kappa, v, a = CartesianFrenetConverter.frenet_to_cartesian(
+ fp.s[i], ix, iy, i_yaw, i_kappa, i_dkappa, s_condition, d_condition
+ )
+ fp.x.append(x)
+ fp.y.append(y)
+ fp.yaw.append(theta)
+ fp.c.append(kappa)
+ fp.v.append(v)
+ fp.a.append(a)
+ return fp
+
+
+class LongitudinalMovementStrategy:
+ def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0):
+ """
+ Calculate the longitudinal trajectory
+ """
+ raise NotImplementedError("calc_longitudinal_trajectory not implemented")
+
+ def get_d_arrange(self, s0):
+ """
+ Get the d sample range
+ """
+ raise NotImplementedError("get_d_arrange not implemented")
+
+ def calc_destination_cost(self, fp):
+ """
+ Calculate the destination cost
+ """
+ raise NotImplementedError("calc_destination_cost not implemented")
+
+
+class VelocityKeepingLongitudinalMovementStrategy(LongitudinalMovementStrategy):
+ def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0):
+ fplist = []
+ for tv in np.arange(
+ TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S
+ ):
+ fp = FrenetPath()
+ lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)
+ fp.t = [t for t in np.arange(0.0, Ti, DT)]
+ fp.s = [lon_qp.calc_point(t) for t in fp.t]
+ fp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
+ fp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
+ fp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
+ fplist.append(fp)
+ return fplist
+
+ def get_d_arrange(self, s0):
+ return np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W)
+
+ def calc_destination_cost(self, fp):
+ ds = (TARGET_SPEED - fp.s_d[-1]) ** 2
+ return K_S_DOT * ds
+
+
+class MergingAndStoppingLongitudinalMovementStrategy(LongitudinalMovementStrategy):
+ def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0):
+ if s0 >= STOP_S:
+ return []
+ fplist = []
+ for s in np.arange(
+ STOP_S - D_S * N_STOP_S_SAMPLE, STOP_S + D_S * N_STOP_S_SAMPLE, D_S
+ ):
+ fp = FrenetPath()
+ lon_qp = QuinticPolynomial(s0, c_speed, c_accel, s, 0.0, 0.0, Ti)
+ fp.t = [t for t in np.arange(0.0, Ti, DT)]
+ fp.s = [lon_qp.calc_point(t) for t in fp.t]
+ fp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
+ fp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
+ fp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
+ fplist.append(fp)
+ return fplist
+
+ def get_d_arrange(self, s0):
+ # Only if s0 is less than STOP_S / 3, then we sample the road width
+ if s0 < STOP_S / 3:
+ return np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W)
+ else:
+ return [0.0]
+
+ def calc_destination_cost(self, fp):
+ ds = (STOP_S - fp.s[-1]) ** 2
+ return K_S * ds
+
+LATERAL_MOVEMENT_STRATEGY: LateralMovementStrategy
+LONGITUDINAL_MOVEMENT_STRATEGY: LongitudinalMovementStrategy
+
+if LATERAL_MOVEMENT == LateralMovement.HIGH_SPEED:
+ LATERAL_MOVEMENT_STRATEGY = HighSpeedLateralMovementStrategy()
+else:
+ LATERAL_MOVEMENT_STRATEGY = LowSpeedLateralMovementStrategy()
+
+if LONGITUDINAL_MOVEMENT == LongitudinalMovement.VELOCITY_KEEPING:
+ LONGITUDINAL_MOVEMENT_STRATEGY = VelocityKeepingLongitudinalMovementStrategy()
+else:
+ LONGITUDINAL_MOVEMENT_STRATEGY = MergingAndStoppingLongitudinalMovementStrategy()
+
+class QuarticPolynomial:
def __init__(self, xs, vxs, axs, vxe, axe, time):
# calc coefficient of quartic polynomial
@@ -67,29 +332,25 @@ def __init__(self, xs, vxs, axs, vxe, axe, time):
self.a1 = vxs
self.a2 = axs / 2.0
- A = np.array([[3 * time ** 2, 4 * time ** 3],
- [6 * time, 12 * time ** 2]])
- b = np.array([vxe - self.a1 - 2 * self.a2 * time,
- axe - 2 * self.a2])
+ A = np.array([[3 * time**2, 4 * time**3], [6 * time, 12 * time**2]])
+ b = np.array([vxe - self.a1 - 2 * self.a2 * time, axe - 2 * self.a2])
x = np.linalg.solve(A, b)
self.a3 = x[0]
self.a4 = x[1]
def calc_point(self, t):
- xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
- self.a3 * t ** 3 + self.a4 * t ** 4
+ xt = self.a0 + self.a1 * t + self.a2 * t**2 + self.a3 * t**3 + self.a4 * t**4
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
+ xt = self.a1 + 2 * self.a2 * t + 3 * self.a3 * t**2 + 4 * self.a4 * t**3
return xt
def calc_second_derivative(self, t):
- xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
+ xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2
return xt
@@ -100,111 +361,85 @@ def calc_third_derivative(self, t):
class FrenetPath:
-
def __init__(self):
self.t = []
self.d = []
- self.d_d = []
- self.d_dd = []
- self.d_ddd = []
+ self.d_d = [] # d'(s)
+ self.d_dd = [] # d''(s)
+ self.d_ddd = [] # d'''(t) in low speed / d'''(s) in high speed
self.s = []
- self.s_d = []
- self.s_dd = []
- self.s_ddd = []
- self.cd = 0.0
- self.cv = 0.0
+ self.s_d = [] # s'(t)
+ self.s_dd = [] # s''(t)
+ self.s_ddd = [] # s'''(t)
self.cf = 0.0
self.x = []
self.y = []
self.yaw = []
+ self.v = []
+ self.a = []
self.ds = []
self.c = []
-
-def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
+ def pop_front(self):
+ self.x.pop(0)
+ self.y.pop(0)
+ self.yaw.pop(0)
+ self.v.pop(0)
+ self.a.pop(0)
+ self.s.pop(0)
+ self.s_d.pop(0)
+ self.s_dd.pop(0)
+ self.s_ddd.pop(0)
+ self.d.pop(0)
+ self.d_d.pop(0)
+ self.d_dd.pop(0)
+ self.d_ddd.pop(0)
+
+
+def calc_frenet_paths(c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, s0):
frenet_paths = []
- # generate path to each offset goal
- for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
-
- # Lateral motion planning
- for Ti in np.arange(MIN_T, MAX_T, DT):
- fp = FrenetPath()
-
- # 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]
- fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
- fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
- fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
-
- # Longitudinal motion planning (Velocity keeping)
- for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
- TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
- tfp = copy.deepcopy(fp)
- lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
-
- tfp.s = [lon_qp.calc_point(t) for t in fp.t]
- tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
- tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
- tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
-
- Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk
- Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
-
- # square of diff from target speed
- ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
-
- tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
- tfp.cv = K_J * Js + K_T * Ti + K_D * ds
- tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
-
- frenet_paths.append(tfp)
+ for Ti in np.arange(MIN_T, MAX_T, DT):
+ lon_paths = LONGITUDINAL_MOVEMENT_STRATEGY.calc_longitudinal_trajectory(
+ c_s_d, c_s_dd, Ti, s0
+ )
+
+ for fp in lon_paths:
+ for di in LONGITUDINAL_MOVEMENT_STRATEGY.get_d_arrange(s0):
+ tp = LATERAL_MOVEMENT_STRATEGY.calc_lateral_trajectory(
+ fp, di, c_d, c_d_d, c_d_dd, Ti
+ )
+
+ Jp = sum(np.power(tp.d_ddd, 2)) # square of jerk
+ Js = sum(np.power(tp.s_ddd, 2)) # square of jerk
+
+ lat_cost = K_J * Jp + K_T * Ti + K_D * tp.d[-1] ** 2
+ lon_cost = (
+ K_J * Js
+ + K_T * Ti
+ + LONGITUDINAL_MOVEMENT_STRATEGY.calc_destination_cost(tp)
+ )
+ tp.cf = K_LAT * lat_cost + K_LON * lon_cost
+ frenet_paths.append(tp)
return frenet_paths
def calc_global_paths(fplist, csp):
- for fp in fplist:
-
- # calc global positions
- for i in range(len(fp.s)):
- ix, iy = csp.calc_position(fp.s[i])
- if ix is None:
- break
- i_yaw = csp.calc_yaw(fp.s[i])
- di = fp.d[i]
- fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
- fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
- fp.x.append(fx)
- fp.y.append(fy)
-
- # calc yaw and ds
- for i in range(len(fp.x) - 1):
- dx = fp.x[i + 1] - fp.x[i]
- dy = fp.y[i + 1] - fp.y[i]
- fp.yaw.append(math.atan2(dy, dx))
- fp.ds.append(math.hypot(dx, dy))
-
- fp.yaw.append(fp.yaw[-1])
- fp.ds.append(fp.ds[-1])
-
- # calc curvature
- for i in range(len(fp.yaw) - 1):
- fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])
-
- return fplist
+ return [
+ LATERAL_MOVEMENT_STRATEGY.calc_cartesian_parameters(fp, csp) for fp in fplist
+ ]
def check_collision(fp, ob):
for i in range(len(ob[:, 0])):
- d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
- for (ix, iy) in zip(fp.x, fp.y)]
+ d = [
+ ((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
+ for (ix, iy) in zip(fp.x, fp.y)
+ ]
- collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
+ collision = any([di <= ROBOT_RADIUS**2 for di in d])
if collision:
return False
@@ -213,38 +448,41 @@ def check_collision(fp, ob):
def check_paths(fplist, ob):
- ok_ind = []
+ path_dict = {
+ "max_speed_error": [],
+ "max_accel_error": [],
+ "max_curvature_error": [],
+ "collision_error": [],
+ "ok": [],
+ }
for i, _ in enumerate(fplist):
- if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check
- continue
- elif any([abs(a) > MAX_ACCEL for a in
- fplist[i].s_dd]): # Max accel check
- continue
- elif any([abs(c) > MAX_CURVATURE for c in
- fplist[i].c]): # Max curvature check
- continue
+ if any([v > MAX_SPEED for v in fplist[i].v]): # Max speed check
+ path_dict["max_speed_error"].append(fplist[i])
+ elif any([abs(a) > MAX_ACCEL for a in fplist[i].a]): # Max accel check
+ path_dict["max_accel_error"].append(fplist[i])
+ elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check
+ path_dict["max_curvature_error"].append(fplist[i])
elif not check_collision(fplist[i], ob):
- continue
-
- ok_ind.append(i)
+ path_dict["collision_error"].append(fplist[i])
+ else:
+ path_dict["ok"].append(fplist[i])
+ return path_dict
- return [fplist[i] for i in ok_ind]
-
-def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob):
- fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)
+def frenet_optimal_planning(csp, s0, c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, ob):
+ fplist = calc_frenet_paths(c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, s0)
fplist = calc_global_paths(fplist, csp)
- fplist = check_paths(fplist, ob)
+ fpdict = check_paths(fplist, ob)
# find minimum cost path
min_cost = float("inf")
best_path = None
- for fp in fplist:
+ for fp in fpdict["ok"]:
if min_cost >= fp.cf:
min_cost = fp.cf
best_path = fp
- return best_path
+ return [best_path, fpdict]
def generate_target_course(x, y):
@@ -265,38 +503,39 @@ def generate_target_course(x, y):
def main():
print(__file__ + " start!!")
- # way points
- wx = [0.0, 10.0, 20.5, 35.0, 70.5]
- wy = [0.0, -6.0, 5.0, 6.5, 0.0]
- # obstacle lists
- ob = np.array([[20.0, 10.0],
- [30.0, 6.0],
- [30.0, 8.0],
- [35.0, 8.0],
- [50.0, 3.0]
- ])
+ tx, ty, tyaw, tc, csp = generate_target_course(WX, WY)
- tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)
+ # Initialize state using global parameters
+ c_s_d = INITIAL_SPEED
+ c_s_dd = INITIAL_ACCEL
+ c_d = INITIAL_LAT_POSITION
+ c_d_d = INITIAL_LAT_SPEED
+ c_d_dd = INITIAL_LAT_ACCELERATION
+ s0 = INITIAL_COURSE_POSITION
- # initial state
- c_speed = 10.0 / 3.6 # current speed [m/s]
- c_d = 2.0 # current lateral position [m]
- c_d_d = 0.0 # current lateral speed [m/s]
- c_d_dd = 0.0 # current lateral acceleration [m/s]
- s0 = 0.0 # current course position
+ area = ANIMATION_AREA
- area = 20.0 # animation area length [m]
+ last_path = None
for i in range(SIM_LOOP):
- path = frenet_optimal_planning(
- csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob)
+ [path, fpdict] = frenet_optimal_planning(
+ csp, s0, c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, OBSTACLES
+ )
+
+ if path is None:
+ path = copy.deepcopy(last_path)
+ path.pop_front()
+ if len(path.x) <= 1:
+ print("Finish")
+ break
+ last_path = path
s0 = path.s[1]
c_d = path.d[1]
c_d_d = path.d_d[1]
c_d_dd = path.d_dd[1]
- c_speed = path.s_d[1]
-
+ c_s_d = path.s_d[1]
+ c_s_dd = path.s_dd[1]
if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0:
print("Goal")
break
@@ -305,15 +544,16 @@ def main():
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ "key_release_event",
+ lambda event: [exit(0) if event.key == "escape" else None],
+ )
plt.plot(tx, ty)
- plt.plot(ob[:, 0], ob[:, 1], "xk")
+ plt.plot(OBSTACLES[:, 0], OBSTACLES[:, 1], "xk")
plt.plot(path.x[1:], path.y[1:], "-or")
plt.plot(path.x[1], path.y[1], "vc")
plt.xlim(path.x[1] - area, path.x[1] + area)
plt.ylim(path.y[1] - area, path.y[1] + area)
- plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4])
+ plt.title("v[km/h]:" + str(path.v[1] * 3.6)[0:4])
plt.grid(True)
plt.pause(0.0001)
@@ -324,5 +564,5 @@ def main():
plt.show()
-if __name__ == '__main__':
+if __name__ == "__main__":
main()
diff --git a/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py b/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py
index f2416fba98..b259beb870 100644
--- a/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py
+++ b/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py
@@ -67,7 +67,7 @@ def planning(self, sx, sy, gx, gy):
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart
- while 1:
+ while True:
if len(open_set) == 0:
print("Open set is empty..")
break
diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
index 2f4d32b187..10ba98cd35 100644
--- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
+++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
@@ -4,25 +4,16 @@
author: Atsushi Sakai
"""
-import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
-
import math
from enum import IntEnum
-
import numpy as np
import matplotlib.pyplot as plt
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
from utils.angle import rot_mat_2d
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Mapping/")
-
-try:
- from grid_map_lib.grid_map_lib import GridMap
-except ImportError:
- raise
+from Mapping.grid_map_lib.grid_map_lib import GridMap, FloatGrid
do_animation = True
@@ -50,8 +41,7 @@ def move_target_grid(self, c_x_index, c_y_index, grid_map):
n_y_index = c_y_index
# found safe grid
- if not grid_map.check_occupied_from_xy_index(n_x_index, n_y_index,
- occupied_val=0.5):
+ if not self.check_occupied(n_x_index, n_y_index, grid_map):
return n_x_index, n_y_index
else: # occupied
next_c_x_index, next_c_y_index = self.find_safe_turning_grid(
@@ -60,19 +50,20 @@ def move_target_grid(self, c_x_index, c_y_index, grid_map):
# moving backward
next_c_x_index = -self.moving_direction + c_x_index
next_c_y_index = c_y_index
- if grid_map.check_occupied_from_xy_index(next_c_x_index,
- next_c_y_index):
+ if self.check_occupied(next_c_x_index, next_c_y_index, grid_map, FloatGrid(1.0)):
# moved backward, but the grid is occupied by obstacle
return None, None
else:
# keep moving until end
- while not grid_map.check_occupied_from_xy_index(
- next_c_x_index + self.moving_direction,
- next_c_y_index, occupied_val=0.5):
+ while not self.check_occupied(next_c_x_index + self.moving_direction, next_c_y_index, grid_map):
next_c_x_index += self.moving_direction
self.swap_moving_direction()
return next_c_x_index, next_c_y_index
+ @staticmethod
+ def check_occupied(c_x_index, c_y_index, grid_map, occupied_val=FloatGrid(0.5)):
+ return grid_map.check_occupied_from_xy_index(c_x_index, c_y_index, occupied_val)
+
def find_safe_turning_grid(self, c_x_index, c_y_index, grid_map):
for (d_x_ind, d_y_ind) in self.turing_window:
@@ -81,17 +72,14 @@ def find_safe_turning_grid(self, c_x_index, c_y_index, grid_map):
next_y_ind = d_y_ind + c_y_index
# found safe grid
- if not grid_map.check_occupied_from_xy_index(next_x_ind,
- next_y_ind,
- occupied_val=0.5):
+ if not self.check_occupied(next_x_ind, next_y_ind, grid_map):
return next_x_ind, next_y_ind
return None, None
def is_search_done(self, grid_map):
for ix in self.x_indexes_goal_y:
- if not grid_map.check_occupied_from_xy_index(ix, self.goal_y,
- occupied_val=0.5):
+ if not self.check_occupied(ix, self.goal_y, grid_map):
return False
# all lower grid is occupied
@@ -177,7 +165,7 @@ def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
for iy in x_range:
for ix in y_range:
- if not grid_map.check_occupied_from_xy_index(ix, iy):
+ if not SweepSearcher.check_occupied(ix, iy, grid_map):
y_index = iy
x_indexes.append(ix)
if y_index:
@@ -194,7 +182,7 @@ def setup_grid_map(ox, oy, resolution, sweep_direction, offset_grid=10):
grid_map = GridMap(width, height, resolution, center_x, center_y)
grid_map.print_grid_map_info()
- grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
+ grid_map.set_value_from_polygon(ox, oy, FloatGrid(1.0), inside=False)
grid_map.expand_grid()
x_inds_goal_y = []
@@ -212,7 +200,7 @@ def setup_grid_map(ox, oy, resolution, sweep_direction, offset_grid=10):
def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
# search start grid
c_x_index, c_y_index = sweep_searcher.search_start_grid(grid_map)
- if not grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5):
+ if not grid_map.set_value_from_xy_index(c_x_index, c_y_index, FloatGrid(0.5)):
print("Cannot find start grid")
return [], []
@@ -244,7 +232,7 @@ def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
px.append(x)
py.append(y)
- grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5)
+ grid_map.set_value_from_xy_index(c_x_index, c_y_index, FloatGrid(0.5))
if grid_search_animation:
grid_map.plot_grid_map(ax=ax)
diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py
index 870dd18535..959db74078 100644
--- a/PathPlanning/HybridAStar/car.py
+++ b/PathPlanning/HybridAStar/car.py
@@ -7,8 +7,9 @@
"""
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+root_dir = pathlib.Path(__file__).parent.parent.parent
+sys.path.append(str(root_dir))
from math import cos, sin, tan, pi
@@ -58,9 +59,9 @@ def rectangle_check(x, y, yaw, ox, oy):
rx, ry = converted_xy[0], converted_xy[1]
if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
- return False # no collision
+ return False # collision
- return True # collision
+ return True # no collision
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
@@ -76,7 +77,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
def plot_car(x, y, yaw):
car_color = '-k'
c, s = cos(yaw), sin(yaw)
- rot = Rot.from_euler('z', -yaw).as_matrix()[0:2, 0:2]
+ rot = rot_mat_2d(-yaw)
car_outline_x, car_outline_y = [], []
for rx, ry in zip(VRX, VRY):
converted_xy = np.stack([rx, ry]).T @ rot
@@ -96,7 +97,7 @@ def pi_2_pi(angle):
def move(x, y, yaw, distance, steer, L=WB):
x += distance * cos(yaw)
y += distance * sin(yaw)
- yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
+ yaw = pi_2_pi(yaw + distance * tan(steer) / L) # distance/2
return x, y, yaw
diff --git a/PathPlanning/HybridAStar/dynamic_programming_heuristic.py b/PathPlanning/HybridAStar/dynamic_programming_heuristic.py
index cc635260d6..09bcd556a6 100644
--- a/PathPlanning/HybridAStar/dynamic_programming_heuristic.py
+++ b/PathPlanning/HybridAStar/dynamic_programming_heuristic.py
@@ -65,7 +65,7 @@ def calc_distance_heuristic(gx, gy, ox, oy, resolution, rr):
open_set[calc_index(goal_node, x_w, min_x, min_y)] = goal_node
priority_queue = [(0, calc_index(goal_node, x_w, min_x, min_y))]
- while 1:
+ while True:
if not priority_queue:
break
cost, c_id = heapq.heappop(priority_queue)
@@ -150,7 +150,7 @@ def calc_obstacle_map(ox, oy, resolution, vr):
y = iy + min_y
# print(x, y)
for iox, ioy in zip(ox, oy):
- d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2)
+ d = math.hypot(iox - x, ioy - y)
if d <= vr / resolution:
obstacle_map[ix][iy] = True
break
diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py
index 4ef0c1dd21..0fa04189c6 100644
--- a/PathPlanning/HybridAStar/hybrid_a_star.py
+++ b/PathPlanning/HybridAStar/hybrid_a_star.py
@@ -8,22 +8,16 @@
import heapq
import math
-import os
-import sys
-
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial import cKDTree
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__))
- + "/../ReedsSheppPath")
-try:
- from dynamic_programming_heuristic import calc_distance_heuristic
- import reeds_shepp_path_planning as rs
- from car import move, check_car_collision, MAX_STEER, WB, plot_car,\
- BUBBLE_R
-except Exception:
- raise
+from dynamic_programming_heuristic import calc_distance_heuristic
+from ReedsSheppPath import reeds_shepp_path_planning as rs
+from car import move, check_car_collision, MAX_STEER, WB, plot_car, BUBBLE_R
XY_GRID_RESOLUTION = 2.0 # [m]
YAW_GRID_RESOLUTION = np.deg2rad(15.0) # [rad]
@@ -111,12 +105,13 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1]
arc_l = XY_GRID_RESOLUTION * 1.5
- x_list, y_list, yaw_list = [], [], []
+ x_list, y_list, yaw_list, direction_list = [], [], [], []
for _ in np.arange(0, arc_l, MOTION_RESOLUTION):
x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
x_list.append(x)
y_list.append(y)
yaw_list.append(yaw)
+ direction_list.append(direction == 1)
if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
return None
@@ -140,7 +135,7 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
cost = current.cost + added_cost + arc_l
node = Node(x_ind, y_ind, yaw_ind, d, x_list,
- y_list, yaw_list, [d],
+ y_list, yaw_list, direction_list,
parent_index=calc_index(current, config),
cost=cost, steer=steer)
@@ -287,7 +282,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
while True:
if not openList:
print("Error: Cannot find path, No open set")
- return [], [], []
+ return Path([], [], [], [], 0)
cost, c_id = heapq.heappop(pq)
if c_id in openList:
@@ -317,7 +312,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
neighbor_index = calc_index(neighbor, config)
if neighbor_index in closedList:
continue
- if neighbor not in openList \
+ if neighbor_index not in openList \
or openList[neighbor_index].cost > neighbor.cost:
heapq.heappush(
pq, (calc_cost(neighbor, h_dp, config),
diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index 10af3a81bd..0483949c99 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -6,12 +6,13 @@
Reference: Informed RRT*: Optimal Sampling-based Path planning Focused via
Direct Sampling of an Admissible Ellipsoidal Heuristic
-https://arxiv.org/pdf/1404.2334.pdf
+https://arxiv.org/pdf/1404.2334
"""
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import copy
import math
@@ -27,18 +28,17 @@
class InformedRRTStar:
- def __init__(self, start, goal,
- obstacleList, randArea,
- expandDis=0.5, goalSampleRate=10, maxIter=200):
+ def __init__(self, start, goal, obstacle_list, rand_area, expand_dis=0.5,
+ goal_sample_rate=10, max_iter=200):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
- self.min_rand = randArea[0]
- self.max_rand = randArea[1]
- self.expand_dis = expandDis
- self.goal_sample_rate = goalSampleRate
- self.max_iter = maxIter
- self.obstacle_list = obstacleList
+ self.min_rand = rand_area[0]
+ self.max_rand = rand_area[1]
+ self.expand_dis = expand_dis
+ self.goal_sample_rate = goal_sample_rate
+ self.max_iter = max_iter
+ self.obstacle_list = obstacle_list
self.node_list = None
def informed_rrt_star_search(self, animation=True):
@@ -46,110 +46,109 @@ def informed_rrt_star_search(self, animation=True):
self.node_list = [self.start]
# max length we expect to find in our 'informed' sample space,
# starts as infinite
- cBest = float('inf')
- solutionSet = set()
+ c_best = float('inf')
+ solution_set = set()
path = None
# Computing the sampling space
- cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
- + pow(self.start.y - self.goal.y, 2))
- xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
- [(self.start.y + self.goal.y) / 2.0], [0]])
- a1 = np.array([[(self.goal.x - self.start.x) / cMin],
- [(self.goal.y - self.start.y) / cMin], [0]])
-
- e_theta = math.atan2(a1[1], a1[0])
+ c_min = math.hypot(self.start.x - self.goal.x,
+ self.start.y - self.goal.y)
+ x_center = np.array([[(self.start.x + self.goal.x) / 2.0],
+ [(self.start.y + self.goal.y) / 2.0], [0]])
+ a1 = np.array([[(self.goal.x - self.start.x) / c_min],
+ [(self.goal.y - self.start.y) / c_min], [0]])
+
+ e_theta = math.atan2(a1[1, 0], a1[0, 0])
# first column of identity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
- M = a1 @ id1_t
- U, S, Vh = np.linalg.svd(M, True, True)
- C = np.dot(np.dot(U, np.diag(
- [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
- Vh)
+ m = a1 @ id1_t
+ u, s, vh = np.linalg.svd(m, True, True)
+ c = u @ np.diag(
+ [1.0, 1.0,
+ np.linalg.det(u) * np.linalg.det(np.transpose(vh))]) @ vh
for i in range(self.max_iter):
- # Sample space is defined by cBest
- # cMin is the minimum distance between the start point and the goal
- # xCenter is the midpoint between the start and the goal
- # cBest changes when a new path is found
+ # Sample space is defined by c_best
+ # c_min is the minimum distance between the start point and
+ # the goal x_center is the midpoint between the start and the
+ # goal c_best changes when a new path is found
- rnd = self.informed_sample(cBest, cMin, xCenter, C)
+ rnd = self.informed_sample(c_best, c_min, x_center, c)
n_ind = self.get_nearest_list_index(self.node_list, rnd)
- nearestNode = self.node_list[n_ind]
+ nearest_node = self.node_list[n_ind]
# steer
- theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
- newNode = self.get_new_node(theta, n_ind, nearestNode)
- d = self.line_cost(nearestNode, newNode)
+ theta = math.atan2(rnd[1] - nearest_node.y,
+ rnd[0] - nearest_node.x)
+ new_node = self.get_new_node(theta, n_ind, nearest_node)
+ d = self.line_cost(nearest_node, new_node)
- noCollision = self.check_collision(nearestNode, theta, d)
+ no_collision = self.check_collision(nearest_node, theta, d)
- if noCollision:
- nearInds = self.find_near_nodes(newNode)
- newNode = self.choose_parent(newNode, nearInds)
+ if no_collision:
+ near_inds = self.find_near_nodes(new_node)
+ new_node = self.choose_parent(new_node, near_inds)
- self.node_list.append(newNode)
- self.rewire(newNode, nearInds)
+ self.node_list.append(new_node)
+ self.rewire(new_node, near_inds)
- if self.is_near_goal(newNode):
- if self.check_segment_collision(newNode.x, newNode.y,
+ if self.is_near_goal(new_node):
+ if self.check_segment_collision(new_node.x, new_node.y,
self.goal.x, self.goal.y):
- solutionSet.add(newNode)
- lastIndex = len(self.node_list) - 1
- tempPath = self.get_final_course(lastIndex)
- tempPathLen = self.get_path_len(tempPath)
- if tempPathLen < cBest:
- path = tempPath
- cBest = tempPathLen
+ solution_set.add(new_node)
+ last_index = len(self.node_list) - 1
+ temp_path = self.get_final_course(last_index)
+ temp_path_len = self.get_path_len(temp_path)
+ if temp_path_len < c_best:
+ path = temp_path
+ c_best = temp_path_len
if animation:
- self.draw_graph(xCenter=xCenter,
- cBest=cBest, cMin=cMin,
+ self.draw_graph(x_center=x_center, c_best=c_best, c_min=c_min,
e_theta=e_theta, rnd=rnd)
return path
- def choose_parent(self, newNode, nearInds):
- if len(nearInds) == 0:
- return newNode
+ def choose_parent(self, new_node, near_inds):
+ if len(near_inds) == 0:
+ return new_node
- dList = []
- for i in nearInds:
- dx = newNode.x - self.node_list[i].x
- dy = newNode.y - self.node_list[i].y
+ d_list = []
+ for i in near_inds:
+ dx = new_node.x - self.node_list[i].x
+ dy = new_node.y - self.node_list[i].y
d = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
if self.check_collision(self.node_list[i], theta, d):
- dList.append(self.node_list[i].cost + d)
+ d_list.append(self.node_list[i].cost + d)
else:
- dList.append(float('inf'))
+ d_list.append(float('inf'))
- minCost = min(dList)
- minInd = nearInds[dList.index(minCost)]
+ min_cost = min(d_list)
+ min_ind = near_inds[d_list.index(min_cost)]
- if minCost == float('inf'):
+ if min_cost == float('inf'):
print("min cost is inf")
- return newNode
+ return new_node
- newNode.cost = minCost
- newNode.parent = minInd
+ new_node.cost = min_cost
+ new_node.parent = min_ind
- return newNode
+ return new_node
- def find_near_nodes(self, newNode):
+ def find_near_nodes(self, new_node):
n_node = len(self.node_list)
- r = 50.0 * math.sqrt((math.log(n_node) / n_node))
- d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2
- for node in self.node_list]
+ r = 50.0 * math.sqrt(math.log(n_node) / n_node)
+ d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2 for
+ node in self.node_list]
near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
return near_inds
- def informed_sample(self, cMax, cMin, xCenter, C):
- if cMax < float('inf'):
- r = [cMax / 2.0,
- math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
- math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
- L = np.diag(r)
- xBall = self.sample_unit_ball()
- rnd = np.dot(np.dot(C, L), xBall) + xCenter
+ def informed_sample(self, c_max, c_min, x_center, c):
+ if c_max < float('inf'):
+ r = [c_max / 2.0, math.sqrt(c_max ** 2 - c_min ** 2) / 2.0,
+ math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]
+ rl = np.diag(r)
+ x_ball = self.sample_unit_ball()
+ rnd = np.dot(np.dot(c, rl), x_ball) + x_center
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
else:
rnd = self.sample_free_space()
@@ -179,37 +178,36 @@ def sample_free_space(self):
@staticmethod
def get_path_len(path):
- pathLen = 0
+ path_len = 0
for i in range(1, len(path)):
node1_x = path[i][0]
node1_y = path[i][1]
node2_x = path[i - 1][0]
node2_y = path[i - 1][1]
- pathLen += math.sqrt((node1_x - node2_x)
- ** 2 + (node1_y - node2_y) ** 2)
+ path_len += math.hypot(node1_x - node2_x, node1_y - node2_y)
- return pathLen
+ return path_len
@staticmethod
def line_cost(node1, node2):
- return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
+ return math.hypot(node1.x - node2.x, node1.y - node2.y)
@staticmethod
def get_nearest_list_index(nodes, rnd):
- dList = [(node.x - rnd[0]) ** 2
- + (node.y - rnd[1]) ** 2 for node in nodes]
- minIndex = dList.index(min(dList))
- return minIndex
+ d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in
+ nodes]
+ min_index = d_list.index(min(d_list))
+ return min_index
- def get_new_node(self, theta, n_ind, nearestNode):
- newNode = copy.deepcopy(nearestNode)
+ def get_new_node(self, theta, n_ind, nearest_node):
+ new_node = copy.deepcopy(nearest_node)
- newNode.x += self.expand_dis * math.cos(theta)
- newNode.y += self.expand_dis * math.sin(theta)
+ new_node.x += self.expand_dis * math.cos(theta)
+ new_node.y += self.expand_dis * math.sin(theta)
- newNode.cost += self.expand_dis
- newNode.parent = n_ind
- return newNode
+ new_node.cost += self.expand_dis
+ new_node.parent = n_ind
+ return new_node
def is_near_goal(self, node):
d = self.line_cost(node, self.goal)
@@ -217,22 +215,21 @@ def is_near_goal(self, node):
return True
return False
- def rewire(self, newNode, nearInds):
+ def rewire(self, new_node, near_inds):
n_node = len(self.node_list)
- for i in nearInds:
- nearNode = self.node_list[i]
+ for i in near_inds:
+ near_node = self.node_list[i]
- d = math.sqrt((nearNode.x - newNode.x) ** 2
- + (nearNode.y - newNode.y) ** 2)
+ d = math.hypot(near_node.x - new_node.x, near_node.y - new_node.y)
- s_cost = newNode.cost + d
+ s_cost = new_node.cost + d
- if nearNode.cost > s_cost:
- theta = math.atan2(newNode.y - nearNode.y,
- newNode.x - nearNode.x)
- if self.check_collision(nearNode, theta, d):
- nearNode.parent = n_node - 1
- nearNode.cost = s_cost
+ if near_node.cost > s_cost:
+ theta = math.atan2(new_node.y - near_node.y,
+ new_node.x - near_node.x)
+ if self.check_collision(near_node, theta, d):
+ near_node.parent = n_node - 1
+ near_node.cost = s_cost
@staticmethod
def distance_squared_point_to_segment(v, w, p):
@@ -252,63 +249,62 @@ def distance_squared_point_to_segment(v, w, p):
def check_segment_collision(self, x1, y1, x2, y2):
for (ox, oy, size) in self.obstacle_list:
dd = self.distance_squared_point_to_segment(
- np.array([x1, y1]),
- np.array([x2, y2]),
- np.array([ox, oy]))
+ np.array([x1, y1]), np.array([x2, y2]), np.array([ox, oy]))
if dd <= size ** 2:
return False # collision
return True
- def check_collision(self, nearNode, theta, d):
- tmpNode = copy.deepcopy(nearNode)
- end_x = tmpNode.x + math.cos(theta) * d
- end_y = tmpNode.y + math.sin(theta) * d
- return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)
+ def check_collision(self, near_node, theta, d):
+ tmp_node = copy.deepcopy(near_node)
+ end_x = tmp_node.x + math.cos(theta) * d
+ end_y = tmp_node.y + math.sin(theta) * d
+ return self.check_segment_collision(tmp_node.x, tmp_node.y,
+ end_x, end_y)
- def get_final_course(self, lastIndex):
+ def get_final_course(self, last_index):
path = [[self.goal.x, self.goal.y]]
- while self.node_list[lastIndex].parent is not None:
- node = self.node_list[lastIndex]
+ while self.node_list[last_index].parent is not None:
+ node = self.node_list[last_index]
path.append([node.x, node.y])
- lastIndex = node.parent
+ last_index = node.parent
path.append([self.start.x, self.start.y])
return path
- def draw_graph(self, xCenter=None, cBest=None, cMin=None, e_theta=None,
+ def draw_graph(self, x_center=None, c_best=None, c_min=None, e_theta=None,
rnd=None):
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ 'key_release_event', lambda event:
+ [exit(0) if event.key == 'escape' else None])
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
- if cBest != float('inf'):
- self.plot_ellipse(xCenter, cBest, cMin, e_theta)
+ if c_best != float('inf'):
+ self.plot_ellipse(x_center, c_best, c_min, e_theta)
for node in self.node_list:
if node.parent is not None:
if node.x or node.y is not None:
- plt.plot([node.x, self.node_list[node.parent].x], [
- node.y, self.node_list[node.parent].y], "-g")
+ plt.plot([node.x, self.node_list[node.parent].x],
+ [node.y, self.node_list[node.parent].y], "-g")
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.goal.x, self.goal.y, "xr")
- plt.axis([-2, 15, -2, 15])
+ plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.grid(True)
plt.pause(0.01)
@staticmethod
- def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover
+ def plot_ellipse(x_center, c_best, c_min, e_theta): # pragma: no cover
- a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
- b = cBest / 2.0
+ a = math.sqrt(c_best ** 2 - c_min ** 2) / 2.0
+ b = c_best / 2.0
angle = math.pi / 2.0 - e_theta
- cx = xCenter[0]
- cy = xCenter[1]
+ cx = x_center[0]
+ cy = x_center[1]
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
@@ -332,18 +328,12 @@ def main():
print("Start informed rrt star planning")
# create obstacles
- obstacleList = [
- (5, 5, 0.5),
- (9, 6, 1),
- (7, 5, 1),
- (1, 5, 1),
- (3, 6, 1),
- (7, 9, 1)
- ]
+ obstacle_list = [(5, 5, 0.5), (9, 6, 1), (7, 5, 1), (1, 5, 1), (3, 6, 1),
+ (7, 9, 1)]
# Set params
- rrt = InformedRRTStar(start=[0, 0], goal=[5, 10],
- randArea=[-2, 15], obstacleList=obstacleList)
+ rrt = InformedRRTStar(start=[0, 0], goal=[5, 10], rand_area=[-2, 15],
+ obstacle_list=obstacle_list)
path = rrt.informed_rrt_star_search(animation=show_animation)
print("Done!!")
diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/lqr_planner.py
similarity index 98%
rename from PathPlanning/LQRPlanner/LQRplanner.py
rename to PathPlanning/LQRPlanner/lqr_planner.py
index ba01526a2c..0f58f93ea3 100644
--- a/PathPlanning/LQRPlanner/LQRplanner.py
+++ b/PathPlanning/LQRPlanner/lqr_planner.py
@@ -47,7 +47,7 @@ def lqr_planning(self, sx, sy, gx, gy, show_animation=True):
rx.append(x[0, 0] + gx)
ry.append(x[1, 0] + gy)
- d = math.sqrt((gx - rx[-1]) ** 2 + (gy - ry[-1]) ** 2)
+ d = math.hypot(gx - rx[-1], gy - ry[-1])
if d <= self.GOAL_DIST:
found_path = True
break
diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py
index 28d332aa00..0ed08123ea 100644
--- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py
+++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py
@@ -7,21 +7,15 @@
"""
import copy
import math
-import os
import random
-import sys
-
import matplotlib.pyplot as plt
import numpy as np
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../LQRPlanner/")
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRTStar/")
-
-try:
- from LQRplanner import LQRPlanner
- from rrt_star import RRTStar
-except ImportError:
- raise
+from LQRPlanner.lqr_planner import LQRPlanner
+from RRTStar.rrt_star import RRTStar
show_animation = True
@@ -185,7 +179,7 @@ def sample_path(self, wx, wy, step):
dx = np.diff(px)
dy = np.diff(py)
- clen = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
+ clen = [math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)]
return px, py, clen
diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py
new file mode 100644
index 0000000000..4c3b32f280
--- /dev/null
+++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py
@@ -0,0 +1,110 @@
+"""
+
+Lookup Table generation for model predictive trajectory generator
+
+author: Atsushi Sakai
+
+"""
+import sys
+import pathlib
+path_planning_dir = pathlib.Path(__file__).parent.parent
+sys.path.append(str(path_planning_dir))
+
+from matplotlib import pyplot as plt
+import numpy as np
+import math
+
+from ModelPredictiveTrajectoryGenerator import trajectory_generator,\
+ motion_model
+
+
+def calc_states_list(max_yaw=np.deg2rad(-30.0)):
+
+ x = np.arange(10.0, 30.0, 5.0)
+ y = np.arange(0.0, 20.0, 2.0)
+ yaw = np.arange(-max_yaw, max_yaw, max_yaw)
+
+ states = []
+ for iyaw in yaw:
+ for iy in y:
+ for ix in x:
+ states.append([ix, iy, iyaw])
+ print("n_state:", len(states))
+
+ return states
+
+
+def search_nearest_one_from_lookup_table(tx, ty, tyaw, lookup_table):
+ mind = float("inf")
+ minid = -1
+
+ for (i, table) in enumerate(lookup_table):
+
+ dx = tx - table[0]
+ dy = ty - table[1]
+ dyaw = tyaw - table[2]
+ d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
+ if d <= mind:
+ minid = i
+ mind = d
+
+ # print(minid)
+
+ return lookup_table[minid]
+
+
+def save_lookup_table(file_name, table):
+ np.savetxt(file_name, np.array(table),
+ fmt='%s', delimiter=",", header="x,y,yaw,s,km,kf", comments="")
+
+ print("lookup table file is saved as " + file_name)
+
+
+def generate_lookup_table():
+ states = calc_states_list(max_yaw=np.deg2rad(-30.0))
+ k0 = 0.0
+
+ # x, y, yaw, s, km, kf
+ lookup_table = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
+
+ for state in states:
+ best_p = search_nearest_one_from_lookup_table(
+ state[0], state[1], state[2], lookup_table)
+
+ target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
+ init_p = np.array(
+ [np.hypot(state[0], state[1]), best_p[4], best_p[5]]).reshape(3, 1)
+
+ x, y, yaw, p = trajectory_generator.optimize_trajectory(target,
+ k0, init_p)
+
+ if x is not None:
+ print("find good path")
+ lookup_table.append(
+ [x[-1], y[-1], yaw[-1], float(p[0, 0]), float(p[1, 0]), float(p[2, 0])])
+
+ print("finish lookup table generation")
+
+ save_lookup_table("lookup_table.csv", lookup_table)
+
+ for table in lookup_table:
+ x_c, y_c, yaw_c = motion_model.generate_trajectory(
+ table[3], table[4], table[5], k0)
+ plt.plot(x_c, y_c, "-r")
+ x_c, y_c, yaw_c = motion_model.generate_trajectory(
+ table[3], -table[4], -table[5], k0)
+ plt.plot(x_c, y_c, "-r")
+
+ plt.grid(True)
+ plt.axis("equal")
+ plt.show()
+
+ print("Done")
+
+
+def main():
+ generate_lookup_table()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py
deleted file mode 100644
index c57a05da57..0000000000
--- a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py
+++ /dev/null
@@ -1,114 +0,0 @@
-"""
-
-Lookup Table generation for model predictive trajectory generator
-
-author: Atsushi Sakai
-
-"""
-from matplotlib import pyplot as plt
-import numpy as np
-import math
-import model_predictive_trajectory_generator as planner
-import motion_model
-import pandas as pd
-
-
-def calc_states_list():
- maxyaw = np.deg2rad(-30.0)
-
- x = np.arange(10.0, 30.0, 5.0)
- y = np.arange(0.0, 20.0, 2.0)
- yaw = np.arange(-maxyaw, maxyaw, maxyaw)
-
- states = []
- for iyaw in yaw:
- for iy in y:
- for ix in x:
- states.append([ix, iy, iyaw])
- print("nstate:", len(states))
-
- return states
-
-
-def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
- mind = float("inf")
- minid = -1
-
- for (i, table) in enumerate(lookuptable):
-
- dx = tx - table[0]
- dy = ty - table[1]
- dyaw = tyaw - table[2]
- d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
- if d <= mind:
- minid = i
- mind = d
-
- # print(minid)
-
- return lookuptable[minid]
-
-
-def save_lookup_table(fname, table):
- mt = np.array(table)
- print(mt)
- # save csv
- df = pd.DataFrame()
- df["x"] = mt[:, 0]
- df["y"] = mt[:, 1]
- df["yaw"] = mt[:, 2]
- df["s"] = mt[:, 3]
- df["km"] = mt[:, 4]
- df["kf"] = mt[:, 5]
- df.to_csv(fname, index=None)
-
- print("lookup table file is saved as " + fname)
-
-
-def generate_lookup_table():
- states = calc_states_list()
- k0 = 0.0
-
- # x, y, yaw, s, km, kf
- lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
-
- for state in states:
- bestp = search_nearest_one_from_lookuptable(
- state[0], state[1], state[2], lookuptable)
-
- target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
- init_p = np.array(
- [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1)
-
- x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
-
- if x is not None:
- print("find good path")
- lookuptable.append(
- [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
-
- print("finish lookup table generation")
-
- save_lookup_table("lookuptable.csv", lookuptable)
-
- for table in lookuptable:
- xc, yc, yawc = motion_model.generate_trajectory(
- table[3], table[4], table[5], k0)
- plt.plot(xc, yc, "-r")
- xc, yc, yawc = motion_model.generate_trajectory(
- table[3], -table[4], -table[5], k0)
- plt.plot(xc, yc, "-r")
-
- plt.grid(True)
- plt.axis("equal")
- plt.show()
-
- print("Done")
-
-
-def main():
- generate_lookup_table()
-
-
-if __name__ == '__main__':
- main()
diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
index f6c78d3bc1..5ef6d2e23f 100644
--- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
+++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
@@ -1,10 +1,11 @@
import math
import numpy as np
-import scipy.interpolate
+from scipy.interpolate import interp1d
+from utils.angle import angle_mod
# motion parameter
L = 1.0 # wheel base
-ds = 0.1 # course distanse
+ds = 0.1 # course distance
v = 10.0 / 3.6 # velocity [m/s]
@@ -18,11 +19,10 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def update(state, v, delta, dt, L):
-
state.v = v
state.x = state.x + state.v * math.cos(state.yaw) * dt
state.y = state.y + state.v * math.sin(state.yaw) * dt
@@ -33,18 +33,20 @@ def update(state, v, delta, dt, L):
def generate_trajectory(s, km, kf, k0):
-
n = s / ds
time = s / v # [s]
-
- if isinstance(time, type(np.array([]))): time = time[0]
- if isinstance(km, type(np.array([]))): km = km[0]
- if isinstance(kf, type(np.array([]))): kf = kf[0]
-
+
+ if isinstance(time, type(np.array([]))):
+ time = time[0]
+ if isinstance(km, type(np.array([]))):
+ km = km[0]
+ if isinstance(kf, type(np.array([]))):
+ kf = kf[0]
+
tk = np.array([0.0, time / 2.0, time])
kk = np.array([k0, km, kf])
t = np.arange(0.0, time, time / n)
- fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic")
+ fkp = interp1d(tk, kk, kind="quadratic")
kp = [fkp(ti) for ti in t]
dt = float(time / n)
@@ -64,18 +66,22 @@ def generate_trajectory(s, km, kf, k0):
def generate_last_state(s, km, kf, k0):
-
n = s / ds
time = s / v # [s]
-
- if isinstance(time, type(np.array([]))): time = time[0]
- if isinstance(km, type(np.array([]))): km = km[0]
- if isinstance(kf, type(np.array([]))): kf = kf[0]
-
+
+ if isinstance(n, type(np.array([]))):
+ n = n[0]
+ if isinstance(time, type(np.array([]))):
+ time = time[0]
+ if isinstance(km, type(np.array([]))):
+ km = km[0]
+ if isinstance(kf, type(np.array([]))):
+ kf = kf[0]
+
tk = np.array([0.0, time / 2.0, time])
kk = np.array([k0, km, kf])
t = np.arange(0.0, time, time / n)
- fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic")
+ fkp = interp1d(tk, kk, kind="quadratic")
kp = [fkp(ti) for ti in t]
dt = time / n
diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py
similarity index 89%
rename from PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
rename to PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py
index 3ba1dd9fc2..6084fc1a07 100644
--- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
+++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py
@@ -7,15 +7,18 @@
"""
import math
-
import matplotlib.pyplot as plt
import numpy as np
+import sys
+import pathlib
+path_planning_dir = pathlib.Path(__file__).parent.parent
+sys.path.append(str(path_planning_dir))
-import motion_model
+import ModelPredictiveTrajectoryGenerator.motion_model as motion_model
# optimization parameter
max_iter = 100
-h = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance
+h: np.ndarray = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance
cost_th = 0.1
show_animation = True
@@ -103,7 +106,7 @@ def show_trajectory(target, xc, yc): # pragma: no cover
def optimize_trajectory(target, k0, p):
for i in range(max_iter):
- xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0)
+ xc, yc, yawc = motion_model.generate_trajectory(p[0, 0], p[1, 0], p[2, 0], k0)
dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1)
cost = np.linalg.norm(dc)
@@ -132,7 +135,7 @@ def optimize_trajectory(target, k0, p):
return xc, yc, yawc, p
-def test_optimize_trajectory(): # pragma: no cover
+def optimize_trajectory_demo(): # pragma: no cover
# target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0))
target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0))
@@ -152,7 +155,7 @@ def test_optimize_trajectory(): # pragma: no cover
def main(): # pragma: no cover
print(__file__ + " start!!")
- test_optimize_trajectory()
+ optimize_trajectory_demo()
if __name__ == '__main__':
diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
index 8f136b5ee3..603a9d16cf 100644
--- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
+++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
@@ -4,7 +4,7 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
"""
diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
index 294c389023..8bacfd5d19 100644
--- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
+++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
@@ -273,20 +273,20 @@ def main(rng=None):
oy = []
for i in range(60):
- ox.append(i)
+ ox.append(float(i))
oy.append(0.0)
for i in range(60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(61):
ox.append(0.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
index 8ba11a23d2..86f9f662da 100644
--- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
+++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
@@ -4,9 +4,9 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
-- [Local Path planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
+- [Local Path planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
"""
diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py
index 55092e6e00..e6dd9b648b 100644
--- a/PathPlanning/RRT/rrt.py
+++ b/PathPlanning/RRT/rrt.py
@@ -199,7 +199,7 @@ def draw_graph(self, rnd=None):
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis("equal")
- plt.axis([-2, 15, -2, 15])
+ plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.grid(True)
plt.pause(0.01)
diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py
index 9826dba072..ac68efe23f 100644
--- a/PathPlanning/RRT/rrt_with_pathsmoothing.py
+++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py
@@ -7,18 +7,13 @@
"""
import math
-import os
import random
-import sys
-
import matplotlib.pyplot as plt
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__)))
-
-try:
- from rrt import RRT
-except ImportError:
- raise
+from rrt import RRT
show_animation = True
@@ -28,7 +23,7 @@ def get_path_length(path):
for i in range(len(path) - 1):
dx = path[i + 1][0] - path[i][0]
dy = path[i + 1][1] - path[i][1]
- d = math.sqrt(dx * dx + dy * dy)
+ d = math.hypot(dx, dy)
le += d
return le
@@ -41,7 +36,7 @@ def get_target_point(path, targetL):
for i in range(len(path) - 1):
dx = path[i + 1][0] - path[i][0]
dy = path[i + 1][1] - path[i][1]
- d = math.sqrt(dx * dx + dy * dy)
+ d = math.hypot(dx, dy)
le += d
if le >= targetL:
ti = i - 1
@@ -56,30 +51,93 @@ def get_target_point(path, targetL):
return [x, y, ti]
-def line_collision_check(first, second, obstacleList):
- # Line Equation
-
- x1 = first[0]
- y1 = first[1]
- x2 = second[0]
- y2 = second[1]
-
- try:
- a = y2 - y1
- b = -(x2 - x1)
- c = y2 * (x2 - x1) - x2 * (y2 - y1)
- except ZeroDivisionError:
- return False
-
- for (ox, oy, size) in obstacleList:
- d = abs(a * ox + b * oy + c) / (math.sqrt(a * a + b * b))
- if d <= size:
- return False
-
- return True # OK
-
-
-def path_smoothing(path, max_iter, obstacle_list):
+def is_point_collision(x, y, obstacle_list, robot_radius):
+ """
+ Check whether a single point collides with any obstacle.
+
+ This function calculates the Euclidean distance between the given point (x, y)
+ and each obstacle center. If the distance is less than or equal to the sum of
+ the obstacle's radius and the robot's radius, a collision is detected.
+
+ Args:
+ x (float): X-coordinate of the point to check.
+ y (float): Y-coordinate of the point to check.
+ obstacle_list (List[Tuple[float, float, float]]): List of obstacles defined as (ox, oy, radius).
+ robot_radius (float): Radius of the robot, used to inflate the obstacles.
+
+ Returns:
+ bool: True if the point is in collision with any obstacle, False otherwise.
+ """
+ for (ox, oy, obstacle_radius) in obstacle_list:
+ d = math.hypot(ox - x, oy - y)
+ if d <= obstacle_radius + robot_radius:
+ return True # Collided
+ return False
+
+
+def line_collision_check(first, second, obstacle_list, robot_radius=0.0, sample_step=0.2):
+ """
+ Check if the line segment between `first` and `second` collides with any obstacle.
+ Considers the robot_radius by inflating the obstacle size.
+
+ Args:
+ first (List[float]): Start point of the line [x, y]
+ second (List[float]): End point of the line [x, y]
+ obstacle_list (List[Tuple[float, float, float]]): Obstacles as (x, y, radius)
+ robot_radius (float): Radius of robot
+ sample_step (float): Distance between sampling points along the segment
+
+ Returns:
+ bool: True if collision-free, False otherwise
+ """
+ x1, y1 = first[0], first[1]
+ x2, y2 = second[0], second[1]
+
+ dx = x2 - x1
+ dy = y2 - y1
+ length = math.hypot(dx, dy)
+
+ if length == 0:
+ # Degenerate case: point collision check
+ return not is_point_collision(x1, y1, obstacle_list, robot_radius)
+
+ steps = int(length / sample_step) + 1 # Sampling every sample_step along the segment
+
+ for i in range(steps + 1):
+ t = i / steps
+ x = x1 + t * dx
+ y = y1 + t * dy
+
+ if is_point_collision(x, y, obstacle_list, robot_radius):
+ return False # Collision found
+
+ return True # Safe
+
+
+def path_smoothing(path, max_iter, obstacle_list, robot_radius=0.0):
+ """
+ Smooths a given path by iteratively replacing segments with shortcut connections,
+ while ensuring the new segments are collision-free.
+
+ The algorithm randomly picks two points along the original path and attempts to
+ connect them with a straight line. If the line does not collide with any obstacles
+ (considering the robot's radius), the intermediate path points between them are
+ replaced with the direct connection.
+
+ Args:
+ path (List[List[float]]): The original path as a list of [x, y] coordinates.
+ max_iter (int): Number of iterations for smoothing attempts.
+ obstacle_list (List[Tuple[float, float, float]]): List of obstacles represented as
+ (x, y, radius).
+ robot_radius (float, optional): Radius of the robot, used to inflate obstacle size
+ during collision checking. Defaults to 0.0.
+
+ Returns:
+ List[List[float]]: The smoothed path as a list of [x, y] coordinates.
+
+ Example:
+ >>> smoothed = path_smoothing(path, 1000, obstacle_list, robot_radius=0.5)
+ """
le = get_path_length(path)
for i in range(max_iter):
@@ -99,7 +157,7 @@ def path_smoothing(path, max_iter, obstacle_list):
continue
# collision check
- if not line_collision_check(first, second, obstacle_list):
+ if not line_collision_check(first, second, obstacle_list, robot_radius):
continue
# Create New path
@@ -124,14 +182,16 @@ def main():
(3, 10, 2),
(7, 5, 2),
(9, 5, 2)
- ] # [x,y,size]
+ ] # [x,y,radius]
rrt = RRT(start=[0, 0], goal=[6, 10],
- rand_area=[-2, 15], obstacle_list=obstacleList)
+ rand_area=[-2, 15], obstacle_list=obstacleList,
+ robot_radius=0.3)
path = rrt.planning(animation=show_animation)
# Path smoothing
maxIter = 1000
- smoothedPath = path_smoothing(path, maxIter, obstacleList)
+ smoothedPath = path_smoothing(path, maxIter, obstacleList,
+ robot_radius=rrt.robot_radius)
# Draw final path
if show_animation:
diff --git a/PathPlanning/RRT/rrt_with_sobol_sampler.py b/PathPlanning/RRT/rrt_with_sobol_sampler.py
index 8045d8f764..06e0c04c68 100644
--- a/PathPlanning/RRT/rrt_with_sobol_sampler.py
+++ b/PathPlanning/RRT/rrt_with_sobol_sampler.py
@@ -28,12 +28,12 @@
import math
import random
-from sobol import sobol_quasirand
import sys
-
import matplotlib.pyplot as plt
import numpy as np
+from sobol import sobol_quasirand
+
show_animation = True
diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py
index 428ba58a98..520d686a1d 100644
--- a/PathPlanning/RRT/sobol/sobol.py
+++ b/PathPlanning/RRT/sobol/sobol.py
@@ -13,7 +13,7 @@
PYTHON versions by Corrado Chisari
Original code is available at
- http://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html
+ https://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html
Note: the i4 prefix means that the function takes a numeric argument or
returns a number which is interpreted inside the function as a 4
@@ -370,8 +370,8 @@ def i4_sobol(dim_num, seed):
if (dim_num < 1 or dim_max < dim_num):
print('I4_SOBOL - Fatal error!')
print(' The spatial dimension DIM_NUM should satisfy:')
- print(' 1 <= DIM_NUM <= %d' % dim_max)
- print(' But this input value is DIM_NUM = %d' % dim_num)
+ print(f' 1 <= DIM_NUM <= {dim_max:d}')
+ print(f' But this input value is DIM_NUM = {dim_num:d}')
return None
dim_num_save = dim_num
@@ -443,7 +443,7 @@ def i4_sobol(dim_num, seed):
#
l_var = i4_bit_lo0(seed)
- elif (seed <= seed_save):
+ elif seed <= seed_save:
seed_save = 0
lastq = np.zeros(dim_num)
@@ -471,8 +471,8 @@ def i4_sobol(dim_num, seed):
if maxcol < l_var:
print('I4_SOBOL - Fatal error!')
print(' Too many calls!')
- print(' MAXCOL = %d\n' % maxcol)
- print(' L = %d\n' % l_var)
+ print(f' MAXCOL = {maxcol:d}\n')
+ print(f' L = {l_var:d}\n')
return None
@@ -819,7 +819,7 @@ def r8mat_write(filename, m, n, a):
with open(filename, 'w') as output:
for i in range(0, m):
for j in range(0, n):
- s = ' %g' % (a[i, j])
+ s = f' {a[i, j]:g}'
output.write(s)
output.write('\n')
diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py
index f1bd03ca02..f938419f35 100644
--- a/PathPlanning/RRTDubins/rrt_dubins.py
+++ b/PathPlanning/RRTDubins/rrt_dubins.py
@@ -6,23 +6,17 @@
"""
import copy
import math
-import os
import random
-import sys
-
-import matplotlib.pyplot as plt
import numpy as np
+import matplotlib.pyplot as plt
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) # root dir
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../DubinsPath/")
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../RRT/")
-
-try:
- from rrt import RRT
- import dubins_path_planner
-except ImportError:
- raise
+from RRT.rrt import RRT
+from DubinsPath import dubins_path_planner
+from utils.plot import plot_arrow
show_animation = True
@@ -130,10 +124,8 @@ def draw_graph(self, rnd=None): # pragma: no cover
plt.pause(0.01)
def plot_start_goal_arrow(self): # pragma: no cover
- dubins_path_planner.plot_arrow(
- self.start.x, self.start.y, self.start.yaw)
- dubins_path_planner.plot_arrow(
- self.end.x, self.end.y, self.end.yaw)
+ plot_arrow(self.start.x, self.start.y, self.start.yaw)
+ plot_arrow(self.end.x, self.end.y, self.end.yaw)
def steer(self, from_node, to_node):
@@ -214,6 +206,7 @@ def generate_final_course(self, goal_index):
def main():
+
print("Start " + __file__)
# ====Search Path with RRT====
obstacleList = [
diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py
index e2c05456de..dcb1a066eb 100644
--- a/PathPlanning/RRTStar/rrt_star.py
+++ b/PathPlanning/RRTStar/rrt_star.py
@@ -7,17 +7,12 @@
"""
import math
-import os
import sys
-
import matplotlib.pyplot as plt
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRT/")
-
-try:
- from rrt import RRT
-except ImportError:
- raise
+from RRT.rrt import RRT
show_animation = True
@@ -59,6 +54,7 @@ def __init__(self,
self.connect_circle_dist = connect_circle_dist
self.goal_node = self.Node(goal[0], goal[1])
self.search_until_max_iter = search_until_max_iter
+ self.node_list = []
def planning(self, animation=True):
"""
@@ -167,9 +163,13 @@ def search_best_goal_node(self):
if not safe_goal_inds:
return None
- min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
- for i in safe_goal_inds:
- if self.node_list[i].cost == min_cost:
+ safe_goal_costs = [self.node_list[i].cost +
+ self.calc_dist_to_goal(self.node_list[i].x, self.node_list[i].y)
+ for i in safe_goal_inds]
+
+ min_cost = min(safe_goal_costs)
+ for i, cost in zip(safe_goal_inds, safe_goal_costs):
+ if cost == min_cost:
return i
return None
@@ -190,7 +190,7 @@ def find_near_nodes(self, new_node):
radius r
"""
nnode = len(self.node_list) + 1
- r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
+ r = self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode)
# if expand_dist exists, search vertices in a range no more than
# expand_dist
if hasattr(self, 'expand_dis'):
@@ -229,13 +229,11 @@ def rewire(self, new_node, near_inds):
improved_cost = near_node.cost > edge_node.cost
if no_collision and improved_cost:
- near_node.x = edge_node.x
- near_node.y = edge_node.y
- near_node.cost = edge_node.cost
- near_node.path_x = edge_node.path_x
- near_node.path_y = edge_node.path_y
- near_node.parent = edge_node.parent
- self.propagate_cost_to_leaves(new_node)
+ for node in self.node_list:
+ if node.parent == self.node_list[i]:
+ node.parent = edge_node
+ self.node_list[i] = edge_node
+ self.propagate_cost_to_leaves(self.node_list[i])
def calc_new_cost(self, from_node, to_node):
d, _ = self.calc_distance_and_angle(from_node, to_node)
@@ -284,7 +282,7 @@ def main():
rrt_star.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
plt.grid(True)
- plt.show()
+ plt.show()
if __name__ == '__main__':
diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py
index e2a456d75f..7c52879b7c 100644
--- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py
+++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py
@@ -4,26 +4,19 @@
author: AtsushiSakai(@Atsushi_twi)
"""
-
import copy
import math
-import os
import random
-import sys
-
import matplotlib.pyplot as plt
import numpy as np
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) # root dir
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../DubinsPath/")
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../RRTStar/")
-
-try:
- import dubins_path_planner
- from rrt_star import RRTStar
-except ImportError:
- raise
+from DubinsPath import dubins_path_planner
+from RRTStar.rrt_star import RRTStar
+from utils.plot import plot_arrow
show_animation = True
@@ -136,10 +129,8 @@ def draw_graph(self, rnd=None):
plt.pause(0.01)
def plot_start_goal_arrow(self):
- dubins_path_planner.plot_arrow(
- self.start.x, self.start.y, self.start.yaw)
- dubins_path_planner.plot_arrow(
- self.end.x, self.end.y, self.end.yaw)
+ plot_arrow(self.start.x, self.start.y, self.start.yaw)
+ plot_arrow(self.end.x, self.end.y, self.end.yaw)
def steer(self, from_node, to_node):
diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
index 323bc76be2..c4c3e7c8a8 100644
--- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
+++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
@@ -7,23 +7,15 @@
"""
import copy
import math
-import os
import random
import sys
-
+import pathlib
import matplotlib.pyplot as plt
import numpy as np
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../ReedsSheppPath/")
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../RRTStar/")
-
-try:
- import reeds_shepp_path_planning
- from rrt_star import RRTStar
-except ImportError:
- raise
+from ReedsSheppPath import reeds_shepp_path_planning
+from RRTStar.rrt_star import RRTStar
show_animation = True
@@ -44,7 +36,7 @@ def __init__(self, x, y, yaw):
self.path_yaw = []
def __init__(self, start, goal, obstacle_list, rand_area,
- max_iter=200,
+ max_iter=200, step_size=0.2,
connect_circle_dist=50.0,
robot_radius=0.0
):
@@ -63,6 +55,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.max_iter = max_iter
+ self.step_size = step_size
self.obstacle_list = obstacle_list
self.connect_circle_dist = connect_circle_dist
self.robot_radius = robot_radius
@@ -71,6 +64,9 @@ def __init__(self, start, goal, obstacle_list, rand_area,
self.goal_yaw_th = np.deg2rad(1.0)
self.goal_xy_th = 0.5
+ def set_random_seed(self, seed):
+ random.seed(seed)
+
def planning(self, animation=True, search_until_max_iter=True):
"""
planning
@@ -155,8 +151,8 @@ def plot_start_goal_arrow(self):
def steer(self, from_node, to_node):
px, py, pyaw, mode, course_lengths = reeds_shepp_path_planning.reeds_shepp_path_planning(
- from_node.x, from_node.y, from_node.yaw,
- to_node.x, to_node.y, to_node.yaw, self.curvature)
+ from_node.x, from_node.y, from_node.yaw, to_node.x,
+ to_node.y, to_node.yaw, self.curvature, self.step_size)
if not px:
return None
@@ -177,8 +173,8 @@ def steer(self, from_node, to_node):
def calc_new_cost(self, from_node, to_node):
_, _, _, _, course_lengths = reeds_shepp_path_planning.reeds_shepp_path_planning(
- from_node.x, from_node.y, from_node.yaw,
- to_node.x, to_node.y, to_node.yaw, self.curvature)
+ from_node.x, from_node.y, from_node.yaw, to_node.x,
+ to_node.y, to_node.yaw, self.curvature, self.step_size)
if not course_lengths:
return float("inf")
diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
index 5bc04c8ee3..618d1d99ba 100644
--- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
+++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
@@ -3,12 +3,18 @@
Reeds Shepp path planner sample code
author Atsushi Sakai(@Atsushi_twi)
+co-author Videh Patel(@videh25) : Added the missing RS paths
"""
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
import math
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
show_animation = True
@@ -40,6 +46,9 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
plt.plot(x, y)
+def pi_2_pi(x):
+ return angle_mod(x)
+
def mod2pi(x):
# Be consistent with fmod in cplusplus here.
v = np.mod(x, np.copysign(2.0 * math.pi, x))
@@ -50,25 +59,6 @@ def mod2pi(x):
v -= 2.0 * math.pi
return v
-
-def straight_left_straight(x, y, phi):
- phi = mod2pi(phi)
- if y > 0.0 and 0.0 < phi < math.pi * 0.99:
- xd = - y / math.tan(phi) + x
- t = xd - math.tan(phi / 2.0)
- u = phi
- v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
- return True, t, u, v
- elif y < 0.0 < phi < math.pi * 0.99:
- xd = - y / math.tan(phi) + x
- t = xd - math.tan(phi / 2.0)
- u = phi
- v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
- return True, t, u, v
-
- return False, 0.0, 0.0, 0.0
-
-
def set_path(paths, lengths, ctypes, step_size):
path = Path()
path.ctypes = ctypes
@@ -90,137 +80,208 @@ def set_path(paths, lengths, ctypes, step_size):
return paths
-def straight_curve_straight(x, y, phi, paths, step_size):
- flag, t, u, v = straight_left_straight(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["S", "L", "S"], step_size)
-
- flag, t, u, v = straight_left_straight(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["S", "R", "S"], step_size)
-
- return paths
-
-
def polar(x, y):
- r = math.sqrt(x ** 2 + y ** 2)
+ r = math.hypot(x, y)
theta = math.atan2(y, x)
return r, theta
def left_straight_left(x, y, phi):
u, t = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
- if t >= 0.0:
+ if 0.0 <= t <= math.pi:
v = mod2pi(phi - t)
- if v >= 0.0:
- return True, t, u, v
+ if 0.0 <= v <= math.pi:
+ return True, [t, u, v], ['L', 'S', 'L']
+
+ return False, [], []
- return False, 0.0, 0.0, 0.0
+def left_straight_right(x, y, phi):
+ u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi))
+ u1 = u1 ** 2
+ if u1 >= 4.0:
+ u = math.sqrt(u1 - 4.0)
+ theta = math.atan2(2.0, u)
+ t = mod2pi(t1 + theta)
+ v = mod2pi(t - phi)
+
+ if (t >= 0.0) and (v >= 0.0):
+ return True, [t, u, v], ['L', 'S', 'R']
-def left_right_left(x, y, phi):
- u1, t1 = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
+ return False, [], []
+
+
+def left_x_right_x_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
if u1 <= 4.0:
- u = -2.0 * math.asin(0.25 * u1)
- t = mod2pi(t1 + 0.5 * u + math.pi)
- v = mod2pi(phi - t + u)
+ A = math.acos(0.25 * u1)
+ t = mod2pi(A + theta + math.pi/2)
+ u = mod2pi(math.pi - 2 * A)
+ v = mod2pi(phi - t - u)
+ return True, [t, -u, v], ['L', 'R', 'L']
+
+ return False, [], []
- if t >= 0.0 >= u:
- return True, t, u, v
- return False, 0.0, 0.0, 0.0
+def left_x_right_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
+ if u1 <= 4.0:
+ A = math.acos(0.25 * u1)
+ t = mod2pi(A + theta + math.pi/2)
+ u = mod2pi(math.pi - 2*A)
+ v = mod2pi(-phi + t + u)
+ return True, [t, -u, -v], ['L', 'R', 'L']
-def curve_curve_curve(x, y, phi, paths, step_size):
- flag, t, u, v = left_right_left(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["L", "R", "L"], step_size)
+ return False, [], []
- flag, t, u, v = left_right_left(-x, y, -phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"], step_size)
- flag, t, u, v = left_right_left(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["R", "L", "R"], step_size)
+def left_right_x_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- flag, t, u, v = left_right_left(-x, -y, phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"], step_size)
+ if u1 <= 4.0:
+ u = math.acos(1 - u1**2 * 0.125)
+ A = math.asin(2 * math.sin(u) / u1)
+ t = mod2pi(-A + theta + math.pi/2)
+ v = mod2pi(t - u - phi)
+ return True, [t, u, -v], ['L', 'R', 'L']
+
+ return False, [], []
+
+
+def left_right_x_left_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
+
+ # Solutions refering to (2 < u1 <= 4) are considered sub-optimal in paper
+ # Solutions do not exist for u1 > 4
+ if u1 <= 2:
+ A = math.acos((u1 + 2) * 0.25)
+ t = mod2pi(theta + A + math.pi/2)
+ u = mod2pi(A)
+ v = mod2pi(phi - t + 2*u)
+ if ((t >= 0) and (u >= 0) and (v >= 0)):
+ return True, [t, u, -u, -v], ['L', 'R', 'L', 'R']
+
+ return False, [], []
+
+
+def left_x_right_left_x_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
+ u2 = (20 - u1**2) / 16
+
+ if (0 <= u2 <= 1):
+ u = math.acos(u2)
+ A = math.asin(2 * math.sin(u) / u1)
+ t = mod2pi(theta + A + math.pi/2)
+ v = mod2pi(t - phi)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -u, -u, v], ['L', 'R', 'L', 'R']
- # backwards
- xb = x * math.cos(phi) + y * math.sin(phi)
- yb = x * math.sin(phi) - y * math.cos(phi)
+ return False, [], []
- flag, t, u, v = left_right_left(xb, yb, phi)
- if flag:
- paths = set_path(paths, [v, u, t], ["L", "R", "L"], step_size)
- flag, t, u, v = left_right_left(-xb, yb, -phi)
- if flag:
- paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"], step_size)
+def left_x_right90_straight_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- flag, t, u, v = left_right_left(xb, -yb, -phi)
- if flag:
- paths = set_path(paths, [v, u, t], ["R", "L", "R"], step_size)
+ if u1 >= 2.0:
+ u = math.sqrt(u1**2 - 4) - 2
+ A = math.atan2(2, math.sqrt(u1**2 - 4))
+ t = mod2pi(theta + A + math.pi/2)
+ v = mod2pi(t - phi + math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -math.pi/2, -u, -v], ['L', 'R', 'S', 'L']
- flag, t, u, v = left_right_left(-xb, -yb, phi)
- if flag:
- paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"], step_size)
+ return False, [], []
- return paths
+def left_straight_right90_x_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
-def curve_straight_curve(x, y, phi, paths, step_size):
- flag, t, u, v = left_straight_left(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["L", "S", "L"], step_size)
+ if u1 >= 2.0:
+ u = math.sqrt(u1**2 - 4) - 2
+ A = math.atan2(math.sqrt(u1**2 - 4), 2)
+ t = mod2pi(theta - A + math.pi/2)
+ v = mod2pi(t - phi - math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, u, math.pi/2, -v], ['L', 'S', 'R', 'L']
- flag, t, u, v = left_straight_left(-x, y, -phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"], step_size)
+ return False, [], []
- flag, t, u, v = left_straight_left(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["R", "S", "R"], step_size)
- flag, t, u, v = left_straight_left(-x, -y, phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"], step_size)
+def left_x_right90_straight_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- flag, t, u, v = left_straight_right(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["L", "S", "R"], step_size)
+ if u1 >= 2.0:
+ t = mod2pi(theta + math.pi/2)
+ u = u1 - 2
+ v = mod2pi(phi - t - math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -math.pi/2, -u, -v], ['L', 'R', 'S', 'R']
- flag, t, u, v = left_straight_right(-x, y, -phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"], step_size)
+ return False, [], []
- flag, t, u, v = left_straight_right(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["R", "S", "L"], step_size)
- flag, t, u, v = left_straight_right(-x, -y, phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"], step_size)
+def left_straight_left90_x_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- return paths
+ if u1 >= 2.0:
+ t = mod2pi(theta)
+ u = u1 - 2
+ v = mod2pi(phi - t - math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, u, math.pi/2, -v], ['L', 'S', 'L', 'R']
+ return False, [], []
+
+
+def left_x_right90_straight_left90_x_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
-def left_straight_right(x, y, phi):
- u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi))
- u1 = u1 ** 2
if u1 >= 4.0:
- u = math.sqrt(u1 - 4.0)
- theta = math.atan2(2.0, u)
- t = mod2pi(t1 + theta)
+ u = math.sqrt(u1**2 - 4) - 4
+ A = math.atan2(2, math.sqrt(u1**2 - 4))
+ t = mod2pi(theta + A + math.pi/2)
v = mod2pi(t - phi)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -math.pi/2, -u, -math.pi/2, v], ['L', 'R', 'S', 'L', 'R']
+
+ return False, [], []
+
- if t >= 0.0 and v >= 0.0:
- return True, t, u, v
+def timeflip(travel_distances):
+ return [-x for x in travel_distances]
- return False, 0.0, 0.0, 0.0
+
+def reflect(steering_directions):
+ def switch_dir(dirn):
+ if dirn == 'L':
+ return 'R'
+ elif dirn == 'R':
+ return 'L'
+ else:
+ return 'S'
+ return[switch_dir(dirn) for dirn in steering_directions]
def generate_path(q0, q1, max_curvature, step_size):
@@ -231,11 +292,52 @@ def generate_path(q0, q1, max_curvature, step_size):
s = math.sin(q0[2])
x = (c * dx + s * dy) * max_curvature
y = (-s * dx + c * dy) * max_curvature
+ step_size *= max_curvature
paths = []
- paths = straight_curve_straight(x, y, dth, paths, step_size)
- paths = curve_straight_curve(x, y, dth, paths, step_size)
- paths = curve_curve_curve(x, y, dth, paths, step_size)
+ path_functions = [left_straight_left, left_straight_right, # CSC
+ left_x_right_x_left, left_x_right_left, left_right_x_left, # CCC
+ left_right_x_left_right, left_x_right_left_x_right, # CCCC
+ left_x_right90_straight_left, left_x_right90_straight_right, # CCSC
+ left_straight_right90_x_left, left_straight_left90_x_right, # CSCC
+ left_x_right90_straight_left90_x_right] # CCSCC
+
+ for path_func in path_functions:
+ flag, travel_distances, steering_dirns = path_func(x, y, dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+ flag, travel_distances, steering_dirns = path_func(-x, y, -dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ travel_distances = timeflip(travel_distances)
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+ flag, travel_distances, steering_dirns = path_func(x, -y, -dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ steering_dirns = reflect(steering_dirns)
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+ flag, travel_distances, steering_dirns = path_func(-x, -y, dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ travel_distances = timeflip(travel_distances)
+ steering_dirns = reflect(steering_dirns)
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
return paths
@@ -252,7 +354,7 @@ def calc_interpolate_dists_list(lengths, step_size):
def generate_local_course(lengths, modes, max_curvature, step_size):
- interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size)
+ interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size * max_curvature)
origin_x, origin_y, origin_yaw = 0.0, 0.0, 0.0
@@ -299,10 +401,6 @@ def interpolate(dist, length, mode, max_curvature, origin_x, origin_y,
return x, y, yaw, 1 if length > 0.0 else -1
-def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
-
-
def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
q0 = [sx, sy, syaw]
q1 = [gx, gy, gyaw]
@@ -311,7 +409,7 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
for path in paths:
xs, ys, yaws, directions = generate_local_course(path.lengths,
path.ctypes, maxc,
- step_size * maxc)
+ step_size)
# convert global coordinate
path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for
@@ -358,6 +456,9 @@ def main():
curvature,
step_size)
+ if not xs:
+ assert False, "No path"
+
if show_animation: # pragma: no cover
plt.cla()
plt.plot(xs, ys, label="final course " + str(modes))
@@ -372,9 +473,6 @@ def main():
plt.axis("equal")
plt.show()
- if not xs:
- assert False, "No path"
-
if __name__ == '__main__':
main()
diff --git a/PathPlanning/StateLatticePlanner/lookup_table.csv b/PathPlanning/StateLatticePlanner/lookup_table.csv
new file mode 100644
index 0000000000..2c48a11a18
--- /dev/null
+++ b/PathPlanning/StateLatticePlanner/lookup_table.csv
@@ -0,0 +1,82 @@
+x,y,yaw,s,km,kf
+1.0,0.0,0.0,1.0,0.0,0.0
+9.975352133559392,0.0482183513545736,0.5660837104214496,10.00000000000002,0.0015736624607596847,0.31729782170754367
+15.021899857204536,0.023109001221800096,0.541061424167431,15.128443053611093,0.0006480950273134919,0.20847475849103875
+20.062147834064536,0.0406648159648112,0.5374967866814861,20.205553097986094,0.000452860235044122,0.15502921850050788
+24.924468605496358,-0.04047324014767662,0.5146575311501209,25.16775431470035,6.940620839146646e-05,0.12259452810198132
+9.971782095506931,1.9821448683146543,0.5206511572266477,10.287478424823748,0.05861430948618236,0.07036494964262185
+15.00170010872385,2.0003864283110473,0.5236741185892617,15.245993376540827,0.02657730439557895,0.09933479864250763
+19.991716537639487,1.9940629519465154,0.5226444441451559,20.277923997037238,0.015108540596275507,0.09478988637993524
+24.946447973869596,2.0815930190993206,0.5306354765686239,25.20115925294013,0.010036251429787917,0.08505936469481987
+10.033694822745312,3.9724800521928056,0.5349578864544497,11.087694168363686,0.10279429366023954,-0.12501011715795404
+15.010712586144749,4.0046776414868095,0.5234972445048012,15.729582155835587,0.05010930398580602,-0.0008557723200034717
+19.9175798257299,4.053680042954521,0.5265397234296523,20.52466275843717,0.029584390559990882,0.035276591227371874
+24.98769016158626,3.991699950598091,0.5229000018897194,25.543297770996556,0.018800715817231053,0.04750751098144048
+10.018665105170687,6.004814533505462,0.5183921334245007,12.249438857228967,0.13207408248643182,-0.2742892277307502
+14.988626131330372,5.991226410357179,0.5248160422552801,16.53593823576699,0.06924423592936522,-0.08634227857486092
+20.014420271646646,6.006767110727591,0.5233060851224174,21.23271362632659,0.041402041787912916,-0.01770377839603589
+24.998338724667267,5.997352722087869,0.5235621854299422,26.009046544833613,0.027285850882345728,0.011507045054418165
+10.040118020822444,8.017131894913126,0.5076867575242261,13.8061261785273,0.14561700178565884,-0.3527538468214878
+14.96397914886416,7.974972375534203,0.5303378183744862,17.667338175004062,0.08318912494381935,-0.15372672981944802
+20.045725182938817,8.023486945646207,0.5201839069343577,22.126364299043573,0.05173969669894265,-0.06557547083017647
+25.004687466358227,8.00036398460779,0.5234938146870878,26.740089158520917,0.034867425244601645,-0.02199309906456302
+10.065138949829214,10.03244363616002,0.49375882493214895,15.701940360254637,0.14847998727328912,-0.39355037236614626
+15.05373212031198,10.026401491912143,0.5111826366036252,19.105461052226858,0.09205576730549936,-0.20458802229704312
+19.965550451103926,9.977668905006206,0.5278605653376056,23.14082140870299,0.06010674632014157,-0.10340577652521214
+25.04062496016141,9.956781577401756,0.5252395961316738,27.641194908523495,0.04115083532669924,-0.05054407239730677
+9.980172344087242,11.981944953180841,0.5354924711458446,17.764377273124804,0.14616069587267325,-0.40115138946106776
+15.031707905116134,12.011530784459552,0.5157261778129998,20.745000892047745,0.0970285785481706,-0.2379719980195133
+20.05384921212195,12.02621662711961,0.5153884987125119,24.513013940487117,0.06601543941341544,-0.13666530932375262
+25.04326204059146,12.019946808479768,0.5198699857547844,28.74306622689766,0.04671545692054678,-0.07827401225777673
+10.005005976167096,13.993516346269931,0.5249997047973469,20.063732124124442,0.14007166951362482,-0.39994549637994103
+15.013469777117386,13.998572375088138,0.5211760827701193,22.591299061495683,0.0989196134377572,-0.25909446951756165
+19.980150236409695,13.98233838451409,0.5278966095736082,25.971685915503254,0.07029833263412807,-0.15993299513197096
+25.009669110020404,14.000751236643762,0.5227555344229664,29.949071374991423,0.05106114063333748,-0.09952052168406796
+9.996712859814979,15.986637217372996,0.5301458018536311,22.47478825250167,0.1329741433122606,-0.38823042580907063
+15.02373126750475,16.00384009060484,0.5182833077580984,24.557463511123004,0.0989491582793761,-0.26836010532851323
+20.023756339113735,16.004847752803656,0.5197401980953318,27.669260302891157,0.07275720314277462,-0.178811991371391
+25.015003771679122,16.002919774604504,0.5219791758565742,31.36524983655211,0.05429827198598215,-0.11766440355003502
+10.078596822781892,18.025309925487992,0.49768408992179225,25.02580432036455,0.1252233187334947,-0.3747545825918585
+15.001968473293188,17.988033772017467,0.5262415135796346,26.67625473617623,0.09746306394892065,-0.27167606206451594
+20.026047062413117,18.00445752595148,0.5193568548054093,29.442158339897595,0.07417227896231118,-0.19015828496001386
+24.984711558393403,17.982744830235063,0.5266809346220684,32.855828700083094,0.05675308229799072,-0.13090299334069386
+9.999999973554228,8.906672256498078e-05,-0.00024912926939091307,9.993250237275609,1.9794429093204823e-06,-0.00016167063578544257
+14.999999988951053,0.00030609885737583985,-9.259737492950393e-05,14.939586274030715,4.066161982234725e-06,-5.3230908443270726e-05
+19.999999963637627,0.0008196433029304879,-0.00010277758318455454,19.985770355960977,6.0902800817987275e-06,-5.581407356116362e-05
+24.999999906323,0.001558015443394581,-0.0001252423879458675,24.925430653319882,7.508303551937611e-06,-5.98269885073166e-05
+9.93201732790474,1.9700581591656137,-0.006606314895513332,10.1625049701131,0.05795554613825405,-0.23594780459354886
+15.017121844754504,2.000131018972639,-0.001958259181754851,15.130494387563031,0.026367577418638183,-0.10529363184139814
+19.962697589600058,2.0003823634817484,0.0021983556339688626,20.055058569558643,0.014972854970102445,-0.0592998512022201
+24.990093248087035,2.0008914594513647,0.0003319006512292333,25.020899019312747,0.009609456446194334,-0.03808543941908436
+9.942924437331126,3.9434423219621073,-0.047789349898090805,10.916318098481405,0.10417074854184473,-0.42509733550937057
+14.976393375378994,3.9987876606083557,0.004653465622298736,15.69826778341493,0.04981535482126709,-0.20027162173052074
+19.954160472557877,4.000101578371634,0.0053292950039418585,20.459066225465484,0.02905576509783228,-0.11479451096219842
+25.06247590490118,3.9997579161047643,-0.00486183691237807,25.40723367563786,0.01874893916371208,-0.07533000027879669
+9.974854017566281,5.998183884411291,0.01394025812352817,12.27808815775426,0.13163310345287574,-0.5111693653344966
+14.99829771854157,5.999020207860274,0.0007330116466723879,16.57520987140955,0.06880393034208837,-0.27508456151767885
+19.98389776689381,5.999506195067484,0.002770060727207646,21.17690590277397,0.04131547230609369,-0.1652252863196287
+25.022089217041394,5.998166050230614,-0.002551136444779001,25.974625009044832,0.02718132258204399,-0.10978755041013998
+9.940106683734614,7.99448983539684,0.03735909486314526,13.864600506318645,0.14554135993596395,-0.5498471044599721
+15.015405965817797,7.996301502316838,-0.004430455799697253,17.779484729664652,0.08234534796805798,-0.3300198333333338
+19.965919061860355,7.998456498324741,0.00732927315681664,22.0665101267907,0.05178054118886435,-0.20507088323830897
+24.97580637673196,7.998036396987909,0.0034859866489540536,26.699711792661176,0.03478260921646504,-0.13959734880932403
+10.003237328881212,9.994037173180942,-0.002542633641336778,15.800576175296408,0.1482242831571022,-0.5606578442626601
+14.95848212484301,9.995827033229693,0.016804720248816185,19.19635868417634,0.09159937492256161,-0.3610497877526804
+20.018365340632464,9.997789133099982,-0.003880405312526758,23.259977677838524,0.05967179836565363,-0.23873172503708404
+25.034844162753302,9.996613275552045,-0.005490232481425661,27.647073656497884,0.04122997694830456,-0.16548182742762063
+10.041413516307436,11.988808245039152,-0.015743247245750158,18.0174427655263,0.14424296158815444,-0.5545987939832356
+15.0710608536628,11.993636485613393,-0.025235844052727163,20.92474299071291,0.0960774359909814,-0.38199459745149106
+20.061838597733104,11.995243972143648,-0.015325438311212025,24.63090823780847,0.06556771814265559,-0.2626353022718591
+24.90813949494271,11.995929681233529,0.01760171116909426,28.6986397040137,0.046810556161518815,-0.1847353186190147
+10.005191819464756,13.97797567430312,0.018961636911005275,20.358534835690133,0.13825179056925302,-0.5307789523538471
+14.978392340358946,13.991362718235834,0.012411272386128935,22.755419658274054,0.0984622955030996,-0.38447788120958937
+20.015767113356507,13.992558840024987,-0.002205036951612893,26.18420998778461,0.06961025144239422,-0.2786494668163888
+25.01318440442437,13.994258255793202,-0.0016239998449329995,30.09124393513656,0.05071043613803722,-0.20387658283659768
+10.038844117562423,15.966797017942504,0.016384527088525225,22.88736140380268,0.13044436631301143,-0.5070826347325453
+14.91898245890566,15.984279670640529,0.03784081306841358,24.796728185207627,0.09830913950807817,-0.38207974071854045
+19.999487117727806,15.99041117221354,0.0034823225688951354,27.881676426972927,0.07220430103629995,-0.2873083396987492
+25.056418472201756,15.995103453935709,-0.011257522827095023,31.50238694595278,0.05406499488342877,-0.21526296035737832
+10.076107447676621,17.952889979512353,0.017798231103724138,25.454959881832874,0.1231232463335769,-0.47600174850950705
+15.032725028551983,17.978015286760307,0.0020752804670070013,27.089888269358894,0.09590219542773218,-0.3801465515462427
+20.03544756240551,17.98685790169768,-0.005300968094156033,29.75070206477736,0.07340450527104486,-0.29182757725382324
+24.960019173190652,17.98909417109214,0.011594018486178026,33.0995680641525,0.05634561447882407,-0.22402297280749597
diff --git a/PathPlanning/StateLatticePlanner/lookuptable.csv b/PathPlanning/StateLatticePlanner/lookuptable.csv
deleted file mode 100644
index 85eff9b932..0000000000
--- a/PathPlanning/StateLatticePlanner/lookuptable.csv
+++ /dev/null
@@ -1,25 +0,0 @@
-x,y,yaw,s,km,kf
-1.0,0.0,0.0,1.0,0.0,0.0
-0.9734888894493215,-0.009758406565994977,0.5358080146312756,0.9922329557399788,-0.10222538550473198,3.0262632253145982
-10.980728996433243,-0.0003093605787364978,0.522622783944529,11.000391678142623,0.00010296091030877934,0.2731556687244648
-16.020309241920156,0.0001292339008200291,0.5243399938698222,16.100019813021202,0.00013263212395994706,0.18999138959173634
-20.963495745193626,-0.00033031017429944326,0.5226120033275024,21.10082901143343,0.00011687467551566884,0.14550546012583987
-6.032553475650599,2.008504211720188,0.5050517859971599,6.400329805864408,0.1520002249689879,-0.13105940607691127
-10.977487445230075,2.0078696810700034,0.5263634407901872,11.201040572298973,0.04895863722280565,0.08356555007223682
-15.994057699325753,2.025659106131227,0.5303858891065698,16.200300421483128,0.0235708657178127,0.10002225103921249
-20.977228843605943,2.0281289825388513,0.5300376140865567,21.20043308669372,0.013795675421657671,0.09331700188063087
-25.95453914157977,1.9926432818499131,0.5226203078411618,26.200880299840527,0.00888830054451281,0.0830622000626594
-0.9999999999999999,0.0,0.0,1.0,0.0,0.0
-5.999999999670752,5.231312388722274e-05,1.4636120911014667e-05,5.996117185283419,4.483756968024291e-06,-3.4422519205046243e-06
-10.999749470720566,-0.011978787043239986,0.022694802592583763,10.99783855994015,-0.00024209503125174496,0.01370491008661795
-15.999885224357776,-0.018937230084507616,0.011565580878015763,15.99839381389597,-0.0002086399372890716,0.005267247862667496
-20.999882688881286,-0.030304200126461317,0.009117836885732596,20.99783120184498,-0.00020013159571184735,0.0034529188783636866
-25.999911270030413,-0.025754431694664327,0.0074809531598503615,25.99674782258235,-0.0001111138115390646,0.0021946603965658368
-10.952178818975062,1.993067260835455,0.0045572480669707136,11.17961498195845,0.04836813285436623,-0.19328716251760758
-16.028306009258,2.0086286208315407,0.009306594796759554,16.122411866381054,0.02330689045417979,-0.08877002085985948
-20.971603115419715,1.9864158336174966,0.007016819403167119,21.093006725076872,0.013439123130720928,-0.05238318300611623
-25.997019676818372,1.9818581321430093,0.007020172975955249,26.074021794586585,0.00876496148602802,-0.03362579291686346
-16.017903482982604,4.009490840390534,-5.293114796312698e-05,16.600937712976638,0.044543450568614244,-0.17444651314466567
-20.98845988414163,3.956600199823583,-0.01050744134070728,21.40149119463485,0.02622674388276059,-0.10625681152519345
-25.979448381017914,3.9968223055054977,-0.00012819253386682928,26.30504721211744,0.017467093413146118,-0.06914750106424669
-25.96268055563514,5.9821266846168,4.931311239565056e-05,26.801388563459287,0.025426008913226557,-0.10047663078001536
diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
index b5fec697a4..554cd0f3b7 100644
--- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py
+++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
@@ -4,11 +4,16 @@
author: Atsushi Sakai (@Atsushi_twi)
-- lookuptable.csv is generated with this script: https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py
+- plookuptable.csv is generated with this script:
+https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning
+/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py
-Ref:
+Reference:
-- State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.187.8210&rep=rep1&type=pdf
+- State Space Sampling of Feasible Motions for High-Performance Mobile Robot
+Navigation in Complex Environments
+https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf
+&doi=e2256b5b24137f89e473f01df288cb3aa72e56a0
"""
import sys
@@ -16,33 +21,27 @@
from matplotlib import pyplot as plt
import numpy as np
import math
-import pandas as pd
+import pathlib
-sys.path.append(os.path.dirname(os.path.abspath(__file__))
- + "/../ModelPredictiveTrajectoryGenerator/")
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+import ModelPredictiveTrajectoryGenerator.trajectory_generator as planner
+import ModelPredictiveTrajectoryGenerator.motion_model as motion_model
-try:
- import model_predictive_trajectory_generator as planner
- import motion_model
-except ImportError:
- raise
-
-
-table_path = os.path.dirname(os.path.abspath(__file__)) + "/lookuptable.csv"
+TABLE_PATH = os.path.dirname(os.path.abspath(__file__)) + "/lookup_table.csv"
show_animation = True
-def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table):
+def search_nearest_one_from_lookup_table(t_x, t_y, t_yaw, lookup_table):
mind = float("inf")
minid = -1
for (i, table) in enumerate(lookup_table):
-
- dx = tx - table[0]
- dy = ty - table[1]
- dyaw = tyaw - table[2]
+ dx = t_x - table[0]
+ dy = t_y - table[1]
+ dyaw = t_yaw - table[2]
d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
if d <= mind:
minid = i
@@ -51,31 +50,29 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table):
return lookup_table[minid]
-def get_lookup_table():
- data = pd.read_csv(table_path)
-
- return np.array(data)
+def get_lookup_table(table_path):
+ return np.loadtxt(table_path, delimiter=',', skiprows=1)
def generate_path(target_states, k0):
# x, y, yaw, s, km, kf
- lookup_table = get_lookup_table()
+ lookup_table = get_lookup_table(TABLE_PATH)
result = []
for state in target_states:
- bestp = search_nearest_one_from_lookuptable(
+ bestp = search_nearest_one_from_lookup_table(
state[0], state[1], state[2], lookup_table)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.array(
- [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1)
+ [np.hypot(state[0], state[1]), bestp[4], bestp[5]]).reshape(3, 1)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
result.append(
- [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
+ [x[-1], y[-1], yaw[-1], float(p[0, 0]), float(p[1, 0]), float(p[2, 0])])
print("finish path generation")
return result
@@ -83,18 +80,28 @@ def generate_path(target_states, k0):
def calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max):
"""
- calc uniform state
- :param nxy: number of position sampling
- :param nh: number of heading sampleing
- :param d: distance of terminal state
- :param a_min: position sampling min angle
- :param a_max: position sampling max angle
- :param p_min: heading sampling min angle
- :param p_max: heading sampling max angle
- :return: states list
- """
+ Parameters
+ ----------
+ nxy :
+ number of position sampling
+ nh :
+ number of heading sampleing
+ d :
+ distance of terminal state
+ a_min :
+ position sampling min angle
+ a_max :
+ position sampling max angle
+ p_min :
+ heading sampling min angle
+ p_max :
+ heading sampling max angle
+
+ Returns
+ -------
+ """
angle_samples = [i / (nxy - 1) for i in range(nxy)]
states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh)
@@ -313,11 +320,11 @@ def lane_state_sampling_test1():
plt.close("all")
for table in result:
- xc, yc, yawc = motion_model.generate_trajectory(
+ x_c, y_c, yaw_c = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
if show_animation:
- plt.plot(xc, yc, "-r")
+ plt.plot(x_c, y_c, "-r")
if show_animation:
plt.grid(True)
diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py
new file mode 100644
index 0000000000..745cde45fb
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py
@@ -0,0 +1,49 @@
+from abc import ABC, abstractmethod
+from dataclasses import dataclass
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath
+import random
+import numpy.random as numpy_random
+
+# Seed randomness for reproducibility
+RANDOM_SEED = 50
+random.seed(RANDOM_SEED)
+numpy_random.seed(RANDOM_SEED)
+
+class SingleAgentPlanner(ABC):
+ """
+ Base class for single agent planners
+ """
+
+ @staticmethod
+ @abstractmethod
+ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
+ pass
+
+@dataclass
+class StartAndGoal:
+ # Index of this agent
+ index: int
+ # Start position of the robot
+ start: Position
+ # Goal position of the robot
+ goal: Position
+
+ def distance_start_to_goal(self) -> float:
+ return pow(self.goal.x - self.start.x, 2) + pow(self.goal.y - self.start.y, 2)
+
+class MultiAgentPlanner(ABC):
+ """
+ Base class for multi-agent planners
+ """
+
+ @staticmethod
+ @abstractmethod
+ def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]:
+ """
+ Plan for all agents. Returned paths are in order corresponding to the returned list of `StartAndGoal` objects
+ """
+ pass
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
new file mode 100644
index 0000000000..ccc2989001
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
@@ -0,0 +1,370 @@
+"""
+This file implements a grid with a 3d reservation matrix with dimensions for x, y, and time. There
+is also infrastructure to generate dynamic obstacles that move around the grid. The obstacles' paths
+are stored in the reservation matrix on creation.
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+from enum import Enum
+from dataclasses import dataclass
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position
+
+@dataclass
+class Interval:
+ start_time: int
+ end_time: int
+
+class ObstacleArrangement(Enum):
+ # Random obstacle positions and movements
+ RANDOM = 0
+ # Obstacles start in a line in y at center of grid and move side-to-side in x
+ ARRANGEMENT1 = 1
+ # Static obstacle arrangement
+ NARROW_CORRIDOR = 2
+
+"""
+Generates a 2d numpy array with lists for elements.
+"""
+def empty_2d_array_of_lists(x: int, y: int) -> np.ndarray:
+ arr = np.empty((x, y), dtype=object)
+ # assign each element individually - np.full creates references to the same list
+ arr[:] = [[[] for _ in range(y)] for _ in range(x)]
+ return arr
+
+class Grid:
+ # Set in constructor
+ grid_size: np.ndarray
+ reservation_matrix: np.ndarray
+ obstacle_paths: list[list[Position]] = []
+ # Obstacles will never occupy these points. Useful to avoid impossible scenarios
+ obstacle_avoid_points: list[Position] = []
+
+ # Number of time steps in the simulation
+ time_limit: int
+
+ # Logging control
+ verbose = False
+
+ def __init__(
+ self,
+ grid_size: np.ndarray,
+ num_obstacles: int = 40,
+ obstacle_avoid_points: list[Position] = [],
+ obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM,
+ time_limit: int = 100,
+ ):
+ self.obstacle_avoid_points = obstacle_avoid_points
+ self.time_limit = time_limit
+ self.grid_size = grid_size
+ self.reservation_matrix = np.zeros((grid_size[0], grid_size[1], self.time_limit))
+
+ if num_obstacles > self.grid_size[0] * self.grid_size[1]:
+ raise Exception("Number of obstacles is greater than grid size!")
+
+ if obstacle_arrangement == ObstacleArrangement.RANDOM:
+ self.obstacle_paths = self.generate_dynamic_obstacles(num_obstacles)
+ elif obstacle_arrangement == ObstacleArrangement.ARRANGEMENT1:
+ self.obstacle_paths = self.obstacle_arrangement_1(num_obstacles)
+ elif obstacle_arrangement == ObstacleArrangement.NARROW_CORRIDOR:
+ self.obstacle_paths = self.generate_narrow_corridor_obstacles(num_obstacles)
+
+ for i, path in enumerate(self.obstacle_paths):
+ obs_idx = i + 1 # avoid using 0 - that indicates free space in the grid
+ for t, position in enumerate(path):
+ # Reserve old & new position at this time step
+ if t > 0:
+ self.reservation_matrix[path[t - 1].x, path[t - 1].y, t] = obs_idx
+ self.reservation_matrix[position.x, position.y, t] = obs_idx
+
+ """
+ Generate dynamic obstacles that move around the grid. Initial positions and movements are random
+ """
+ def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
+ obstacle_paths = []
+ for _ in range(0, obs_count):
+ # Sample until a free starting space is found
+ initial_position = self.sample_random_position()
+ while not self.valid_obstacle_position(initial_position, 0):
+ initial_position = self.sample_random_position()
+
+ positions = [initial_position]
+ if self.verbose:
+ print("Obstacle initial position: ", initial_position)
+
+ # Encourage obstacles to mostly stay in place - too much movement leads to chaotic planning scenarios
+ # that are not fun to watch
+ weights = [0.05, 0.05, 0.05, 0.05, 0.8]
+ diffs = [
+ Position(0, 1),
+ Position(0, -1),
+ Position(1, 0),
+ Position(-1, 0),
+ Position(0, 0),
+ ]
+
+ for t in range(1, self.time_limit - 1):
+ sampled_indices = np.random.choice(
+ len(diffs), size=5, replace=False, p=weights
+ )
+ rand_diffs = [diffs[i] for i in sampled_indices]
+
+ valid_position = None
+ for diff in rand_diffs:
+ new_position = positions[-1] + diff
+
+ if not self.valid_obstacle_position(new_position, t):
+ continue
+
+ valid_position = new_position
+ break
+
+ # Impossible situation for obstacle - stay in place
+ # -> this can happen if the oaths of other obstacles this one
+ if valid_position is None:
+ valid_position = positions[-1]
+
+ positions.append(valid_position)
+
+ obstacle_paths.append(positions)
+
+ return obstacle_paths
+
+ """
+ Generate a line of obstacles in y at the center of the grid that move side-to-side in x
+ Bottom half start moving right, top half start moving left. If `obs_count` is less than the length of
+ the grid, only the first `obs_count` obstacles will be generated.
+ """
+ def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]:
+ obstacle_paths = []
+ half_grid_x = self.grid_size[0] // 2
+ half_grid_y = self.grid_size[1] // 2
+
+ for y_idx in range(0, min(obs_count, self.grid_size[1])):
+ moving_right = y_idx < half_grid_y
+ position = Position(half_grid_x, y_idx)
+ path = [position]
+
+ for t in range(1, self.time_limit - 1):
+ # sit in place every other time step
+ if t % 2 == 0:
+ path.append(position)
+ continue
+
+ # first check if we should switch direction (at edge of grid)
+ if (moving_right and position.x == self.grid_size[0] - 1) or (
+ not moving_right and position.x == 0
+ ):
+ moving_right = not moving_right
+ # step in direction
+ position = Position(
+ position.x + (1 if moving_right else -1), position.y
+ )
+ path.append(position)
+
+ obstacle_paths.append(path)
+
+ return obstacle_paths
+
+ def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Position]]:
+ obstacle_paths = []
+
+ for y in range(0, self.grid_size[1]):
+ if y > obs_count:
+ break
+
+ if y == self.grid_size[1] // 2:
+ # Skip the middle row
+ continue
+
+ obstacle_path = []
+ x = self.grid_size[0] // 2 # middle of the grid
+ for t in range(0, self.time_limit - 1):
+ obstacle_path.append(Position(x, y))
+
+ obstacle_paths.append(obstacle_path)
+
+ return obstacle_paths
+
+ """
+ Check if the given position is valid at time t
+
+ input:
+ position (Position): (x, y) position
+ t (int): time step
+
+ output:
+ bool: True if position/time combination is valid, False otherwise
+ """
+ def valid_position(self, position: Position, t: int) -> bool:
+ # Check if position is in grid
+ if not self.inside_grid_bounds(position):
+ return False
+
+ # Check if position is not occupied at time t
+ return self.reservation_matrix[position.x, position.y, t] == 0
+
+ """
+ Returns True if the given position is valid at time t and is not in the set of obstacle_avoid_points
+ """
+ def valid_obstacle_position(self, position: Position, t: int) -> bool:
+ return (
+ self.valid_position(position, t)
+ and position not in self.obstacle_avoid_points
+ )
+
+ """
+ Returns True if the given position is within the grid's boundaries
+ """
+ def inside_grid_bounds(self, position: Position) -> bool:
+ return (
+ position.x >= 0
+ and position.x < self.grid_size[0]
+ and position.y >= 0
+ and position.y < self.grid_size[1]
+ )
+
+ """
+ Sample a random position that is within the grid's boundaries
+
+ output:
+ Position: (x, y) position
+ """
+ def sample_random_position(self) -> Position:
+ return Position(
+ np.random.randint(0, self.grid_size[0]),
+ np.random.randint(0, self.grid_size[1]),
+ )
+
+ """
+ Returns a tuple of (x_positions, y_positions) of the obstacles at time t
+ """
+ def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]:
+ x_positions = []
+ y_positions = []
+ for obs_path in self.obstacle_paths:
+ x_positions.append(obs_path[t].x)
+ y_positions.append(obs_path[t].y)
+ return (x_positions, y_positions)
+
+ """
+ Returns safe intervals for each cell.
+ """
+ def get_safe_intervals(self) -> np.ndarray:
+ intervals = empty_2d_array_of_lists(self.grid_size[0], self.grid_size[1])
+ for x in range(intervals.shape[0]):
+ for y in range(intervals.shape[1]):
+ intervals[x, y] = self.get_safe_intervals_at_cell(Position(x, y))
+
+ return intervals
+
+ """
+ Generate the safe intervals for a given cell. The intervals will be in order of start time.
+ ex: Interval (2, 3) will be before Interval (4, 5)
+ """
+ def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]:
+ vals = self.reservation_matrix[cell.x, cell.y, :]
+ # Find where the array is zero
+ zero_mask = (vals == 0)
+
+ # Identify transitions between zero and nonzero elements
+ diff = np.diff(zero_mask.astype(int))
+
+ # Start indices: where zeros begin (1 after a nonzero)
+ start_indices = np.where(diff == 1)[0] + 1
+
+ # End indices: where zeros stop (just before a nonzero)
+ end_indices = np.where(diff == -1)[0]
+
+ # Handle edge cases if the array starts or ends with zeros
+ if zero_mask[0]: # If the first element is zero, add index 0 to start_indices
+ start_indices = np.insert(start_indices, 0, 0)
+ if zero_mask[-1]: # If the last element is zero, add the last index to end_indices
+ end_indices = np.append(end_indices, len(vals) - 1)
+
+ # Create pairs of (first zero, last zero)
+ intervals = [Interval(int(start), int(end)) for start, end in zip(start_indices, end_indices)]
+
+ # Remove intervals where a cell is only free for one time step. Those intervals not provide enough time to
+ # move into and out of the cell each take 1 time step, and the cell is considered occupied during
+ # both the time step when it is entering the cell, and the time step when it is leaving the cell.
+ intervals = [interval for interval in intervals if interval.start_time != interval.end_time]
+ return intervals
+
+ """
+ Reserve an agent's path in the grid. Raises an exception if the agent's index is 0, or if a position is
+ already reserved by a different agent.
+ """
+ def reserve_path(self, node_path: NodePath, agent_index: int):
+ if agent_index == 0:
+ raise Exception("Agent index cannot be 0")
+
+ for i, node in enumerate(node_path.path):
+ reservation_finish_time = node.time + 1
+ if i < len(node_path.path) - 1:
+ reservation_finish_time = node_path.path[i + 1].time
+
+ self.reserve_position(node.position, agent_index, Interval(node.time, reservation_finish_time))
+
+ """
+ Reserve a position for the provided agent during the provided time interval.
+ Raises an exception if the agent's index is 0, or if the position is already reserved by a different agent during the interval.
+ """
+ def reserve_position(self, position: Position, agent_index: int, interval: Interval):
+ if agent_index == 0:
+ raise Exception("Agent index cannot be 0")
+
+ for t in range(interval.start_time, interval.end_time + 1):
+ current_reserver = self.reservation_matrix[position.x, position.y, t]
+ if current_reserver not in [0, agent_index]:
+ raise Exception(
+ f"Agent {agent_index} tried to reserve a position already reserved by another agent: {position} at time {t}, reserved by {current_reserver}"
+ )
+ self.reservation_matrix[position.x, position.y, t] = agent_index
+
+ """
+ Clears the initial reservation for an agent by clearing reservations at its start position with its index for
+ from time 0 to the time limit.
+ """
+ def clear_initial_reservation(self, position: Position, agent_index: int):
+ for t in range(self.time_limit):
+ if self.reservation_matrix[position.x, position.y, t] == agent_index:
+ self.reservation_matrix[position.x, position.y, t] = 0
+
+show_animation = True
+
+def main():
+ grid = Grid(
+ np.array([11, 11]),
+ num_obstacles=10,
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ if not show_animation:
+ return
+
+ fig = plt.figure(figsize=(8, 7))
+ ax = fig.add_subplot(
+ autoscale_on=False,
+ xlim=(0, grid.grid_size[0] - 1),
+ ylim=(0, grid.grid_size[1] - 1),
+ )
+ ax.set_aspect("equal")
+ ax.grid()
+ ax.set_xticks(np.arange(0, 11, 1))
+ ax.set_yticks(np.arange(0, 11, 1))
+ (obs_points,) = ax.plot([], [], "ro", ms=15)
+
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect(
+ "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+ )
+
+ for i in range(0, grid.time_limit - 1):
+ obs_positions = grid.get_obstacle_positions_at_time(i)
+ obs_points.set_data(obs_positions[0], obs_positions[1])
+ plt.pause(0.2)
+ plt.show()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/PathPlanning/TimeBasedPathPlanning/Node.py b/PathPlanning/TimeBasedPathPlanning/Node.py
new file mode 100644
index 0000000000..728eebb676
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/Node.py
@@ -0,0 +1,99 @@
+from dataclasses import dataclass
+from functools import total_ordering
+import numpy as np
+from typing import Sequence
+
+@dataclass(order=True)
+class Position:
+ x: int
+ y: int
+
+ def as_ndarray(self) -> np.ndarray:
+ return np.array([self.x, self.y])
+
+ def __add__(self, other):
+ if isinstance(other, Position):
+ return Position(self.x + other.x, self.y + other.y)
+ raise NotImplementedError(
+ f"Addition not supported for Position and {type(other)}"
+ )
+
+ def __sub__(self, other):
+ if isinstance(other, Position):
+ return Position(self.x - other.x, self.y - other.y)
+ raise NotImplementedError(
+ f"Subtraction not supported for Position and {type(other)}"
+ )
+
+ def __hash__(self):
+ return hash((self.x, self.y))
+
+@dataclass()
+# Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because
+# this class needs to override the __lt__ and __eq__ methods to ignore parent_index. Parent
+# index is just used to track the path found by the algorithm, and has no effect on the quality
+# of a node.
+@total_ordering
+class Node:
+ position: Position
+ time: int
+ heuristic: int
+ parent_index: int
+
+ """
+ This is what is used to drive node expansion. The node with the lowest value is expanded next.
+ This comparison prioritizes the node with the lowest cost-to-come (self.time) + cost-to-go (self.heuristic)
+ """
+ def __lt__(self, other: object):
+ if not isinstance(other, Node):
+ return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}")
+ return (self.time + self.heuristic) < (other.time + other.heuristic)
+
+ """
+ Note: cost and heuristic are not included in eq or hash, since they will always be the same
+ for a given (position, time) pair. Including either cost or heuristic would be redundant.
+ """
+ def __eq__(self, other: object):
+ if not isinstance(other, Node):
+ return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}")
+ return self.position == other.position and self.time == other.time
+
+ def __hash__(self):
+ return hash((self.position, self.time))
+
+class NodePath:
+ path: Sequence[Node]
+ positions_at_time: dict[int, Position]
+ # Number of nodes expanded while finding this path
+ expanded_node_count: int
+
+ def __init__(self, path: Sequence[Node], expanded_node_count: int):
+ self.path = path
+ self.expanded_node_count = expanded_node_count
+
+ self.positions_at_time = {}
+ for i, node in enumerate(path):
+ reservation_finish_time = node.time + 1
+ if i < len(path) - 1:
+ reservation_finish_time = path[i + 1].time
+
+ for t in range(node.time, reservation_finish_time):
+ self.positions_at_time[t] = node.position
+
+ """
+ Get the position of the path at a given time
+ """
+ def get_position(self, time: int) -> Position | None:
+ return self.positions_at_time.get(time)
+
+ """
+ Time stamp of the last node in the path
+ """
+ def goal_reached_time(self) -> int:
+ return self.path[-1].time
+
+ def __repr__(self):
+ repr_string = ""
+ for i, node in enumerate(self.path):
+ repr_string += f"{i}: {node}\n"
+ return repr_string
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py
new file mode 100644
index 0000000000..7cd1f615d8
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py
@@ -0,0 +1,135 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.backend_bases import KeyEvent
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath
+
+'''
+Plot a single agent path.
+'''
+def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath):
+ fig = plt.figure(figsize=(10, 7))
+ ax = fig.add_subplot(
+ autoscale_on=False,
+ xlim=(0, grid.grid_size[0] - 1),
+ ylim=(0, grid.grid_size[1] - 1),
+ )
+ ax.set_aspect("equal")
+ ax.grid()
+ ax.set_xticks(np.arange(0, grid.grid_size[0], 1))
+ ax.set_yticks(np.arange(0, grid.grid_size[1], 1))
+
+ (start_and_goal,) = ax.plot([], [], "mD", ms=15, label="Start and Goal")
+ start_and_goal.set_data([start.x, goal.x], [start.y, goal.y])
+ (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles")
+ (path_points,) = ax.plot([], [], "bo", ms=10, label="Path Found")
+ ax.legend(bbox_to_anchor=(1.05, 1))
+
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect(
+ "key_release_event",
+ lambda event: [exit(0) if event.key == "escape" else None]
+ if isinstance(event, KeyEvent) else None
+ )
+
+ for i in range(0, path.goal_reached_time()):
+ obs_positions = grid.get_obstacle_positions_at_time(i)
+ obs_points.set_data(obs_positions[0], obs_positions[1])
+ path_position = path.get_position(i)
+ if not path_position:
+ raise Exception(f"Path position not found for time {i}.")
+
+ path_points.set_data([path_position.x], [path_position.y])
+ plt.pause(0.2)
+ plt.show()
+
+'''
+Plot a series of agent paths.
+'''
+def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath]):
+ fig = plt.figure(figsize=(10, 7))
+
+ ax = fig.add_subplot(
+ autoscale_on=False,
+ xlim=(0, grid.grid_size[0] - 1),
+ ylim=(0, grid.grid_size[1] - 1),
+ )
+ ax.set_aspect("equal")
+ ax.grid()
+ ax.set_xticks(np.arange(0, grid.grid_size[0], 1))
+ ax.set_yticks(np.arange(0, grid.grid_size[1], 1))
+
+ # Plot start and goal positions for each agent
+ colors = [] # generated randomly in loop
+ markers = ['D', 's', '^', 'o', 'p'] # Different markers for visual distinction
+
+ # Create plots for start and goal positions
+ start_and_goal_plots = []
+ for i, path in enumerate(paths):
+ marker_idx = i % len(markers)
+ agent_id = start_and_goals[i].index
+ start = start_and_goals[i].start
+ goal = start_and_goals[i].goal
+
+ color = np.random.rand(3,)
+ colors.append(color)
+ sg_plot, = ax.plot([], [], markers[marker_idx], c=color, ms=15,
+ label=f"Agent {agent_id} Start/Goal")
+ sg_plot.set_data([start.x, goal.x], [start.y, goal.y])
+ start_and_goal_plots.append(sg_plot)
+
+ # Plot for obstacles
+ (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles")
+
+ # Create plots for each agent's path
+ path_plots = []
+ for i, path in enumerate(paths):
+ agent_id = start_and_goals[i].index
+ path_plot, = ax.plot([], [], "o", c=colors[i], ms=10,
+ label=f"Agent {agent_id} Path")
+ path_plots.append(path_plot)
+
+ ax.legend(bbox_to_anchor=(1.05, 1))
+
+ # For stopping simulation with the esc key
+ plt.gcf().canvas.mpl_connect(
+ "key_release_event",
+ lambda event: [exit(0) if event.key == "escape" else None]
+ if isinstance(event, KeyEvent) else None
+ )
+
+ # Find the maximum time across all paths
+ max_time = max(path.goal_reached_time() for path in paths)
+
+ # Animation loop
+ for i in range(0, max_time + 1):
+ # Update obstacle positions
+ obs_positions = grid.get_obstacle_positions_at_time(i)
+ obs_points.set_data(obs_positions[0], obs_positions[1])
+
+ # Update each agent's position
+ for (j, path) in enumerate(paths):
+ path_positions = []
+ if i <= path.goal_reached_time():
+ res = path.get_position(i)
+ if not res:
+ print(path)
+ print(i)
+ path_position = path.get_position(i)
+ if not path_position:
+ raise Exception(f"Path position not found for time {i}.")
+
+ # Verify position is valid
+ assert not path_position in obs_positions
+ assert not path_position in path_positions
+ path_positions.append(path_position)
+
+ path_plots[j].set_data([path_position.x], [path_position.y])
+
+ plt.pause(0.2)
+
+ plt.show()
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py
new file mode 100644
index 0000000000..2573965cf6
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py
@@ -0,0 +1,95 @@
+"""
+Priority Based Planner for multi agent path planning.
+The planner generates an order to plan in, and generates plans for the robots in that order. Each planned
+path is reserved in the grid, and all future plans must avoid that path.
+
+Algorithm outlined in section III of this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf
+"""
+
+import numpy as np
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Interval,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import MultiAgentPlanner, StartAndGoal
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths
+import time
+
+class PriorityBasedPlanner(MultiAgentPlanner):
+
+ @staticmethod
+ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]:
+ """
+ Generate a path from the start to the goal for each agent in the `start_and_goals` list.
+ Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans
+ corresponds to the order of the `start_and_goals` list.
+ """
+ print(f"Using single-agent planner: {single_agent_planner_class}")
+
+ # Reserve initial positions
+ for start_and_goal in start_and_goals:
+ grid.reserve_position(start_and_goal.start, start_and_goal.index, Interval(0, 10))
+
+ # Plan in descending order of distance from start to goal
+ start_and_goals = sorted(start_and_goals,
+ key=lambda item: item.distance_start_to_goal(),
+ reverse=True)
+
+ paths = []
+ for start_and_goal in start_and_goals:
+ if verbose:
+ print(f"\nPlanning for agent: {start_and_goal}" )
+
+ grid.clear_initial_reservation(start_and_goal.start, start_and_goal.index)
+ path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, verbose)
+
+ if path is None:
+ print(f"Failed to find path for {start_and_goal}")
+ return []
+
+ agent_index = start_and_goal.index
+ grid.reserve_path(path, agent_index)
+ paths.append(path)
+
+ return (start_and_goals, paths)
+
+verbose = False
+show_animation = True
+def main():
+ grid_side_length = 21
+
+ start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+ obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=250,
+ obstacle_avoid_points=obstacle_avoid_points,
+ # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ # obstacle_arrangement=ObstacleArrangement.RANDOM,
+ )
+
+ start_time = time.time()
+ start_and_goals, paths = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose)
+
+ runtime = time.time() - start_time
+ print(f"\nPlanning took: {runtime:.5f} seconds")
+
+ if verbose:
+ print(f"Paths:")
+ for path in paths:
+ print(f"{path}\n")
+
+ if not show_animation:
+ return
+
+ PlotNodePaths(grid, start_and_goals, paths)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
new file mode 100644
index 0000000000..446847ac6d
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
@@ -0,0 +1,212 @@
+"""
+Safe interval path planner
+ This script implements a safe-interval path planner for a 2d grid with dynamic obstacles. It is faster than
+ SpaceTime A* because it reduces the number of redundant node expansions by pre-computing regions of adjacent
+ time steps that are safe ("safe intervals") at each position. This allows the algorithm to skip expanding nodes
+ that are in intervals that have already been visited earlier.
+
+ Reference: https://www.cs.cmu.edu/~maxim/files/sipp_icra11.pdf
+"""
+
+import numpy as np
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Interval,
+ ObstacleArrangement,
+ Position,
+ empty_2d_array_of_lists,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath
+
+import heapq
+from dataclasses import dataclass
+from functools import total_ordering
+import time
+
+@dataclass()
+# Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because
+# this class needs to override the __lt__ and __eq__ methods to ignore parent_index. The Parent
+# index and interval member variables are just used to track the path found by the algorithm,
+# and has no effect on the quality of a node.
+@total_ordering
+class SIPPNode(Node):
+ interval: Interval
+
+@dataclass
+class EntryTimeAndInterval:
+ entry_time: int
+ interval: Interval
+
+class SafeIntervalPathPlanner(SingleAgentPlanner):
+
+ """
+ Generate a plan given the loaded problem statement. Raises an exception if it fails to find a path.
+ Arguments:
+ verbose (bool): set to True to print debug information
+ """
+ @staticmethod
+ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
+
+ safe_intervals = grid.get_safe_intervals()
+
+ open_set: list[SIPPNode] = []
+ first_node_interval = safe_intervals[start.x, start.y][0]
+ heapq.heappush(
+ open_set, SIPPNode(start, 0, SafeIntervalPathPlanner.calculate_heuristic(start, goal), -1, first_node_interval)
+ )
+
+ expanded_list: list[SIPPNode] = []
+ visited_intervals = empty_2d_array_of_lists(grid.grid_size[0], grid.grid_size[1])
+ while open_set:
+ expanded_node: SIPPNode = heapq.heappop(open_set)
+ if verbose:
+ print("Expanded node:", expanded_node)
+
+ if expanded_node.time + 1 >= grid.time_limit:
+ if verbose:
+ print(f"\tSkipping node that is past time limit: {expanded_node}")
+ continue
+
+ if expanded_node.position == goal:
+ if verbose:
+ print(f"Found path to goal after {len(expanded_list)} expansions")
+
+ path = []
+ path_walker: SIPPNode = expanded_node
+ while True:
+ path.append(path_walker)
+ if path_walker.parent_index == -1:
+ break
+ path_walker = expanded_list[path_walker.parent_index]
+
+ # reverse path so it goes start -> goal
+ path.reverse()
+ return NodePath(path, len(expanded_list))
+
+ expanded_idx = len(expanded_list)
+ expanded_list.append(expanded_node)
+ entry_time_and_node = EntryTimeAndInterval(expanded_node.time, expanded_node.interval)
+ add_entry_to_visited_intervals_array(entry_time_and_node, visited_intervals, expanded_node)
+
+ for child in SafeIntervalPathPlanner.generate_successors(grid, goal, expanded_node, expanded_idx, safe_intervals, visited_intervals):
+ heapq.heappush(open_set, child)
+
+ raise Exception("No path found")
+
+ """
+ Generate list of possible successors of the provided `parent_node` that are worth expanding
+ """
+ @staticmethod
+ def generate_successors(
+ grid: Grid, goal: Position, parent_node: SIPPNode, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray
+ ) -> list[SIPPNode]:
+ new_nodes = []
+ diffs = [
+ Position(0, 0),
+ Position(1, 0),
+ Position(-1, 0),
+ Position(0, 1),
+ Position(0, -1),
+ ]
+ for diff in diffs:
+ new_pos = parent_node.position + diff
+ if not grid.inside_grid_bounds(new_pos):
+ continue
+
+ current_interval = parent_node.interval
+
+ new_cell_intervals: list[Interval] = intervals[new_pos.x, new_pos.y]
+ for interval in new_cell_intervals:
+ # if interval starts after current ends, break
+ # assumption: intervals are sorted by start time, so all future intervals will hit this condition as well
+ if interval.start_time > current_interval.end_time:
+ break
+
+ # if interval ends before current starts, skip
+ if interval.end_time < current_interval.start_time:
+ continue
+
+ # if we have already expanded a node in this interval with a <= starting time, skip
+ better_node_expanded = False
+ for visited in visited_intervals[new_pos.x, new_pos.y]:
+ if interval == visited.interval and visited.entry_time <= parent_node.time + 1:
+ better_node_expanded = True
+ break
+ if better_node_expanded:
+ continue
+
+ # We know there is a node worth expanding. Generate successor at the earliest possible time the
+ # new interval can be entered
+ for possible_t in range(max(parent_node.time + 1, interval.start_time), min(current_interval.end_time, interval.end_time)):
+ if grid.valid_position(new_pos, possible_t):
+ new_nodes.append(SIPPNode(
+ new_pos,
+ # entry is max of interval start and parent node time + 1 (get there as soon as possible)
+ max(interval.start_time, parent_node.time + 1),
+ SafeIntervalPathPlanner.calculate_heuristic(new_pos, goal),
+ parent_node_idx,
+ interval,
+ ))
+ # break because all t's after this will make nodes with a higher cost, the same heuristic, and are in the same interval
+ break
+
+ return new_nodes
+
+ """
+ Calculate the heuristic for a given position - Manhattan distance to the goal
+ """
+ @staticmethod
+ def calculate_heuristic(position: Position, goal: Position) -> int:
+ diff = goal - position
+ return abs(diff.x) + abs(diff.y)
+
+
+"""
+Adds a new entry to the visited intervals array. If the entry is already present, the entry time is updated if the new
+entry time is better. Otherwise, the entry is added to `visited_intervals` at the position of `expanded_node`.
+"""
+def add_entry_to_visited_intervals_array(entry_time_and_interval: EntryTimeAndInterval, visited_intervals: np.ndarray, expanded_node: SIPPNode):
+ # if entry is present, update entry time if better
+ for existing_entry_and_interval in visited_intervals[expanded_node.position.x, expanded_node.position.y]:
+ if existing_entry_and_interval.interval == entry_time_and_interval.interval:
+ existing_entry_and_interval.entry_time = min(existing_entry_and_interval.entry_time, entry_time_and_interval.entry_time)
+
+ # Otherwise, append
+ visited_intervals[expanded_node.position.x, expanded_node.position.y].append(entry_time_and_interval)
+
+
+show_animation = True
+verbose = False
+
+def main():
+ start = Position(1, 18)
+ goal = Position(19, 19)
+ grid_side_length = 21
+
+ start_time = time.time()
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=250,
+ obstacle_avoid_points=[start, goal],
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ # obstacle_arrangement=ObstacleArrangement.RANDOM,
+ )
+
+ path = SafeIntervalPathPlanner.plan(grid, start, goal, verbose)
+ runtime = time.time() - start_time
+ print(f"Planning took: {runtime:.5f} seconds")
+
+ if verbose:
+ print(f"Path: {path}")
+
+ if not show_animation:
+ return
+
+ PlotNodePath(grid, start, goal, path)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
new file mode 100644
index 0000000000..b85569f5dc
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
@@ -0,0 +1,139 @@
+"""
+Space-time A* Algorithm
+ This script demonstrates the Space-time A* algorithm for path planning in a grid world with moving obstacles.
+ This algorithm is different from normal 2D A* in one key way - the cost (often notated as g(n)) is
+ the number of time steps it took to get to a given node, instead of the number of cells it has
+ traversed. This ensures the path is time-optimal, while respecting any dynamic obstacles in the environment.
+
+ Reference: https://www.davidsilver.uk/wp-content/uploads/2020/03/coop-path-AIWisdom.pdf
+"""
+
+import numpy as np
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath
+import heapq
+from collections.abc import Generator
+import time
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath
+
+class SpaceTimeAStar(SingleAgentPlanner):
+
+ @staticmethod
+ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
+ open_set: list[Node] = []
+ heapq.heappush(
+ open_set, Node(start, 0, SpaceTimeAStar.calculate_heuristic(start, goal), -1)
+ )
+
+ expanded_list: list[Node] = []
+ expanded_set: set[Node] = set()
+ while open_set:
+ expanded_node: Node = heapq.heappop(open_set)
+ if verbose:
+ print("Expanded node:", expanded_node)
+
+ if expanded_node.time + 1 >= grid.time_limit:
+ if verbose:
+ print(f"\tSkipping node that is past time limit: {expanded_node}")
+ continue
+
+ if expanded_node.position == goal:
+ if verbose:
+ print(f"Found path to goal after {len(expanded_list)} expansions")
+
+ path = []
+ path_walker: Node = expanded_node
+ while True:
+ path.append(path_walker)
+ if path_walker.parent_index == -1:
+ break
+ path_walker = expanded_list[path_walker.parent_index]
+
+ # reverse path so it goes start -> goal
+ path.reverse()
+ return NodePath(path, len(expanded_set))
+
+ expanded_idx = len(expanded_list)
+ expanded_list.append(expanded_node)
+ expanded_set.add(expanded_node)
+
+ for child in SpaceTimeAStar.generate_successors(grid, goal, expanded_node, expanded_idx, verbose, expanded_set):
+ heapq.heappush(open_set, child)
+
+ raise Exception("No path found")
+
+ """
+ Generate possible successors of the provided `parent_node`
+ """
+ @staticmethod
+ def generate_successors(
+ grid: Grid, goal: Position, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node]
+ ) -> Generator[Node, None, None]:
+ diffs = [
+ Position(0, 0),
+ Position(1, 0),
+ Position(-1, 0),
+ Position(0, 1),
+ Position(0, -1),
+ ]
+ for diff in diffs:
+ new_pos = parent_node.position + diff
+ new_node = Node(
+ new_pos,
+ parent_node.time + 1,
+ SpaceTimeAStar.calculate_heuristic(new_pos, goal),
+ parent_node_idx,
+ )
+
+ if new_node in expanded_set:
+ continue
+
+ # Check if the new node is valid for the next 2 time steps - one step to enter, and another to leave
+ if all([grid.valid_position(new_pos, parent_node.time + dt) for dt in [1, 2]]):
+ if verbose:
+ print("\tNew successor node: ", new_node)
+ yield new_node
+
+ @staticmethod
+ def calculate_heuristic(position: Position, goal: Position) -> int:
+ diff = goal - position
+ return abs(diff.x) + abs(diff.y)
+
+
+show_animation = True
+verbose = False
+
+def main():
+ start = Position(1, 5)
+ goal = Position(19, 19)
+ grid_side_length = 21
+
+ start_time = time.time()
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=40,
+ obstacle_avoid_points=[start, goal],
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ path = SpaceTimeAStar.plan(grid, start, goal, verbose)
+
+ runtime = time.time() - start_time
+ print(f"Planning took: {runtime:.5f} seconds")
+
+ if verbose:
+ print(f"Path: {path}")
+
+ if not show_animation:
+ return
+
+ PlotNodePath(grid, start, goal, path)
+
+if __name__ == "__main__":
+ main()
diff --git a/Control/__init__.py b/PathPlanning/TimeBasedPathPlanning/__init__.py
similarity index 100%
rename from Control/__init__.py
rename to PathPlanning/TimeBasedPathPlanning/__init__.py
diff --git a/PathPlanning/VisibilityRoadMap/visibility_road_map.py b/PathPlanning/VisibilityRoadMap/visibility_road_map.py
index 6458cd882c..5f7ffadd16 100644
--- a/PathPlanning/VisibilityRoadMap/visibility_road_map.py
+++ b/PathPlanning/VisibilityRoadMap/visibility_road_map.py
@@ -6,17 +6,15 @@
"""
-import os
import sys
import math
import numpy as np
import matplotlib.pyplot as plt
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
-from PathPlanning.VisibilityRoadMap.geometry import Geometry
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../VoronoiRoadMap/")
-from dijkstra_search import DijkstraSearch
+from VisibilityRoadMap.geometry import Geometry
+from VoronoiRoadMap.dijkstra_search import DijkstraSearch
show_animation = True
@@ -64,8 +62,9 @@ def generate_visibility_nodes(self, start_x, start_y, goal_x, goal_y,
for (vx, vy) in zip(cvx_list, cvy_list):
nodes.append(DijkstraSearch.Node(vx, vy))
- for node in nodes:
- plt.plot(node.x, node.y, "xr")
+ if self.do_plot:
+ for node in nodes:
+ plt.plot(node.x, node.y, "xr")
return nodes
diff --git a/PathPlanning/VoronoiRoadMap/__init__.py b/PathPlanning/VoronoiRoadMap/__init__.py
index 087dab646e..e69de29bb2 100644
--- a/PathPlanning/VoronoiRoadMap/__init__.py
+++ b/PathPlanning/VoronoiRoadMap/__init__.py
@@ -1,4 +0,0 @@
-import os
-import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)))
diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py
index d184230fd5..a27e1b6928 100644
--- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py
+++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py
@@ -9,8 +9,12 @@
import math
import numpy as np
import matplotlib.pyplot as plt
-from dijkstra_search import DijkstraSearch
from scipy.spatial import cKDTree, Voronoi
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
+
+from VoronoiRoadMap.dijkstra_search import DijkstraSearch
show_animation = True
@@ -142,20 +146,20 @@ def main():
oy = []
for i in range(60):
- ox.append(i)
+ ox.append(float(i))
oy.append(0.0)
for i in range(60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(61):
ox.append(0.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
diff --git a/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py b/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py
index 8968c54dcc..c5a139454b 100644
--- a/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py
+++ b/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py
@@ -4,7 +4,7 @@
author: Todd Tang
paper: Planning paths of complete coverage of an unstructured environment
by a mobile robot - Zelinsky et.al.
-link: http://pinkwink.kr/attachment/cfile3.uf@1354654A4E8945BD13FE77.pdf
+link: https://pinkwink.kr/attachment/cfile3.uf@1354654A4E8945BD13FE77.pdf
"""
import os
@@ -64,7 +64,7 @@ def transform(
is_visited = np.zeros_like(transform_matrix, dtype=bool)
is_visited[src[0], src[1]] = True
traversal_queue = [src]
- calculated = [(src[0] - 1) * n_cols + src[1]]
+ calculated = set([(src[0] - 1) * n_cols + src[1]])
def is_valid_neighbor(g_i, g_j):
return 0 <= g_i < n_rows and 0 <= g_j < n_cols \
@@ -86,7 +86,7 @@ def is_valid_neighbor(g_i, g_j):
if not is_visited[ni][nj] \
and ((ni - 1) * n_cols + nj) not in calculated:
traversal_queue.append((ni, nj))
- calculated.append((ni - 1) * n_cols + nj)
+ calculated.add((ni - 1) * n_cols + nj)
return transform_matrix
diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py
index bb5699b888..ee40e73504 100644
--- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py
+++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py
@@ -4,9 +4,9 @@
author Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
Shunichi09/nonlinear_control: Implementing the nonlinear model predictive
-control, sliding mode control https://github.com/Shunichi09/nonlinear_control
+control, sliding mode control https://github.com/Shunichi09/PythonLinearNonlinearControl
"""
diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
index db5a755008..5831d02d30 100644
--- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
+++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
@@ -7,19 +7,14 @@
"""
import math
import sys
-import os
-
import matplotlib.pyplot as plt
import numpy as np
import scipy.linalg as la
+import pathlib
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../PathPlanning/CubicSpline/")
-
-try:
- import cubic_spline_planner
-except ImportError:
- raise
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+from PathPlanning.CubicSpline import cubic_spline_planner
# === Parameters =====
@@ -58,7 +53,7 @@ def update(state, a, delta):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def solve_dare(A, B, Q, R):
@@ -227,8 +222,9 @@ def do_simulation(cx, cy, cyaw, ck, speed_profile, goal):
if target_ind % 1 == 0 and show_animation:
plt.cla()
# for stopping simulation with the esc key.
- plt.gcf().canvas.mpl_connect('key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
@@ -296,6 +292,13 @@ def main():
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
+ plt.subplots(1)
+
+ plt.plot(t, np.array(v)*3.6, label="speed")
+ plt.grid(True)
+ plt.xlabel("Time [sec]")
+ plt.ylabel("Speed [m/s]")
+ plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py
index 5596ea0fdf..3c066917ff 100644
--- a/PathTracking/lqr_steer_control/lqr_steer_control.py
+++ b/PathTracking/lqr_steer_control/lqr_steer_control.py
@@ -10,16 +10,11 @@
import math
import numpy as np
import sys
-import os
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../PathPlanning/CubicSpline/")
-
-try:
- import cubic_spline_planner
-except:
- raise
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+from PathPlanning.CubicSpline import cubic_spline_planner
Kp = 1.0 # speed proportional gain
@@ -29,7 +24,7 @@
# parameters
dt = 0.1 # time tick[s]
-L = 0.5 # Wheel base of the vehicle [m]
+L = 0.5 # Wheelbase of the vehicle [m]
max_steer = np.deg2rad(45.0) # maximum steering angle[rad]
show_animation = True
@@ -60,14 +55,14 @@ def update(state, a, delta):
return state
-def PIDControl(target, current):
+def pid_control(target, current):
a = Kp * (target - current)
return a
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def solve_DARE(A, B, Q, R):
@@ -75,10 +70,11 @@ def solve_DARE(A, B, Q, R):
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X = Q
- maxiter = 150
+ Xn = Q
+ max_iter = 150
eps = 0.01
- for i in range(maxiter):
+ for i in range(max_iter):
Xn = A.T @ X @ A - A.T @ X @ B @ \
la.inv(R + B.T @ X @ B) @ B.T @ X @ A + Q
if (abs(Xn - X)).max() < eps:
@@ -183,7 +179,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
dl, target_ind, e, e_th = lqr_steering_control(
state, cx, cy, cyaw, ck, e, e_th)
- ai = PIDControl(speed_profile[target_ind], state.v)
+ ai = pid_control(speed_profile[target_ind], state.v)
state = update(state, ai, dl)
if abs(state.v) <= stop_speed:
@@ -207,8 +203,9 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
if target_ind % 1 == 0 and show_animation:
plt.cla()
# for stopping simulation with the esc key.
- plt.gcf().canvas.mpl_connect('key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
index ac8b883bd9..eb2d7b6a73 100644
--- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
+++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
@@ -6,20 +6,17 @@
"""
import matplotlib.pyplot as plt
+import time
import cvxpy
import math
import numpy as np
import sys
-import os
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../PathPlanning/CubicSpline/")
-
-try:
- import cubic_spline_planner
-except:
- raise
+from utils.angle import angle_mod
+from PathPlanning.CubicSpline import cubic_spline_planner
NX = 4 # x = x, y, v, yaw
NU = 2 # a = [accel, steer]
@@ -75,13 +72,7 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
def pi_2_pi(angle):
- while(angle > math.pi):
- angle = angle - 2.0 * math.pi
-
- while(angle < -math.pi):
- angle = angle + 2.0 * math.pi
-
- return angle
+ return angle_mod(angle)
def get_linear_model_matrix(v, phi, delta):
@@ -231,8 +222,9 @@ def predict_motion(x0, oa, od, xref):
def iterative_linear_mpc_control(xref, x0, dref, oa, od):
"""
- MPC contorl with updating operational point iteraitvely
+ MPC control with updating operational point iteratively
"""
+ ox, oy, oyaw, ov = None, None, None, None
if oa is None or od is None:
oa = [0.0] * T
@@ -291,7 +283,7 @@ def linear_mpc_control(xref, xbar, x0, dref):
constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]
prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
- prob.solve(solver=cvxpy.ECOS, verbose=False)
+ prob.solve(solver=cvxpy.CLARABEL, verbose=False)
if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
ox = get_nparray_from_matrix(x.value[0, :])
@@ -412,10 +404,11 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control(
xref, x0, dref, oa, odelta)
+ di, ai = 0.0, 0.0
if odelta is not None:
di, ai = odelta[0], oa[0]
+ state = update_state(state, ai, di)
- state = update_state(state, ai, di)
time = time + DT
x.append(state.x)
@@ -554,6 +547,7 @@ def get_switch_back_course(dl):
def main():
print(__file__ + " start!!")
+ start = time.time()
dl = 1.0 # course tick
# cx, cy, cyaw, ck = get_straight_course(dl)
@@ -569,6 +563,9 @@ def main():
t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state)
+ elapsed_time = time.time() - start
+ print(f"calc time:{elapsed_time:.6f} [sec]")
+
if show_animation: # pragma: no cover
plt.close("all")
plt.subplots()
@@ -591,6 +588,7 @@ def main():
def main2():
print(__file__ + " start!!")
+ start = time.time()
dl = 1.0 # course tick
cx, cy, cyaw, ck = get_straight_course3(dl)
@@ -602,6 +600,9 @@ def main2():
t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state)
+ elapsed_time = time.time() - start
+ print(f"calc time:{elapsed_time:.6f} [sec]")
+
if show_animation: # pragma: no cover
plt.close("all")
plt.subplots()
diff --git a/Control/move_to_pose/__init__.py b/PathTracking/move_to_pose/__init__.py
similarity index 100%
rename from Control/move_to_pose/__init__.py
rename to PathTracking/move_to_pose/__init__.py
diff --git a/Control/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
similarity index 64%
rename from Control/move_to_pose/move_to_pose.py
rename to PathTracking/move_to_pose/move_to_pose.py
index 73604ccd7f..faf1264953 100644
--- a/Control/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -5,6 +5,7 @@
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)
Seied Muhammad Yazdian (@Muhammad-Yazdian)
+ Wang Zheng (@Aglargil)
P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
@@ -12,6 +13,11 @@
import matplotlib.pyplot as plt
import numpy as np
+import sys
+import pathlib
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
from random import random
@@ -70,15 +76,20 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
# [-pi, pi] to prevent unstable behavior e.g. difference going
# from 0 rad to 2*pi rad with slight turn
+ # The velocity v always has a constant sign which depends on the initial value of α.
rho = np.hypot(x_diff, y_diff)
- alpha = (np.arctan2(y_diff, x_diff)
- - theta + np.pi) % (2 * np.pi) - np.pi
- beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
v = self.Kp_rho * rho
- w = self.Kp_alpha * alpha - controller.Kp_beta * beta
+ alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta)
+ beta = angle_mod(theta_goal - theta - alpha)
if alpha > np.pi / 2 or alpha < -np.pi / 2:
+ # recalculate alpha to make alpha in the range of [-pi/2, pi/2]
+ alpha = angle_mod(np.arctan2(-y_diff, -x_diff) - theta)
+ beta = angle_mod(theta_goal - theta - alpha)
+ w = self.Kp_alpha * alpha - self.Kp_beta * beta
v = -v
+ else:
+ w = self.Kp_alpha * alpha - self.Kp_beta * beta
return rho, v, w
@@ -86,6 +97,7 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
# simulation parameters
controller = PathFinderController(9, 15, 3)
dt = 0.01
+MAX_SIM_TIME = 5 # seconds, robot will stop moving when time exceeds this value
# Robot specifications
MAX_LINEAR_SPEED = 15
@@ -102,18 +114,19 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
x_diff = x_goal - x
y_diff = y_goal - y
- x_traj, y_traj = [], []
+ x_traj, y_traj, v_traj, w_traj = [x], [y], [0], [0]
rho = np.hypot(x_diff, y_diff)
- while rho > 0.001:
+ t = 0
+ while rho > 0.001 and t < MAX_SIM_TIME:
+ t += dt
x_traj.append(x)
y_traj.append(y)
x_diff = x_goal - x
y_diff = y_goal - y
- rho, v, w = controller.calc_control_command(
- x_diff, y_diff, theta, theta_goal)
+ rho, v, w = controller.calc_control_command(x_diff, y_diff, theta, theta_goal)
if abs(v) > MAX_LINEAR_SPEED:
v = np.sign(v) * MAX_LINEAR_SPEED
@@ -121,18 +134,35 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
if abs(w) > MAX_ANGULAR_SPEED:
w = np.sign(w) * MAX_ANGULAR_SPEED
+ v_traj.append(v)
+ w_traj.append(w)
+
theta = theta + w * dt
x = x + v * np.cos(theta) * dt
y = y + v * np.sin(theta) * dt
if show_animation: # pragma: no cover
plt.cla()
- plt.arrow(x_start, y_start, np.cos(theta_start),
- np.sin(theta_start), color='r', width=0.1)
- plt.arrow(x_goal, y_goal, np.cos(theta_goal),
- np.sin(theta_goal), color='g', width=0.1)
+ plt.arrow(
+ x_start,
+ y_start,
+ np.cos(theta_start),
+ np.sin(theta_start),
+ color="r",
+ width=0.1,
+ )
+ plt.arrow(
+ x_goal,
+ y_goal,
+ np.cos(theta_goal),
+ np.sin(theta_goal),
+ color="g",
+ width=0.1,
+ )
plot_vehicle(x, y, theta, x_traj, y_traj)
+ return x_traj, y_traj, v_traj, w_traj
+
def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
# Corners of triangular vehicle when pointing to the right (0 radians)
@@ -145,16 +175,16 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
p2 = np.matmul(T, p2_i)
p3 = np.matmul(T, p3_i)
- plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
- plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
- plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')
+ plt.plot([p1[0], p2[0]], [p1[1], p2[1]], "k-")
+ plt.plot([p2[0], p3[0]], [p2[1], p3[1]], "k-")
+ plt.plot([p3[0], p1[0]], [p3[1], p1[1]], "k-")
- plt.plot(x_traj, y_traj, 'b--')
+ plt.plot(x_traj, y_traj, "b--")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+ )
plt.xlim(0, 20)
plt.ylim(0, 20)
@@ -163,28 +193,31 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
def transformation_matrix(x, y, theta):
- return np.array([
- [np.cos(theta), -np.sin(theta), x],
- [np.sin(theta), np.cos(theta), y],
- [0, 0, 1]
- ])
+ return np.array(
+ [
+ [np.cos(theta), -np.sin(theta), x],
+ [np.sin(theta), np.cos(theta), y],
+ [0, 0, 1],
+ ]
+ )
def main():
-
for i in range(5):
- x_start = 20 * random()
- y_start = 20 * random()
- theta_start = 2 * np.pi * random() - np.pi
+ x_start = 20.0 * random()
+ y_start = 20.0 * random()
+ theta_start: float = 2 * np.pi * random() - np.pi
x_goal = 20 * random()
y_goal = 20 * random()
theta_goal = 2 * np.pi * random() - np.pi
- print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
- (x_start, y_start, theta_start))
- print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
- (x_goal, y_goal, theta_goal))
+ print(
+ f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n"
+ )
+ print(
+ f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n"
+ )
move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
-if __name__ == '__main__':
+if __name__ == "__main__":
main()
diff --git a/Control/move_to_pose/move_to_pose_robot.py b/PathTracking/move_to_pose/move_to_pose_robot.py
similarity index 98%
rename from Control/move_to_pose/move_to_pose_robot.py
rename to PathTracking/move_to_pose/move_to_pose_robot.py
index d5c51caa12..fe9f0d06b3 100644
--- a/Control/move_to_pose/move_to_pose_robot.py
+++ b/PathTracking/move_to_pose/move_to_pose_robot.py
@@ -79,9 +79,9 @@ def set_start_target_poses(self, pose_start, pose_target):
Parameters
----------
pose_start : (Pose)
- Start postion of the robot (see the Pose class)
+ Start position of the robot (see the Pose class)
pose_target : (Pose)
- Target postion of the robot (see the Pose class)
+ Target position of the robot (see the Pose class)
"""
self.pose_start = copy.copy(pose_start)
self.pose_target = pose_target
diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py
index ff995033a9..48453927ab 100644
--- a/PathTracking/pure_pursuit/pure_pursuit.py
+++ b/PathTracking/pure_pursuit/pure_pursuit.py
@@ -10,6 +10,11 @@
import math
import matplotlib.pyplot as plt
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+
# Parameters
k = 0.1 # look forward gain
Lfc = 2.0 # [m] look-ahead distance
@@ -17,26 +22,37 @@
dt = 0.1 # [s] time tick
WB = 2.9 # [m] wheel base of vehicle
-show_animation = True
+# Vehicle parameters
+LENGTH = WB + 1.0 # Vehicle length
+WIDTH = 2.0 # Vehicle width
+WHEEL_LEN = 0.6 # Wheel length
+WHEEL_WIDTH = 0.2 # Wheel width
+MAX_STEER = math.pi / 4 # Maximum steering angle [rad]
-class State:
- def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+show_animation = True
+pause_simulation = False # Flag for pause simulation
+is_reverse_mode = False # Flag for reverse driving mode
+
+class State:
+ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, is_reverse=False):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
- self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw))
- self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw))
+ self.direction = -1 if is_reverse else 1 # Direction based on reverse flag
+ self.rear_x = self.x - self.direction * ((WB / 2) * math.cos(self.yaw))
+ self.rear_y = self.y - self.direction * ((WB / 2) * math.sin(self.yaw))
def update(self, a, delta):
self.x += self.v * math.cos(self.yaw) * dt
self.y += self.v * math.sin(self.yaw) * dt
- self.yaw += self.v / WB * math.tan(delta) * dt
+ self.yaw += self.direction * self.v / WB * math.tan(delta) * dt
+ self.yaw = angle_mod(self.yaw)
self.v += a * dt
- self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw))
- self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw))
+ self.rear_x = self.x - self.direction * ((WB / 2) * math.cos(self.yaw))
+ self.rear_y = self.y - self.direction * ((WB / 2) * math.sin(self.yaw))
def calc_distance(self, point_x, point_y):
dx = self.rear_x - point_x
@@ -51,6 +67,7 @@ def __init__(self):
self.y = []
self.yaw = []
self.v = []
+ self.direction = []
self.t = []
def append(self, t, state):
@@ -58,6 +75,7 @@ def append(self, t, state):
self.y.append(state.y)
self.yaw.append(state.yaw)
self.v.append(state.v)
+ self.direction.append(state.direction)
self.t.append(t)
@@ -117,14 +135,18 @@ def pure_pursuit_steer_control(state, trajectory, pind):
if ind < len(trajectory.cx):
tx = trajectory.cx[ind]
ty = trajectory.cy[ind]
- else: # toward goal
+ else:
tx = trajectory.cx[-1]
ty = trajectory.cy[-1]
ind = len(trajectory.cx) - 1
alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw
- delta = math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)
+ # Reverse steering angle when reversing
+ delta = state.direction * math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)
+
+ # Limit steering angle to max value
+ delta = np.clip(delta, -MAX_STEER, MAX_STEER)
return delta, ind
@@ -142,10 +164,111 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
+def plot_vehicle(x, y, yaw, steer=0.0, color='blue', is_reverse=False):
+ """
+ Plot vehicle model with four wheels
+ Args:
+ x, y: Vehicle center position
+ yaw: Vehicle heading angle
+ steer: Steering angle
+ color: Vehicle color
+ is_reverse: Flag for reverse mode
+ """
+ # Adjust heading angle in reverse mode
+ if is_reverse:
+ yaw = angle_mod(yaw + math.pi) # Rotate heading by 180 degrees
+ steer = -steer # Reverse steering direction
+
+ def plot_wheel(x, y, yaw, steer=0.0, color=color):
+ """Plot single wheel"""
+ wheel = np.array([
+ [-WHEEL_LEN/2, WHEEL_WIDTH/2],
+ [WHEEL_LEN/2, WHEEL_WIDTH/2],
+ [WHEEL_LEN/2, -WHEEL_WIDTH/2],
+ [-WHEEL_LEN/2, -WHEEL_WIDTH/2],
+ [-WHEEL_LEN/2, WHEEL_WIDTH/2]
+ ])
+
+ # Rotate wheel if steering
+ if steer != 0:
+ c, s = np.cos(steer), np.sin(steer)
+ rot_steer = np.array([[c, -s], [s, c]])
+ wheel = wheel @ rot_steer.T
+
+ # Apply vehicle heading rotation
+ c, s = np.cos(yaw), np.sin(yaw)
+ rot_yaw = np.array([[c, -s], [s, c]])
+ wheel = wheel @ rot_yaw.T
+
+ # Translate to position
+ wheel[:, 0] += x
+ wheel[:, 1] += y
+
+ # Plot wheel with color
+ plt.plot(wheel[:, 0], wheel[:, 1], color=color)
+
+ # Calculate vehicle body corners
+ corners = np.array([
+ [-LENGTH/2, WIDTH/2],
+ [LENGTH/2, WIDTH/2],
+ [LENGTH/2, -WIDTH/2],
+ [-LENGTH/2, -WIDTH/2],
+ [-LENGTH/2, WIDTH/2]
+ ])
+
+ # Rotation matrix
+ c, s = np.cos(yaw), np.sin(yaw)
+ Rot = np.array([[c, -s], [s, c]])
+
+ # Rotate and translate vehicle body
+ rotated = corners @ Rot.T
+ rotated[:, 0] += x
+ rotated[:, 1] += y
+
+ # Plot vehicle body
+ plt.plot(rotated[:, 0], rotated[:, 1], color=color)
+
+ # Plot wheels (darker color for front wheels)
+ front_color = 'darkblue'
+ rear_color = color
+
+ # Plot four wheels
+ # Front left
+ plot_wheel(x + LENGTH/4 * c - WIDTH/2 * s,
+ y + LENGTH/4 * s + WIDTH/2 * c,
+ yaw, steer, front_color)
+ # Front right
+ plot_wheel(x + LENGTH/4 * c + WIDTH/2 * s,
+ y + LENGTH/4 * s - WIDTH/2 * c,
+ yaw, steer, front_color)
+ # Rear left
+ plot_wheel(x - LENGTH/4 * c - WIDTH/2 * s,
+ y - LENGTH/4 * s + WIDTH/2 * c,
+ yaw, color=rear_color)
+ # Rear right
+ plot_wheel(x - LENGTH/4 * c + WIDTH/2 * s,
+ y - LENGTH/4 * s - WIDTH/2 * c,
+ yaw, color=rear_color)
+
+ # Add direction arrow
+ arrow_length = LENGTH/3
+ plt.arrow(x, y,
+ -arrow_length * math.cos(yaw) if is_reverse else arrow_length * math.cos(yaw),
+ -arrow_length * math.sin(yaw) if is_reverse else arrow_length * math.sin(yaw),
+ head_width=WIDTH/4, head_length=WIDTH/4,
+ fc='r', ec='r', alpha=0.5)
+
+# Keyboard event handler
+def on_key(event):
+ global pause_simulation
+ if event.key == ' ': # Space key
+ pause_simulation = not pause_simulation
+ elif event.key == 'escape':
+ exit(0)
def main():
# target course
- cx = np.arange(0, 50, 0.5)
+ cx = -1 * np.arange(0, 50, 0.5) if is_reverse_mode else np.arange(0, 50, 0.5)
cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
target_speed = 10.0 / 3.6 # [m/s]
@@ -153,7 +276,7 @@ def main():
T = 100.0 # max simulation time
# initial state
- state = State(x=-0.0, y=-3.0, yaw=0.0, v=0.0)
+ state = State(x=-0.0, y=-3.0, yaw=math.pi if is_reverse_mode else 0.0, v=0.0, is_reverse=is_reverse_mode)
lastIndex = len(cx) - 1
time = 0.0
@@ -173,22 +296,33 @@ def main():
time += dt
states.append(time, state)
-
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
- plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
- plot_arrow(state.x, state.y, state.yaw)
+ plt.gcf().canvas.mpl_connect('key_release_event', on_key)
+ # Pass is_reverse parameter
+ plot_vehicle(state.x, state.y, state.yaw, di, is_reverse=is_reverse_mode)
plt.plot(cx, cy, "-r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
+ plt.legend() # Add legend display
+
+ # Add pause state display
+ if pause_simulation:
+ plt.text(0.02, 0.95, 'PAUSED', transform=plt.gca().transAxes,
+ bbox=dict(facecolor='red', alpha=0.5))
+
plt.pause(0.001)
+ # Handle pause state
+ while pause_simulation:
+ plt.pause(0.1) # Reduce CPU usage
+ if not plt.get_fignums(): # Check if window is closed
+ exit(0)
+
# Test
assert lastIndex >= target_ind, "Cannot goal"
diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
similarity index 95%
rename from PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
rename to PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
index ff72454f34..fd04fb6d17 100644
--- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
+++ b/PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
@@ -8,10 +8,14 @@
import matplotlib.pyplot as plt
import math
import numpy as np
-
+import sys
+import pathlib
from scipy import interpolate
from scipy import optimize
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+
Kp = 1.0 # speed proportional gain
# steering control parameter
KTH = 1.0
@@ -51,21 +55,21 @@ def __init__(self, x, y):
self.ddY = self.Y.derivative(2)
self.length = s[-1]
-
+
def calc_yaw(self, s):
dx, dy = self.dX(s), self.dY(s)
return np.arctan2(dy, dx)
-
+
def calc_curvature(self, s):
dx, dy = self.dX(s), self.dY(s)
ddx, ddy = self.ddX(s), self.ddY(s)
return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
-
+
def __find_nearest_point(self, s0, x, y):
def calc_distance(_s, *args):
_x, _y= self.X(_s), self.Y(_s)
return (_x - args[0])**2 + (_y - args[1])**2
-
+
def calc_distance_jacobian(_s, *args):
_x, _y = self.X(_s), self.Y(_s)
_dx, _dy = self.dX(_s), self.dY(_s)
@@ -76,7 +80,7 @@ def calc_distance_jacobian(_s, *args):
def calc_track_error(self, x, y, s0):
ret = self.__find_nearest_point(s0, x, y)
-
+
s = ret[0][0]
e = ret[1]
@@ -96,13 +100,7 @@ def pid_control(target, current):
return a
def pi_2_pi(angle):
- while(angle > math.pi):
- angle = angle - 2.0 * math.pi
-
- while(angle < -math.pi):
- angle = angle + 2.0 * math.pi
-
- return angle
+ return angle_mod(angle)
def rear_wheel_feedback_control(state, e, k, yaw_ref):
v = state.v
@@ -170,7 +168,7 @@ def simulate(path_ref, goal):
plt.plot(path_ref.X(s0), path_ref.Y(s0), "xg", label="target")
plt.axis("equal")
plt.grid(True)
- plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0))
+ plt.title(f"speed[km/h]:{round(state.v * 3.6, 2):.2f}, target s-param:{s0:.2f}")
plt.pause(0.0001)
return t, x, y, yaw, v, goal_flag
@@ -184,7 +182,7 @@ def calc_target_speed(state, yaw_ref):
if switch:
state.direction *= -1
return 0.0
-
+
if state.direction != 1:
return -target_speed
diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_control/stanley_control.py
similarity index 93%
rename from PathTracking/stanley_controller/stanley_controller.py
rename to PathTracking/stanley_control/stanley_control.py
index 3a67d6268e..01c2ec0229 100644
--- a/PathTracking/stanley_controller/stanley_controller.py
+++ b/PathTracking/stanley_control/stanley_control.py
@@ -4,7 +4,7 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
- [Stanley: The robot that won the DARPA grand challenge](http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf)
- [Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf)
@@ -12,16 +12,11 @@
import numpy as np
import matplotlib.pyplot as plt
import sys
-import os
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../PathPlanning/CubicSpline/")
-
-try:
- import cubic_spline_planner
-except:
- raise
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+from PathPlanning.CubicSpline import cubic_spline_planner
k = 0.5 # control gain
Kp = 1.0 # speed proportional gain
@@ -32,7 +27,7 @@
show_animation = True
-class State(object):
+class State:
"""
Class representing the state of a vehicle.
@@ -44,7 +39,7 @@ class State(object):
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
"""Instantiate the object."""
- super(State, self).__init__()
+ super().__init__()
self.x = x
self.y = y
self.yaw = yaw
@@ -112,13 +107,7 @@ def normalize_angle(angle):
:param angle: (float)
:return: (float) Angle in radian in [-pi, pi]
"""
- while angle > np.pi:
- angle -= 2.0 * np.pi
-
- while angle < -np.pi:
- angle += 2.0 * np.pi
-
- return angle
+ return angle_mod(angle)
def calc_target_index(state, cx, cy):
diff --git a/README.md b/README.md
index 2d0ee8d653..d1b801f219 100644
--- a/README.md
+++ b/README.md
@@ -3,12 +3,10 @@
# PythonRobotics
+
[](https://ci.appveyor.com/project/AtsushiSakai/pythonrobotics)
-[](https://codecov.io/gh/AtsushiSakai/PythonRobotics)
-[](https://lgtm.com/projects/g/AtsushiSakai/PythonRobotics/context:python)
-[](https://github.com/AtsushiSakai/PythonRobotics)
-Python codes for robotics algorithm.
+Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index.html) for robotics algorithm.
# Table of Contents
@@ -70,13 +68,14 @@ Python codes for robotics algorithm.
* [Contribution](#contribution)
* [Citing](#citing)
* [Support](#support)
- * [Sponsors](#Sponsors)
+ * [Sponsors](#sponsors)
* [JetBrains](#JetBrains)
+ * [1Password](#1password)
* [Authors](#authors)
-# What is this?
+# What is PythonRobotics?
-This is a Python code collection of robotics algorithms.
+PythonRobotics is a Python code collection and a [textbook](https://atsushisakai.github.io/PythonRobotics/index.html) of robotics algorithms.
Features:
@@ -86,16 +85,24 @@ Features:
3. Minimum dependency.
-See this paper for more details:
+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
+# Requirements to run the code
For running each sample code:
-- [Python 3.10.x](https://www.python.org/)
+- [Python 3.13.x](https://www.python.org/)
- [NumPy](https://numpy.org/)
@@ -103,29 +110,27 @@ For running each sample code:
- [Matplotlib](https://matplotlib.org/)
-- [pandas](https://pandas.pydata.org/)
-
- [cvxpy](https://www.cvxpy.org/)
For development:
-- pytest (for unit tests)
+- [pytest](https://pytest.org/) (for unit tests)
-- pytest-xdist (for parallel unit tests)
+- [pytest-xdist](https://pypi.org/project/pytest-xdist/) (for parallel unit tests)
-- mypy (for type check)
+- [mypy](https://mypy-lang.org/) (for type check)
-- sphinx (for document generation)
+- [sphinx](https://www.sphinx-doc.org/) (for document generation)
-- pycodestyle (for code style check)
+- [pycodestyle](https://pypi.org/project/pycodestyle/) (for code style check)
-# Documentation
+# 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 online: [Welcome to PythonRobotics’s documentation\! — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/index.html)
+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)
@@ -133,18 +138,24 @@ All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation
1. Clone this repo.
-> git clone https://github.com/AtsushiSakai/PythonRobotics.git
+ ```terminal
+ git clone https://github.com/AtsushiSakai/PythonRobotics.git
+ ```
2. Install the required libraries.
-using conda :
+- using conda :
-> conda env create -f requirements/environment.yml
+ ```terminal
+ conda env create -f requirements/environment.yml
+ ```
-using pip :
+- using pip :
-> pip install -r requirements/requirements.txt
+ ```terminal
+ pip install -r requirements/requirements.txt
+ ```
3. Execute python script in each directory.
@@ -157,7 +168,9 @@ using pip :
-Documentation: [Notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb)
+Reference
+
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
## Particle filter localization
@@ -173,7 +186,7 @@ It is assumed that the robot can measure a distance from landmarks (RFID).
These measurements are used for PF localization.
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -194,7 +207,7 @@ The filter integrates speed input and range observations from RFID for localizat
Initial position is not needed.
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -243,7 +256,7 @@ It can calculate a rotation matrix, and a translation vector between points and
-Ref:
+Reference
- [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
@@ -262,7 +275,7 @@ Black points are landmarks, blue crosses are estimated landmark positions by Fas
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -308,7 +321,7 @@ 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.
-Ref:
+Reference
- [D* Algorithm Wikipedia](https://en.wikipedia.org/wiki/D*)
@@ -322,7 +335,7 @@ The animation shows a robot finding its path and rerouting to avoid obstacles as
Refs:
-- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pd)
+- [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
@@ -333,7 +346,7 @@ 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.
-Ref:
+Reference
- [Robotic Motion Planning:Potential Functions](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf)
@@ -349,11 +362,11 @@ This script is a path planning code with state lattice planning.
This code uses the model predictive trajectory generator to solve boundary problem.
-Ref:
+Reference
-- [Optimal rough terrain trajectory generation for wheeled mobile robots](http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
+- [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](http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
+- [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
@@ -377,7 +390,7 @@ Cyan crosses means searched points with Dijkstra method,
The red line is the final path of PRM.
-Ref:
+Reference
- [Probabilistic roadmap \- Wikipedia](https://en.wikipedia.org/wiki/Probabilistic_roadmap)
@@ -393,15 +406,15 @@ 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.
-Ref:
+Reference
- [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416)
-- [Sampling-based Algorithms for Optimal Motion Planning](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf)
+- [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.
@@ -413,9 +426,9 @@ A double integrator motion model is used for LQR local planner.
-Ref:
+Reference
-- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](http://lis.csail.mit.edu/pubs/perez-icra12.pdf)
+- [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)
@@ -428,9 +441,9 @@ Motion planning with quintic polynomials.
It can calculate a 2D path, velocity, and acceleration profile based on quintic polynomials.
-Ref:
+Reference
-- [Local Path Planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
+- [Local Path Planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
## Reeds Shepp planning
@@ -438,7 +451,7 @@ A sample code with Reeds Shepp path planning.
-Ref:
+Reference
- [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html)
@@ -464,7 +477,7 @@ The cyan line is the target course and black crosses are obstacles.
The red line is the predicted path.
-Ref:
+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)
@@ -477,9 +490,9 @@ Ref:
This is a simulation of moving to a pose control
-
+
-Ref:
+Reference
- [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
@@ -490,7 +503,7 @@ Path tracking simulation with Stanley steering control and PID speed control.
-Ref:
+Reference
- [Stanley: The robot that won the DARPA grand challenge](http://robots.stanford.edu/papers/thrun.stanley05.pdf)
@@ -504,7 +517,7 @@ Path tracking simulation with rear wheel feedback steering control and PID speed
-Ref:
+Reference
- [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
@@ -515,9 +528,9 @@ Path tracking simulation with LQR speed and steering control.
-Ref:
+Reference
-- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](http://ieeexplore.ieee.org/document/5940562/)
+- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](https://ieeexplore.ieee.org/document/5940562/)
## Model predictive speed and steering control
@@ -526,9 +539,9 @@ Path tracking simulation with iterative linear model predictive speed and steeri
-Ref:
+Reference
-- [notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb)
+- [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)
@@ -538,9 +551,9 @@ A motion planning and path tracking simulation with NMPC of C-GMRES
-Ref:
+Reference
-- [notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/cgmres_nmpc/cgmres_nmpc.ipynb)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc.html)
# Arm Navigation
@@ -578,9 +591,9 @@ This is a 3d trajectory generation simulation for a rocket powered landing.
-Ref:
+Reference
-- [notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/AerialNavigation/rocket_powered_landing/rocket_powered_landing.ipynb)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
# Bipedal
@@ -608,7 +621,7 @@ This is a list of user's comment and references:[users\_comments](https://github
Any contribution is welcome!!
-Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/how_to_contribute.html)
+Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html)
# Citing
@@ -616,7 +629,7 @@ If you use this project's code for your academic work, we encourage you to cite
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.
-# Supporting this project
+# Supporting this project
If you or your company would like to support this project, please consider:
@@ -624,16 +637,21 @@ If you or your company would like to support this project, please consider:
- [Become a backer or sponsor on Patreon](https://www.patreon.com/myenigma)
-- [One-time donation via PayPal](https://www.paypal.me/myenigmapay/)
+- [One-time donation via PayPal](https://www.paypal.com/paypalme/myenigmapay/)
If you would like to support us in some other way, please contact with creating an issue.
-## Sponsors
+## Sponsors
-### [JetBrains](https://www.jetbrains.com/)
+### [JetBrains](https://www.jetbrains.com/)
They are providing a free license of their IDEs for this OSS development.
+### [1Password](https://github.com/1Password/for-open-source)
+
+They are providing a free license of their 1Password team license for this OSS project.
+
+
# Authors
- [Contributors to AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/graphs/contributors)
diff --git a/SECURITY.md b/SECURITY.md
index 5047a6fc3d..53dcafa450 100644
--- a/SECURITY.md
+++ b/SECURITY.md
@@ -10,4 +10,4 @@ In this project, we are only support latest code.
## Reporting a Vulnerability
-If you find any sequrity related problem, let us know by creating an issue about it.
+If you find any security related problem, let us know by creating an issue about it.
diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index bb648ce9d9..5c4417d7c3 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/SLAM/EKFSLAM/ekf_slam.py
@@ -8,6 +8,7 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2
@@ -28,10 +29,9 @@
def ekf_slam(xEst, PEst, u, z):
# Predict
- S = STATE_SIZE
- G, Fx = jacob_motion(xEst[0:S], u)
- xEst[0:S] = motion_model(xEst[0:S], u)
- PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
+ G, Fx = jacob_motion(xEst, u)
+ xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
+ PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
initP = np.eye(2)
# Update
@@ -119,7 +119,7 @@ def jacob_motion(x, u):
[0.0, 0.0, DT * u[0, 0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]], dtype=float)
- G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
+ G = np.eye(len(x)) + Fx.T @ jF @ Fx
return G, Fx,
@@ -192,7 +192,7 @@ def jacob_h(q, delta, x, i):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py
index c4beaba257..98f8a66417 100644
--- a/SLAM/FastSLAM1/fast_slam1.py
+++ b/SLAM/FastSLAM1/fast_slam1.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -71,19 +72,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
- # print(xEst)
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -194,7 +194,7 @@ def compute_weight(particle, z, Q_cov):
print("singular")
return 1.0
- num = math.exp(-0.5 * dx.T @ invS @ dx)
+ num = np.exp(-0.5 * (dx.T @ invS @ dx))[0, 0]
den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
w = num / den
@@ -234,28 +234,27 @@ def resampling(particles):
pw = np.array(pw)
n_eff = 1.0 / (pw @ pw.T) # Effective particle number
- # print(n_eff)
if n_eff < NTH: # resampling
w_cum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -274,34 +273,34 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, rfid):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
for i in range(len(rfid[:, 0])):
- dx = rfid[i, 0] - xTrue[0, 0]
- dy = rfid[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_with_noize = angle + np.random.randn() * Q_sim[
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_with_noize = angle + np.random.randn() * Q_SIM[
1, 1] ** 0.5 # add noise
zi = np.array([dn, pi_2_pi(angle_with_noize), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -321,7 +320,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -330,7 +329,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -339,17 +338,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -357,18 +356,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam1(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -376,16 +375,16 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event', lambda event:
[exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py
index 7cd708df81..d4cf0d84dd 100644
--- a/SLAM/FastSLAM2/fast_slam2.py
+++ b/SLAM/FastSLAM2/fast_slam2.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -34,16 +35,16 @@
class Particle:
- def __init__(self, N_LM):
+ def __init__(self, n_landmark):
self.w = 1.0 / N_PARTICLE
self.x = 0.0
self.y = 0.0
self.yaw = 0.0
self.P = np.eye(3)
# landmark x-y positions
- self.lm = np.zeros((N_LM, LM_SIZE))
+ self.lm = np.zeros((n_landmark, LM_SIZE))
# landmark position covariance
- self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))
+ self.lmP = np.zeros((n_landmark * LM_SIZE, LM_SIZE))
def fast_slam2(particles, u, z):
@@ -72,18 +73,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -193,7 +194,7 @@ def compute_weight(particle, z, Q_cov):
except np.linalg.linalg.LinAlgError:
return 1.0
- num = math.exp(-0.5 * dz.T @ invS @ dz)
+ num = np.exp(-0.5 * dz.T @ invS @ dz)[0, 0]
den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
w = num / den
@@ -265,21 +266,21 @@ def resampling(particles):
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -298,35 +299,35 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, RFID):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
- for i in range(len(RFID[:, 0])):
+ for i in range(len(rfid[:, 0])):
- dx = RFID[i, 0] - xTrue[0, 0]
- dy = RFID[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_noise = np.random.randn() * Q_sim[1, 1] ** 0.5
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_noise = np.random.randn() * Q_SIM[1, 1] ** 0.5
angle_with_noise = angle + angle_noise # add noise
zi = np.array([dn, pi_2_pi(angle_with_noise), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -346,7 +347,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -355,7 +356,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -364,17 +365,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -382,18 +383,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam2(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -401,21 +402,21 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for iz in range(len(z[:, 0])):
landmark_id = int(z[2, iz])
- plt.plot([xEst[0], RFID[landmark_id, 0]], [
- xEst[1], RFID[landmark_id, 1]], "-k")
+ plt.plot([x_est[0][0], rfid[landmark_id, 0]], [
+ x_est[1][0], rfid[landmark_id, 1]], "-k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/GraphBasedSLAM/data/README.rst b/SLAM/GraphBasedSLAM/data/README.rst
index c875cce73f..15bc5b6c03 100644
--- a/SLAM/GraphBasedSLAM/data/README.rst
+++ b/SLAM/GraphBasedSLAM/data/README.rst
@@ -3,4 +3,4 @@ Acknowledgments and References
Thanks to Luca Larlone for allowing inclusion of the `Intel dataset `_ in this repo.
-1. Carlone, L. and Censi, A., 2014. `From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization `_. IEEE Transactions on Robotics, 30(2), pp.475-492.
+1. Carlone, L. and Censi, A., 2014. `From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization `_. IEEE Transactions on Robotics, 30(2), pp.475-492.
diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py
index 0827732cd8..edd20a959c 100644
--- a/SLAM/GraphBasedSLAM/graph_based_slam.py
+++ b/SLAM/GraphBasedSLAM/graph_based_slam.py
@@ -11,8 +11,8 @@
"""
import sys
-import os
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import copy
import itertools
@@ -21,6 +21,7 @@
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
+from utils.angle import angle_mod
# Simulation parameter
Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
@@ -117,9 +118,9 @@ def calc_edges(x_list, z_list):
for iz2 in range(len(z_list[t2][:, 0])):
if z_list[t1][iz1, 3] == z_list[t2][iz2, 3]:
d1 = z_list[t1][iz1, 0]
- angle1, phi1 = z_list[t1][iz1, 1], z_list[t1][iz1, 2]
+ angle1, _ = z_list[t1][iz1, 1], z_list[t1][iz1, 2]
d2 = z_list[t2][iz2, 0]
- angle2, phi2 = z_list[t2][iz2, 1], z_list[t2][iz2, 2]
+ angle2, _ = z_list[t2][iz2, 1], z_list[t2][iz2, 2]
edge = calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
angle1, d2, angle2, t1, t2)
@@ -188,7 +189,7 @@ def graph_based_slam(x_init, hz):
for i in range(nt):
x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]
- diff = dx.T @ dx
+ diff = (dx.T @ dx)[0, 0]
print("iteration: %d, diff: %f" % (itr + 1, diff))
if diff < 1.0e-5:
break
@@ -249,7 +250,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
diff --git a/SLAM/GraphBasedSLAM/graphslam/pose/se2.py b/SLAM/GraphBasedSLAM/graphslam/pose/se2.py
index e89317a06f..2a32e765f7 100644
--- a/SLAM/GraphBasedSLAM/graphslam/pose/se2.py
+++ b/SLAM/GraphBasedSLAM/graphslam/pose/se2.py
@@ -136,8 +136,8 @@ def inverse(self):
"""
return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]),
- self[0] * np.sin(self[2]) - self[1] * np.cos(
- [self[2]])], -self[2])
+ self[0] * np.sin(self[2]) - self[1] * np.cos(self[2])],
+ -self[2])
# ======================================================================= #
# #
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/ICPMatching/icp_matching.py
similarity index 100%
rename from SLAM/iterative_closest_point/iterative_closest_point.py
rename to SLAM/ICPMatching/icp_matching.py
diff --git a/appveyor.yml b/appveyor.yml
index 4964bab3da..72d89acf11 100644
--- a/appveyor.yml
+++ b/appveyor.yml
@@ -4,11 +4,11 @@ environment:
global:
# SDK v7.0 MSVC Express 2008's SetEnv.cmd script will fail if the
# /E:ON and /V:ON options are not enabled in the batch script intepreter
- # See: http://stackoverflow.com/a/13751649/163740
+ # See: https://stackoverflow.com/a/13751649/163740
CMD_IN_ENV: "cmd /E:ON /V:ON /C .\\appveyor\\run_with_env.cmd"
matrix:
- - PYTHON_DIR: C:\Python310-x64
+ - PYTHON_DIR: C:\Python313-x64
branches:
only:
diff --git a/docs/Makefile b/docs/Makefile
index 9296811e02..ae495eb36d 100644
--- a/docs/Makefile
+++ b/docs/Makefile
@@ -2,7 +2,8 @@
#
# You can set these variables from the command line.
-SPHINXOPTS =
+# SPHINXOPTS with -W means turn warnings into errors to fail the build when warnings are present.
+SPHINXOPTS = -W
SPHINXBUILD = sphinx-build
SPHINXPROJ = PythonRobotics
SOURCEDIR = .
diff --git a/docs/README.md b/docs/README.md
index 5c4145e8e3..fb7d4cc3bc 100644
--- a/docs/README.md
+++ b/docs/README.md
@@ -8,7 +8,7 @@ This folder contains documentation for the Python Robotics project.
#### Install Sphinx and Theme
```
-pip install sphinx sphinx-autobuild sphinx-rtd-theme
+pip install sphinx sphinx-autobuild sphinx-rtd-theme sphinx_rtd_dark_mode sphinx_copybutton sphinx_rtd_dark_mode
```
#### Building the Docs
diff --git a/docs/_static/img/source_link_1.png b/docs/_static/img/source_link_1.png
new file mode 100644
index 0000000000..53febda7cb
Binary files /dev/null and b/docs/_static/img/source_link_1.png differ
diff --git a/docs/_static/img/source_link_2.png b/docs/_static/img/source_link_2.png
new file mode 100644
index 0000000000..d5647cac60
Binary files /dev/null and b/docs/_static/img/source_link_2.png differ
diff --git a/docs/conf.py b/docs/conf.py
index 24b5518b6f..eeabab11b1 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -1,10 +1,9 @@
-# -*- coding: utf-8 -*-
#
# Configuration file for the Sphinx documentation builder.
#
# This file does only contain a selection of the most common options. For a
# full list see the documentation:
-# http://www.sphinx-doc.org/en/master/config
+# https://www.sphinx-doc.org/en/master/config
# -- Path setup --------------------------------------------------------------
@@ -14,13 +13,15 @@
#
import os
import sys
+from pathlib import Path
+
sys.path.insert(0, os.path.abspath('../'))
# -- Project information -----------------------------------------------------
project = 'PythonRobotics'
-copyright = '2018-2021, Atsushi Sakai'
+copyright = '2018-now, Atsushi Sakai'
author = 'Atsushi Sakai'
# The short X.Y version
@@ -42,10 +43,12 @@
'matplotlib.sphinxext.plot_directive',
'sphinx.ext.autodoc',
'sphinx.ext.mathjax',
- 'sphinx.ext.viewcode',
+ 'sphinx.ext.linkcode',
'sphinx.ext.napoleon',
'sphinx.ext.imgconverter',
'IPython.sphinxext.ipython_console_highlighting',
+ 'sphinx_copybutton',
+ 'sphinx_rtd_dark_mode',
]
# Add any paths that contain templates here, relative to this directory.
@@ -86,7 +89,7 @@
if on_rtd:
html_theme = 'default'
else:
- html_theme = 'sphinx_rtd_theme'
+ html_theme = 'sphinx_rtd_light_them'
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
@@ -183,4 +186,45 @@
]
-# -- Extension configuration -------------------------------------------------
+# -- linkcode setting -------------------------------------------------
+
+import inspect
+import os
+import sys
+import functools
+
+GITHUB_REPO = "https://github.com/AtsushiSakai/PythonRobotics"
+GITHUB_BRANCH = "master"
+
+
+def linkcode_resolve(domain, info):
+ if domain != "py":
+ return None
+
+ modname = info["module"]
+ fullname = info["fullname"]
+
+ try:
+ module = __import__(modname, fromlist=[fullname])
+ obj = functools.reduce(getattr, fullname.split("."), module)
+ except (ImportError, AttributeError):
+ return None
+
+ try:
+ srcfile = inspect.getsourcefile(obj)
+ srcfile = get_relative_path_from_parent(srcfile, "PythonRobotics")
+ lineno = inspect.getsourcelines(obj)[1]
+ except Exception:
+ return None
+
+ return f"{GITHUB_REPO}/blob/{GITHUB_BRANCH}/{srcfile}#L{lineno}"
+
+
+def get_relative_path_from_parent(file_path: str, parent_dir: str):
+ path = Path(file_path).resolve()
+
+ try:
+ parent_path = next(p for p in path.parents if p.name == parent_dir)
+ return str(path.relative_to(parent_path))
+ except StopIteration:
+ raise ValueError(f"Parent directory '{parent_dir}' not found in {file_path}")
diff --git a/docs/doc_requirements.txt b/docs/doc_requirements.txt
index b8702ee750..b29f4ba289 100644
--- a/docs/doc_requirements.txt
+++ b/docs/doc_requirements.txt
@@ -1,4 +1,6 @@
-sphinx == 4.3.2 # For sphinx documentation
-sphinx_rtd_theme == 1.0.0
-IPython == 7.31.1 # For sphinx documentation
+sphinx == 7.2.6 # For sphinx documentation
+sphinx_rtd_theme == 2.0.0
+IPython == 8.20.0 # For sphinx documentation
sphinxcontrib-napoleon == 0.7 # For auto doc
+sphinx-copybutton
+sphinx-rtd-dark-mode
diff --git a/docs/getting_started_main.rst b/docs/getting_started_main.rst
deleted file mode 100644
index 4a19126686..0000000000
--- a/docs/getting_started_main.rst
+++ /dev/null
@@ -1,82 +0,0 @@
-.. _`getting started`:
-
-Getting Started
-===============
-
-What is this?
--------------
-
-This is an Open Source Software (OSS) project: PythonRobotics, which is a Python code collection of robotics algorithms.
-
-The focus of the project is on autonomous navigation, and the goal is for beginners in robotics to understand the basic ideas behind each algorithm.
-
-In this project, the algorithms which are practical and widely used in both academia and industry are selected.
-
-Each sample code is written in Python3 and only depends on some standard modules for readability and ease of use.
-
-It includes intuitive animations to understand the behavior of the simulation.
-
-
-See this paper for more details:
-
-- PythonRobotics: a Python code collection of robotics algorithms: https://arxiv.org/abs/1808.10703
-
-.. _`Requirements`:
-
-Requirements
--------------
-
-- `Python 3.10.x`_
-- `NumPy`_
-- `SciPy`_
-- `Matplotlib`_
-- `pandas`_
-- `cvxpy`_
-
-For development:
-
-- pytest (for unit tests)
-- pytest-xdist (for parallel unit tests)
-- mypy (for type check)
-- sphinx (for document generation)
-- pycodestyle (for code style check)
-
-.. _`Python 3.10.x`: https://www.python.org/
-.. _`NumPy`: https://numpy.org/
-.. _`SciPy`: https://scipy.org/
-.. _`Matplotlib`: https://matplotlib.org/
-.. _`pandas`: https://pandas.pydata.org/
-.. _`cvxpy`: https://www.cvxpy.org/
-
-
-How to use
-----------
-
-1. Clone this repo and go into dir.
-
-.. code-block::
-
- >$ git clone https://github.com/AtsushiSakai/PythonRobotics.git
-
- >$ cd PythonRobotics
-
-
-2. Install the required libraries.
-
-using conda :
-
-.. code-block::
-
- >$ conda env create -f requirements/environment.yml
-
-using pip :
-
-.. code-block::
-
- >$ pip install -r requirements/requirements.txt
-
-
-3. Execute python script in each directory.
-
-4. Add star to this repo if you like it 😃.
-
diff --git a/docs/index_main.rst b/docs/index_main.rst
index 151759284d..65634f32e8 100644
--- a/docs/index_main.rst
+++ b/docs/index_main.rst
@@ -6,46 +6,47 @@
Welcome to PythonRobotics's documentation!
==========================================
-Python codes for robotics algorithm. The project is on `GitHub`_.
+"PythonRobotics" is a Python code collections and textbook
+(This document) for robotics algorithm, which is developed on `GitHub`_.
-This is a Python code collection of robotics algorithms.
+See this section (:ref:`What is PythonRobotics?`) for more details of this project.
-Features:
+This project is `the one of the most popular open-source software (OSS) in
+the field of robotics on GitHub`_.
+This is `user comments about this project`_, and
+this graph shows GitHub star history of this project:
-1. Easy to read for understanding each algorithm's basic idea.
+.. image:: https://api.star-history.com/svg?repos=AtsushiSakai/PythonRobotics&type=Date
+ :alt: Star History
+ :width: 80%
+ :align: center
-2. Widely used and practical algorithms are selected.
-3. Minimum dependency.
-
-See this paper for more details:
-
-- `[1808.10703] PythonRobotics: a Python code collection of robotics
- algorithms`_ (`BibTeX`_)
-
-
-.. _`[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
.. _GitHub: https://github.com/AtsushiSakai/PythonRobotics
+.. _`user comments about this project`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/users_comments.md
+.. _`MIT license`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE
+.. _`the one of the most popular open-source software (OSS) in the field of robotics on GitHub`: https://github.com/topics/robotics
+----------------------------------
.. toctree::
:maxdepth: 2
- :caption: Contents
-
- getting_started
- modules/introduction
- modules/localization/localization
- modules/mapping/mapping
- modules/slam/slam
- modules/path_planning/path_planning
- modules/path_tracking/path_tracking
- modules/arm_navigation/arm_navigation
- modules/aerial_navigation/aerial_navigation
- modules/bipedal/bipedal
- modules/control/control
- modules/appendix/appendix
- how_to_contribute
+ :caption: Table of Contents
+
+ modules/0_getting_started/0_getting_started
+ modules/1_introduction/introduction
+ modules/2_localization/localization
+ modules/3_mapping/mapping
+ modules/4_slam/slam
+ modules/5_path_planning/path_planning
+ modules/6_path_tracking/path_tracking
+ modules/7_arm_navigation/arm_navigation
+ modules/8_aerial_navigation/aerial_navigation
+ modules/9_bipedal/bipedal
+ modules/10_inverted_pendulum/inverted_pendulum
+ modules/13_mission_planning/mission_planning
+ modules/11_utils/utils
+ modules/12_appendix/appendix
Indices and tables
diff --git a/docs/make.bat b/docs/make.bat
index 6aab964dcc..07dcebea41 100644
--- a/docs/make.bat
+++ b/docs/make.bat
@@ -22,7 +22,7 @@ if errorlevel 9009 (
echo.may add the Sphinx directory to PATH.
echo.
echo.If you don't have Sphinx installed, grab it from
- echo.http://sphinx-doc.org/
+ echo.https://sphinx-doc.org/
exit /b 1
)
diff --git a/docs/modules/0_getting_started/0_getting_started_main.rst b/docs/modules/0_getting_started/0_getting_started_main.rst
new file mode 100644
index 0000000000..cb2cba4784
--- /dev/null
+++ b/docs/modules/0_getting_started/0_getting_started_main.rst
@@ -0,0 +1,13 @@
+.. _`getting started`:
+
+Getting Started
+===============
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ 1_what_is_python_robotics
+ 2_how_to_run_sample_codes
+ 3_how_to_contribute
+ 4_how_to_read_textbook
diff --git a/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst b/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst
new file mode 100644
index 0000000000..2a7bd574f0
--- /dev/null
+++ b/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst
@@ -0,0 +1,130 @@
+.. _`What is PythonRobotics?`:
+
+What is PythonRobotics?
+------------------------
+
+This is an Open Source Software (OSS) project: PythonRobotics, which is a Python code collection of robotics algorithms.
+These codes are developed under `MIT license`_ and on `GitHub`_.
+
+.. _GitHub: https://github.com/AtsushiSakai/PythonRobotics
+.. _`MIT license`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE
+
+This project has three main philosophies below:
+
+Philosophy 1. Easy to understand each algorithm's basic idea.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The goal is for beginners in robotics to understand the basic ideas behind each algorithm.
+
+`Python`_ programming language is adopted in this project to achieve the goal.
+Python is a high-level programming language that is easy to read and write.
+If the code is not easy to read, it would be difficult to achieve our goal of
+allowing beginners to understand the algorithms.
+Python has great libraries for matrix operation, mathematical and scientific operation,
+and visualization, which makes code more readable because such operations
+don’t need to be re-implemented.
+Having the core Python packages allows the user to focus on the algorithms,
+rather than the implementations.
+
+PythonRobotics provides not only the code but also intuitive animations that
+visually demonstrate the process and behavior of each algorithm over time.
+This is an example of an animation gif file that shows the process of the
+path planning in a highway scenario for autonomous vehicle.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/high_speed_and_velocity_keeping_frenet_path.gif
+
+This animation is a gif file and is created using the `Matplotlib`_ library.
+All animation gif files are stored in the `PythonRoboticsGifs`_ repository and
+all animation movies are also uploaded to this `YouTube channel`_.
+
+PythonRobotics also provides a textbook that explains the basic ideas of each algorithm.
+The PythonRobotics textbook allows you to learn fundamental algorithms used in
+robotics with minimal mathematical formulas and textual explanations,
+based on PythonRobotics’ sample codes and animations.
+The contents of this document, like the code, are managed in the PythonRobotics
+`GitHub`_ repository and converted into HTML-based online documents using `Sphinx`_,
+which is a Python-based documentation builder.
+Please refer to this section ":ref:`How to read this textbook`" for information on
+how to read this document for learning.
+
+
+.. _`Python`: https://www.python.org/
+.. _`PythonRoboticsGifs`: https://github.com/AtsushiSakai/PythonRoboticsGifs
+.. _`YouTube channel`: https://youtube.com/playlist?list=PL12URV8HFpCozuz0SDxke6b2ae5UZvIwa&si=AH2fNPPYufPtK20S
+
+
+Philosophy 2. Widely used and practical algorithms are selected.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The second philosophy is that implemented algorithms have to be practical
+and widely used in both academia and industry.
+We believe learning these algorithms will be useful in many applications.
+For example, Kalman filters and particle filter for localization,
+grid mapping for mapping,
+dynamic programming based approaches and sampling based approaches for path planning,
+and optimal control based approach for path tracking.
+These algorithms are implemented and explained in the textbook in this project.
+
+Philosophy 3. Minimum dependency.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Each sample code of PythonRobotics is written in Python3 and only depends on
+some standard modules for readability and ease to setup and use.
+
+
+.. _`Requirements`:
+
+Requirements
+============
+
+PythonRobotics depends only on the following five libraries,
+including Python itself, to run each sample code.
+
+- `Python 3.12.x`_ (for Python runtime)
+- `NumPy`_ (for matrix operation)
+- `SciPy`_ (for scientific operation)
+- `cvxpy`_ (for convex optimization)
+- `Matplotlib`_ (for visualization)
+
+If you only need to run the code, the five libraries mentioned above are sufficient.
+However, for code development or creating documentation for the textbook,
+the following additional libraries are required.
+
+- `pytest`_ (for unit tests)
+- `pytest-xdist`_ (for parallel unit tests)
+- `mypy`_ (for type check)
+- `Sphinx`_ (for document generation)
+- `ruff`_ (for code style check)
+
+.. _`Python 3.12.x`: https://www.python.org/
+.. _`NumPy`: https://numpy.org/
+.. _`SciPy`: https://scipy.org/
+.. _`Matplotlib`: https://matplotlib.org/
+.. _`cvxpy`: https://www.cvxpy.org/
+.. _`pytest`: https://docs.pytest.org/en/latest/
+.. _`pytest-xdist`: https://github.com/pytest-dev/pytest-xdist
+.. _`mypy`: https://mypy-lang.org/
+.. _`Sphinx`: https://www.sphinx-doc.org/en/master/index.html
+.. _`ruff`: https://github.com/astral-sh/ruff
+
+For instructions on installing the above libraries, please refer to
+this section ":ref:`How to run sample codes`".
+
+Audio overview of this project
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+For an audio overview of this project, please refer to this `YouTube video`_.
+
+.. _`YouTube video`: https://www.youtube.com/watch?v=uMeRnNoJAfU
+
+Arxiv paper
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+We have published a paper on this project on Arxiv in 2018.
+
+See this paper for more details about this Project:
+
+- `PythonRobotics: a Python code collection of robotics algorithms`_ (`BibTeX`_)
+
+.. _`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
+
diff --git a/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst b/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst
new file mode 100644
index 0000000000..b92fc9bde0
--- /dev/null
+++ b/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst
@@ -0,0 +1,118 @@
+.. _`How to run sample codes`:
+
+How to run sample codes
+-------------------------
+
+In this chapter, we will explain the setup process for running each sample code
+in PythonRobotics and describe the contents of each directory.
+
+Steps to setup and run sample codes
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+1. Install `Python 3.12.x`_
+
+Note that older versions of Python3 might work, but we recommend using
+the specified version, because the sample codes are only tested with it.
+
+2. If you prefer to use conda for package management, install
+Anaconda or Miniconda to use `conda`_ command.
+
+3. Clone this repo and go into dir.
+
+.. code-block::
+
+ >$ git clone https://github.com/AtsushiSakai/PythonRobotics.git
+
+ >$ cd PythonRobotics
+
+
+4. Install the required libraries.
+
+We have prepared requirements management files for `conda`_ and `pip`_ under
+the requirements directory. Using these files makes it easy to install the necessary libraries.
+
+using conda :
+
+.. code-block::
+
+ >$ conda env create -f requirements/environment.yml
+
+using pip :
+
+.. code-block::
+
+ >$ pip install -r requirements/requirements.txt
+
+
+5. Execute python script in each directory.
+
+For example, to run the sample code of `Extented Kalman Filter` in the
+`localization` directory, execute the following command:
+
+.. code-block::
+
+ >$ cd localization/extended_kalman_filter
+
+ >$ python extended_kalman_filter.py
+
+Then, you can see this animation of the EKF algorithm based localization:
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif
+
+Please refer to the `Directory structure`_ section for more details on the contents of each directory.
+
+6. Add star to this repo if you like it 😃.
+
+.. _`Python 3.12.x`: https://www.python.org/
+.. _`conda`: https://docs.conda.io/projects/conda/en/stable/user-guide/install/index.html
+.. _`pip`: https://pip.pypa.io/en/stable/
+.. _`the requirements directory`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/requirements
+
+.. _`Directory structure`:
+
+Directory structure
+~~~~~~~~~~~~~~~~~~~~
+
+The top-level directory structure of PythonRobotics is as follows:
+
+Sample codes directories:
+
+- `AerialNavigation`_ : the sample codes of aerial navigation algorithms for drones and rocket landing.
+- `ArmNavigation`_ : the sample codes of arm navigation algorithms for robotic arms.
+- `Localization`_ : the sample codes of localization algorithms.
+- `Bipedal`_ : the sample codes of bipedal walking algorithms for legged robots.
+- `Control`_ : the sample codes of control algorithms for robotic systems.
+- `Mapping`_ : the sample codes of mapping or obstacle shape recognition algorithms.
+- `PathPlanning`_ : the sample codes of path planning algorithms.
+- `PathTracking`_ : the sample codes of path tracking algorithms for car like robots.
+- `SLAM`_ : the sample codes of SLAM algorithms.
+
+Other directories:
+
+- `docs`_ : This directory contains the documentation of PythonRobotics.
+- `requirements`_ : This directory contains the requirements management files.
+- `tests`_ : This directory contains the unit test files.
+- `utils`_ : This directory contains utility functions used in some sample codes in common.
+- `.github`_ : This directory contains the GitHub Actions configuration files.
+- `.circleci`_ : This directory contains the CircleCI configuration files.
+
+The structure of this document is the same as that of the sample code
+directories mentioned above.
+For more details, please refer to the :ref:`How to read this textbook` section.
+
+
+.. _`AerialNavigation`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/AerialNavigation
+.. _`ArmNavigation`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/ArmNavigation
+.. _`Localization`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Localization
+.. _`Bipedal`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Bipedal
+.. _`Control`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Control
+.. _`Mapping`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Mapping
+.. _`PathPlanning`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning
+.. _`PathTracking`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathTracking
+.. _`SLAM`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/SLAM
+.. _`docs`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/docs
+.. _`requirements`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/requirements
+.. _`tests`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/tests
+.. _`utils`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/utils
+.. _`.github`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/.github
+.. _`.circleci`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/.circleci
diff --git a/docs/how_to_contribute_main.rst b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
similarity index 63%
rename from docs/how_to_contribute_main.rst
rename to docs/modules/0_getting_started/3_how_to_contribute_main.rst
index 8a2f73e9cd..9e773e930c 100644
--- a/docs/how_to_contribute_main.rst
+++ b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
@@ -1,10 +1,39 @@
-How To Contribute
+How to contribute
=================
This document describes how to contribute this project.
+There are several ways to contribute to this project as below:
-Adding a new algorithm example
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+#. `Adding a new algorithm example`_
+#. `Reporting and fixing a defect`_
+#. `Adding missed documentations for existing examples`_
+#. `Supporting this project`_
+
+Before contributing
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Please check following items before contributing:
+
+Understanding this project
+---------------------------
+
+Please check this :ref:`What is PythonRobotics?` section and this paper
+`PythonRobotics: a Python code collection of robotics algorithms`_
+to understand the philosophies of this project.
+
+.. _`PythonRobotics: a Python code collection of robotics algorithms`: https://arxiv.org/abs/1808.10703
+
+Check your Python version.
+---------------------------
+
+We only accept a PR for Python 3.13.x or higher.
+
+We will not accept a PR for Python 2.x.
+
+.. _`Adding a new algorithm example`:
+
+1. Adding a new algorithm example
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This is a step by step manual to add a new algorithm example.
@@ -50,7 +79,7 @@ At the least, try to run the example code without animation in the unit test.
If you want to run the test suites locally, you can use the `runtests.sh` script by just executing it.
-The `test_diff_codestyle.py`_ check code style for your PR's codes.
+The `test_codestyle.py`_ check code style for your PR's codes.
.. _`how to write doc`:
@@ -67,9 +96,28 @@ Please check other documents for details.
You can build the doc locally based on `doc README`_.
-Note that the `reStructuredText`_ based doc should only focus on the mathematics and the algorithm of the example.
+For creating a gif animation, you can use this tool: `matplotrecorder`_.
+
+The created gif file should be stored in the `PythonRoboticsGifs`_ repository,
+so please create a PR to add it and refer to it in the doc.
+
+Note that the `reStructuredText`_ based doc should only focus on the
+mathematics and the algorithm of the example.
+
+Documentations related codes should be in the python script as the header
+comments of the script or docstrings of each function.
+
+Also, each document should have a link to the code in Github.
+You can easily add the link by using the `.. autoclass::`, `.. autofunction::`, and `.. automodule` by Sphinx's `autodoc`_ module.
+
+Using this `autodoc`_ module, the generated documentations have the link to the code in Github like:
+
+.. image:: /_static/img/source_link_1.png
+
+When you click the link, you will jump to the source code in Github like:
+
+.. image:: /_static/img/source_link_2.png
-Documentations related codes should be in the python script as the header comments of the script or docstrings of each function.
.. _`submit a pull request`:
@@ -92,9 +140,12 @@ After that, I will start the review.
Note that this is my hobby project; I appreciate your patience during the review process.
+
+
+.. _`Reporting and fixing a defect`:
-Reporting and fixing a defect
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+2. Reporting and fixing a defect
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Reporting and fixing a defect is also great contribution.
@@ -115,19 +166,24 @@ in the test code to show the issue was solved.
This doc `submit a pull request`_ can be helpful to submit a pull request.
-Adding missed documentations for existing examples
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. _`Adding missed documentations for existing examples`:
+
+3. Adding missed documentations for existing examples
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Adding the missed documentations for existing examples is also great contribution.
If you check the `Python Robotics Docs`_, you can notice that some of the examples
-only have a simulation gif or short overview descriptions,
+only have a simulation gif or short overview descriptions or just TBD.,
but no detailed algorithm or mathematical description.
+These documents need to be improved.
This doc `how to write doc`_ can be helpful to write documents.
-Supporting this project
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. _`Supporting this project`:
+
+4. Supporting this project
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Supporting this project financially is also a great contribution!!.
@@ -141,10 +197,12 @@ If you or your company would like to support this project, please consider:
If you would like to support us in some other way, please contact with creating an issue.
-Sponsors
----------
+Current Major Sponsors:
+
+#. `GitHub`_ : They are providing a GitHub Copilot Pro license for this OSS development.
+#. `JetBrains`_ : They are providing a free license of their IDEs for this OSS development.
+#. `1Password`_ : They are providing a free license of their 1Password team license for this OSS project.
-1. `JetBrains`_ : They are providing a free license of their IDEs for this OSS development.
.. _`Python Robotics Docs`: https://atsushisakai.github.io/PythonRobotics
@@ -155,10 +213,15 @@ Sponsors
.. _`reStructuredText`: https://www.sphinx-doc.org/en/master/usage/restructuredtext/basics.html
.. _`doc modules dir`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/docs/modules
.. _`doc README`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/docs/README.md
-.. _`test_diff_codestyle.py`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_diff_codestyle.py
+.. _`test_codestyle.py`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_codestyle.py
.. _`JetBrains`: https://www.jetbrains.com/
+.. _`GitHub`: https://www.github.com/
.. _`Sponsor @AtsushiSakai on GitHub Sponsors`: https://github.com/sponsors/AtsushiSakai
.. _`Become a backer or sponsor on Patreon`: https://www.patreon.com/myenigma
-.. _`One-time donation via PayPal`: https://www.paypal.me/myenigmapay/
+.. _`One-time donation via PayPal`: https://www.paypal.com/paypalme/myenigmapay/
+.. _`1Password`: https://github.com/1Password/for-open-source
+.. _`matplotrecorder`: https://github.com/AtsushiSakai/matplotrecorder
+.. _`PythonRoboticsGifs`: https://github.com/AtsushiSakai/PythonRoboticsGifs
+.. _`autodoc`: https://www.sphinx-doc.org/en/master/usage/extensions/autodoc.html
diff --git a/docs/modules/0_getting_started/4_how_to_read_textbook_main.rst b/docs/modules/0_getting_started/4_how_to_read_textbook_main.rst
new file mode 100644
index 0000000000..1625c838af
--- /dev/null
+++ b/docs/modules/0_getting_started/4_how_to_read_textbook_main.rst
@@ -0,0 +1,14 @@
+.. _`How to read this textbook`:
+
+How to read this textbook
+--------------------------
+
+This document is structured to help you learn the fundamental concepts
+behind each sample code in PythonRobotics.
+
+If you already have some knowledge of robotics technologies, you can start
+by reading any document that interests you.
+
+However, if you have no prior knowledge of robotics technologies, it is
+recommended that you first read the :ref:`Introduction` section and then proceed
+to the documents related to the technical fields that interest you.
\ No newline at end of file
diff --git a/docs/modules/control/inverted_pendulum_control/inverted-pendulum.png b/docs/modules/10_inverted_pendulum/inverted-pendulum.png
similarity index 100%
rename from docs/modules/control/inverted_pendulum_control/inverted-pendulum.png
rename to docs/modules/10_inverted_pendulum/inverted-pendulum.png
diff --git a/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control_main.rst b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
similarity index 92%
rename from docs/modules/control/inverted_pendulum_control/inverted_pendulum_control_main.rst
rename to docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
index e41729fd61..58dc0f2e57 100644
--- a/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control_main.rst
+++ b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
@@ -1,5 +1,7 @@
-Inverted Pendulum Control
------------------------------
+.. _`Inverted Pendulum`:
+
+Inverted Pendulum
+------------------
An inverted pendulum on a cart consists of a mass :math:`m` at the top of a pole of length :math:`l` pivoted on a
horizontally moving base as shown in the adjacent.
@@ -87,6 +89,12 @@ and :math:`P` is the unique positive definite solution to the discrete time
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation_lqr.gif
+Code Link
+^^^^^^^^^^^
+
+.. autofunction:: InvertedPendulum.inverted_pendulum_lqr_control.main
+
+
MPC control
~~~~~~~~~~~~~~~~~~~~~~~~~~~
The MPC controller minimize this cost function defined as:
@@ -99,3 +107,9 @@ subject to:
- Initial state
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation.gif
+
+Code Link
+^^^^^^^^^^^
+
+.. autofunction:: InvertedPendulum.inverted_pendulum_mpc_control.main
+
diff --git a/docs/modules/11_utils/plot/curvature_plot.png b/docs/modules/11_utils/plot/curvature_plot.png
new file mode 100644
index 0000000000..891d300829
Binary files /dev/null and b/docs/modules/11_utils/plot/curvature_plot.png differ
diff --git a/docs/modules/11_utils/plot/plot_main.rst b/docs/modules/11_utils/plot/plot_main.rst
new file mode 100644
index 0000000000..b5fef0c4a1
--- /dev/null
+++ b/docs/modules/11_utils/plot/plot_main.rst
@@ -0,0 +1,16 @@
+.. _plot_utils:
+
+Plotting Utilities
+-------------------
+
+This module contains a number of utility functions for plotting data.
+
+.. _plot_curvature:
+
+plot_curvature
+~~~~~~~~~~~~~~~
+
+.. autofunction:: utils.plot.plot_curvature
+
+.. image:: curvature_plot.png
+
diff --git a/docs/modules/11_utils/utils_main.rst b/docs/modules/11_utils/utils_main.rst
new file mode 100644
index 0000000000..95c982b077
--- /dev/null
+++ b/docs/modules/11_utils/utils_main.rst
@@ -0,0 +1,12 @@
+.. _`utils`:
+
+Utilities
+==========
+
+Common utilities for PythonRobotics.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ plot/plot
diff --git a/docs/modules/appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png b/docs/modules/12_appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_2_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
similarity index 99%
rename from docs/modules/appendix/Kalmanfilter_basics_2_main.rst
rename to docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
index 9ae6fc5bcb..b7ff84e6f6 100644
--- a/docs/modules/appendix/Kalmanfilter_basics_2_main.rst
+++ b/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
@@ -331,7 +331,7 @@ are vectors and matrices, but the concepts are exactly the same:
- Use the process model to predict the next state (the prior)
- Form an estimate part way between the measurement and the prior
-References:
+Reference
~~~~~~~~~~~
1. Roger Labbe’s
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst
similarity index 99%
rename from docs/modules/appendix/Kalmanfilter_basics_main.rst
rename to docs/modules/12_appendix/Kalmanfilter_basics_main.rst
index 6548377e07..a1d99d47ef 100644
--- a/docs/modules/appendix/Kalmanfilter_basics_main.rst
+++ b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst
@@ -552,7 +552,7 @@ a given (X,Y) value.
.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
-References:
+Reference
~~~~~~~~~~~
1. Roger Labbe’s
diff --git a/docs/modules/appendix/appendix_main.rst b/docs/modules/12_appendix/appendix_main.rst
similarity index 61%
rename from docs/modules/appendix/appendix_main.rst
rename to docs/modules/12_appendix/appendix_main.rst
index 8a29d84676..d0b9eeea3a 100644
--- a/docs/modules/appendix/appendix_main.rst
+++ b/docs/modules/12_appendix/appendix_main.rst
@@ -1,4 +1,4 @@
-.. _appendix:
+.. _`Appendix`:
Appendix
==============
@@ -7,6 +7,9 @@ Appendix
:maxdepth: 2
:caption: Contents
+ steering_motion_model
Kalmanfilter_basics
Kalmanfilter_basics_2
+ internal_sensors
+ external_sensors
diff --git a/docs/modules/12_appendix/external_sensors_main.rst b/docs/modules/12_appendix/external_sensors_main.rst
new file mode 100644
index 0000000000..b7caaf2c3a
--- /dev/null
+++ b/docs/modules/12_appendix/external_sensors_main.rst
@@ -0,0 +1,65 @@
+.. _`External Sensors for Robots`:
+
+External Sensors for Robots
+============================
+
+This project, `PythonRobotics`, focuses on algorithms, so hardware is not included.
+However, having basic knowledge of hardware in robotics is also important for understanding algorithms.
+Therefore, we will provide an overview.
+
+Introduction
+------------
+
+In recent years, the application of robotic technology has advanced, particularly in areas such as autonomous vehicles and disaster response robots. A crucial element in these technologies is external recognition—the robot's ability to understand its surrounding environment, identify safe zones, and detect moving objects using onboard sensors. Achieving effective external recognition involves various techniques, but equally important is the selection of appropriate sensors. Robots, like the sensors they employ, come in many forms, but external recognition sensors can be broadly categorized into three types. Developing an advanced external recognition system requires a thorough understanding of each sensor's principles and characteristics to determine their optimal application. This article summarizes the principles and features of these sensors for personal study purposes.
+
+Laser Sensors
+-------------
+
+Laser sensors measure distances by utilizing light, commonly referred to as Light Detection and Ranging (LIDAR). They operate by emitting light towards an object and calculating the distance based on the time it takes for the reflected light to return, using the speed of light as a constant.
+
+Radar Sensors
+-------------
+
+Radar measures distances using radio waves, commonly referred to as Radio Detection and Ranging (RADAR). It operates by transmitting radio signals towards an object and calculating the distance based on the time it takes for the reflected waves to return, using the speed of radio waves as a constant.
+
+
+Monocular Cameras
+-----------------
+
+Monocular cameras utilize a single camera to recognize the external environment. Compared to other sensors, they can detect color and brightness information, making them primarily useful for object recognition. However, they face challenges in independently measuring distances to surrounding objects and may struggle in low-light or dark conditions.
+
+Requirements for Cameras and Image Processing in Robotics
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+While camera sensors are widely used in applications like surveillance, deploying them in robotics necessitates meeting specific requirements:
+
+1. High dynamic range to adapt to various lighting conditions
+2. Wide measurement range
+3. Capability for three-dimensional measurement through techniques like motion stereo
+4. Real-time processing with high frame rates
+5. Cost-effectiveness
+
+Stereo Cameras
+--------------
+
+Stereo cameras employ multiple cameras to measure distances to surrounding objects. By knowing the positions and orientations of each camera and analyzing the disparity in the images (parallax), the distance to a specific point (the object represented by a particular pixel) can be calculated.
+
+Characteristics of Stereo Cameras
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Advantages of stereo cameras include the ability to obtain high-precision and high-density distance information at close range, depending on factors like camera resolution and the distance between cameras (baseline). This makes them suitable for indoor robots that require precise shape recognition of nearby objects. Additionally, stereo cameras are relatively cost-effective compared to other sensors, leading to their use in consumer products like Subaru's EyeSight system. However, stereo cameras are less effective for long-distance measurements due to a decrease in accuracy proportional to the square of the distance. They are also susceptible to environmental factors such as lighting conditions.
+
+Ultrasonic Sensors
+------------------
+
+Ultrasonic sensors are commonly used in indoor robots and some automotive autonomous driving systems. Their features include affordability compared to laser or radar sensors, the ability to detect very close objects, and the capability to sense materials like glass, which may be challenging for lasers or cameras. However, they have limitations such as shorter maximum measurement distances and lower resolution and accuracy.
+
+References
+----------
+
+- Wikipedia articles:
+
+ - `Lidar Sensors `_
+ - `Radar Sensors `_
+ - `Stereo Cameras `_
+ - `Ultrasonic Sensors `_
\ No newline at end of file
diff --git a/docs/modules/12_appendix/internal_sensors_main.rst b/docs/modules/12_appendix/internal_sensors_main.rst
new file mode 100644
index 0000000000..fa2594a2bf
--- /dev/null
+++ b/docs/modules/12_appendix/internal_sensors_main.rst
@@ -0,0 +1,61 @@
+.. _`Internal Sensors for Robots`:
+
+Internal Sensors for Robots
+============================
+
+This project, `PythonRobotics`, focuses on algorithms, so hardware is not included. However, having basic knowledge of hardware in robotics is also important for understanding algorithms. Therefore, we will provide an overview.
+
+Introduction
+-------------
+
+In robotic systems, internal sensors play a crucial role in monitoring the robot’s internal state, such as orientation, acceleration, angular velocity, altitude, and temperature. These sensors provide essential feedback that supports control, localization, and safety mechanisms. While external sensors perceive the environment, internal sensors give the robot self-awareness of its own motion and condition. Understanding the principles and characteristics of these sensors is vital to fully utilize their information within algorithms and decision-making systems. This section outlines the main internal sensors used in robotics.
+
+Global Navigation Satellite System (GNSS)
+-----------------------------------------
+
+GNSS is a satellite-based navigation system that provides global positioning and time information by analyzing signals from multiple satellites. It is commonly used in outdoor environments for absolute positioning. Although GNSS offers global coverage without infrastructure dependency, its performance is limited indoors or in obstructed areas, and it suffers from low update rates and susceptibility to signal noise. It is widely used in outdoor navigation for drones, vehicles, and delivery robots.
+
+Gyroscope
+----------
+
+A gyroscope measures angular velocity around the robot’s axes, enabling orientation and attitude estimation through detection of the Coriolis effect. Gyroscopes are compact, cost-effective, and provide high update rates, but they are prone to drift and require calibration or sensor fusion for long-term accuracy. These sensors are essential in drones, balancing robots, and IMU-based systems for motion tracking.
+
+Accelerometer
+---------------
+
+An accelerometer measures linear acceleration along one or more axes, typically by detecting mass displacement due to motion. It is small, affordable, and useful for detecting movement, tilt, or vibration. However, accelerometers are limited by noisy output and cannot independently determine position without integration and fusion. They are commonly applied in wearable robotics, step counters, and vibration sensing.
+
+Magnetometer
+--------------
+
+A magnetometer measures the direction and intensity of magnetic fields, enabling heading estimation based on Earth’s magnetic field. It is lightweight and useful for orientation, especially in GPS-denied environments, though it is sensitive to interference from electronics and requires calibration. Magnetometers are often used in conjunction with IMUs for navigation and directional awareness.
+
+Inertial Measurement Unit (IMU)
+--------------------------------
+
+An IMU integrates a gyroscope, accelerometer, and sometimes a magnetometer to estimate a robot's motion and orientation through sensor fusion techniques such as Kalman filters. IMUs are compact and provide high-frequency data, which is essential for localization and navigation in GPS-denied areas. Nonetheless, they accumulate drift over time and require complex filtering to maintain accuracy. IMUs are found in drones, mobile robots, and motion tracking systems.
+
+Pressure Sensor
+----------------
+
+Pressure sensors detect atmospheric or fluid pressure by measuring the force exerted on a diaphragm. They are effective for estimating altitude and monitoring environmental conditions, especially in drones and underwater robots. Although cost-effective and efficient, their accuracy may degrade due to temperature variation and limitations in low-altitude resolution.
+
+Temperature Sensor
+--------------------
+
+Temperature sensors monitor environmental or internal component temperatures, using changes in resistance or voltage depending on sensor type (e.g., RTD or thermocouple). They are simple and reliable for safety and thermal regulation, though they may respond slowly and be affected by nearby electronics. Common applications include battery and motor monitoring, safety systems, and ambient sensing.
+
+References
+----------
+
+- *Introduction to Autonomous Mobile Robots*, Roland Siegwart, Illah Nourbakhsh, Davide Scaramuzza
+- *Probabilistic Robotics*, Sebastian Thrun, Wolfram Burgard, Dieter Fox
+- Wikipedia articles:
+
+ - `Inertial Measurement Unit (IMU) `_
+ - `Accelerometer `_
+ - `Gyroscope `_
+ - `Magnetometer `_
+ - `Pressure sensor `_
+ - `Temperature sensor `_
+- `Adafruit Sensor Guides `_
\ No newline at end of file
diff --git a/docs/modules/12_appendix/steering_motion_model/steering_model.png b/docs/modules/12_appendix/steering_motion_model/steering_model.png
new file mode 100644
index 0000000000..c66dded87a
Binary files /dev/null and b/docs/modules/12_appendix/steering_motion_model/steering_model.png differ
diff --git a/docs/modules/12_appendix/steering_motion_model/turning_radius_calc1.png b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc1.png
new file mode 100644
index 0000000000..3de7ed8797
Binary files /dev/null and b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc1.png differ
diff --git a/docs/modules/12_appendix/steering_motion_model/turning_radius_calc2.png b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc2.png
new file mode 100644
index 0000000000..f7e776bf40
Binary files /dev/null and b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc2.png differ
diff --git a/docs/modules/12_appendix/steering_motion_model_main.rst b/docs/modules/12_appendix/steering_motion_model_main.rst
new file mode 100644
index 0000000000..c697123fa2
--- /dev/null
+++ b/docs/modules/12_appendix/steering_motion_model_main.rst
@@ -0,0 +1,97 @@
+
+Steering Motion Model
+-----------------------
+
+Turning radius calculation by steering motion model
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The turning Radius represents the radius of the circle when the robot turns, as shown in the diagram below.
+
+.. image:: steering_motion_model/steering_model.png
+
+When the steering angle is tilted by :math:`\delta`,
+the turning radius :math:`R` can be calculated using the following equation,
+based on the geometric relationship between the wheelbase (WB),
+which is the distance between the rear wheel center and the front wheel center,
+and the assumption that the turning radius circle passes through the center of
+the rear wheels in the diagram above.
+
+:math:`R = \frac{WB}{tan\delta}`
+
+The curvature :math:`\kappa` is the reciprocal of the turning radius:
+
+:math:`\kappa = \frac{tan\delta}{WB}`
+
+In the diagram above, the angular difference :math:`\Delta \theta` in the vehicle’s heading between two points on the turning radius :math:`R`
+is the same as the angle of the vector connecting the two points from the center of the turn.
+
+From the formula for the length of an arc and the radius,
+
+:math:`\Delta \theta = \frac{s}{R}`
+
+Here, :math:`s` is the distance between two points on the turning radius.
+
+So, yaw rate :math:`\omega` can be calculated as follows.
+
+:math:`\omega = \frac{v}{R}`
+
+and
+
+:math:`\omega = v\kappa`
+
+here, :math:`v` is the velocity of the vehicle.
+
+
+Turning radius calculation by 2 consecutive positions of the robot trajectory
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+In this section, we will derive the formula for the turning radius from 2 consecutive positions of the robot trajectory.
+
+.. image:: steering_motion_model/turning_radius_calc1.png
+
+As shown in the upper diagram above, the robot moves from a point at time :math:`t` to a point at time :math:`t+1`.
+Each point is represented by a 2D position :math:`(x_t, y_t)` and an orientation :math:`\theta_t`.
+
+The distance between the two points is :math:`d = \sqrt{(x_{t+1} - x_t)^2 + (y_{t+1} - y_t)^2}`.
+
+The angle between the two vectors from the turning center to the two points is :math:`\theta = \theta_{t+1} - \theta_t`.
+Here, by drawing a perpendicular line from the center of the turning radius
+to a straight line of length :math:`d` connecting two points,
+the following equation can be derived from the resulting right triangle.
+
+:math:`sin\frac{\theta}{2} = \frac{d}{2R}`
+
+So, the turning radius :math:`R` can be calculated as follows.
+
+:math:`R = \frac{d}{2sin\frac{\theta}{2}}`
+
+The curvature :math:`\kappa` is the reciprocal of the turning radius.
+So, the curvature can be calculated as follows.
+
+:math:`\kappa = \frac{2sin\frac{\theta}{2}}{d}`
+
+Target speed by maximum steering speed
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+If the maximum steering speed is given as :math:`\dot{\delta}_{max}`,
+the maximum curvature change rate :math:`\dot{\kappa}_{max}` can be calculated as follows:
+
+:math:`\dot{\kappa}_{max} = \frac{tan\dot{\delta}_{max}}{WB}`
+
+From the curvature calculation by 2 consecutive positions of the robot trajectory,
+
+the maximum curvature change rate :math:`\dot{\kappa}_{max}` can be calculated as follows:
+
+:math:`\dot{\kappa}_{max} = \frac{\kappa_{t+1}-\kappa_{t}}{\Delta t}`
+
+If we can assume that the vehicle will not exceed the maximum curvature change rate,
+
+the target minimum velocity :math:`v_{min}` can be calculated as follows:
+
+:math:`v_{min} = \frac{d_{t+1}+d_{t}}{\Delta t} = \frac{d_{t+1}+d_{t}}{(\kappa_{t+1}-\kappa_{t})}\frac{tan\dot{\delta}_{max}}{WB}`
+
+
+Reference
+~~~~~~~~~~~
+
+- `Vehicle Dynamics and Control `_
diff --git a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
new file mode 100644
index 0000000000..22849f7c54
--- /dev/null
+++ b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
@@ -0,0 +1,104 @@
+Behavior Tree
+-------------
+
+Behavior Tree is a modular, hierarchical decision model that is widely used in robot control, and game development.
+It present some similarities to hierarchical state machines with the key difference that the main building block of a behavior is a task rather than a state.
+Behavior Tree have been shown to generalize several other control architectures (https://ieeexplore.ieee.org/document/7790863)
+
+Code Link
+~~~~~~~~~~~~~
+
+Control Node
+++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ControlNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SequenceNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SelectorNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.WhileDoElseNode
+
+Action Node
+++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ActionNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.EchoNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SleepNode
+
+Decorator Node
+++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.DecoratorNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.InverterNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.TimeoutNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.DelayNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ForceSuccessNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ForceFailureNode
+
+Behavior Tree Factory
++++++++++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.BehaviorTreeFactory
+ :members:
+
+Behavior Tree
++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.BehaviorTree
+ :members:
+
+Example
+~~~~~~~
+
+Visualize the behavior tree by `xml-tree-visual `_.
+
+.. image:: ./robot_behavior_case.svg
+
+Print the behavior tree
+
+.. code-block:: text
+
+ Behavior Tree
+ [Robot Main Controller]
+ [Battery Management]
+ (Low Battery Detection)
+
+
+
+ [Patrol Task]
+
+ [Move to Position A]
+
+ [Obstacle Handling A]
+ [Obstacle Present]
+
+
+
+
+ [Move to Position B]
+ (Short Wait)
+
+
+ (Limited Time Obstacle Handling)
+ [Obstacle Present]
+
+
+
+ [Conditional Move to C]
+
+ [Perform Position C Task]
+
+ (Ensure Completion)
+
+
+
+
+ Behavior Tree
diff --git a/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg b/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
new file mode 100644
index 0000000000..a3d43aed52
--- /dev/null
+++ b/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
@@ -0,0 +1,22 @@
+
\ No newline at end of file
diff --git a/docs/modules/13_mission_planning/mission_planning_main.rst b/docs/modules/13_mission_planning/mission_planning_main.rst
new file mode 100644
index 0000000000..c35eacd8d5
--- /dev/null
+++ b/docs/modules/13_mission_planning/mission_planning_main.rst
@@ -0,0 +1,13 @@
+.. _`Mission Planning`:
+
+Mission Planning
+================
+
+Mission planning includes tools such as finite state machines and behavior trees used to describe robot behavior and high level task planning.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ state_machine/state_machine
+ behavior_tree/behavior_tree
diff --git a/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png b/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png
new file mode 100644
index 0000000000..fbc1369cbc
Binary files /dev/null and b/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png differ
diff --git a/docs/modules/13_mission_planning/state_machine/state_machine_main.rst b/docs/modules/13_mission_planning/state_machine/state_machine_main.rst
new file mode 100644
index 0000000000..3f516d46a9
--- /dev/null
+++ b/docs/modules/13_mission_planning/state_machine/state_machine_main.rst
@@ -0,0 +1,74 @@
+State Machine
+-------------
+
+A state machine is a model used to describe the transitions of an object between different states. It clearly shows how an object changes state based on events and may trigger corresponding actions.
+
+Core Concepts
+~~~~~~~~~~~~~
+
+- **State**: A distinct mode or condition of the system (e.g. "Idle", "Running"). Managed by State class with optional on_enter/on_exit callbacks
+- **Event**: A trigger signal that may cause state transitions (e.g. "start", "stop")
+- **Transition**: A state change path from source to destination state triggered by an event
+- **Action**: An operation executed during transition (before entering new state)
+- **Guard**: A precondition that must be satisfied to allow transition
+
+Code Link
+~~~~~~~~~~~
+
+.. autoclass:: MissionPlanning.StateMachine.state_machine.StateMachine
+ :members: add_transition, process, register_state
+ :special-members: __init__
+
+PlantUML Support
+~~~~~~~~~~~~~~~~
+
+The ``generate_plantuml()`` method creates diagrams showing:
+
+- Current state (marked with [*] arrow)
+- All possible transitions
+- Guard conditions in [brackets]
+- Actions prefixed with /
+
+Example
+~~~~~~~
+
+state machine diagram:
++++++++++++++++++++++++
+.. image:: robot_behavior_case.png
+
+state transition table:
++++++++++++++++++++++++
+.. list-table:: State Transitions
+ :header-rows: 1
+ :widths: 20 15 20 20 20
+
+ * - Source State
+ - Event
+ - Target State
+ - Guard
+ - Action
+ * - patrolling
+ - detect_task
+ - executing_task
+ -
+ -
+ * - executing_task
+ - task_complete
+ - patrolling
+ -
+ - reset_task
+ * - executing_task
+ - low_battery
+ - returning_to_base
+ - is_battery_low
+ -
+ * - returning_to_base
+ - reach_base
+ - charging
+ -
+ -
+ * - charging
+ - charge_complete
+ - patrolling
+ -
+ -
\ No newline at end of file
diff --git a/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst b/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst
new file mode 100644
index 0000000000..ca595301a6
--- /dev/null
+++ b/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst
@@ -0,0 +1,107 @@
+Definition of Robotics
+----------------------
+
+This section explains the definition, history, key components, and applications of robotics.
+
+What is Robotics?
+^^^^^^^^^^^^^^^^^^
+
+Robot is a machine that can perform tasks automatically or semi-autonomously.
+Robotics is the study of robots.
+
+The word “robot” comes from the Czech word “robota,” which means “forced labor” or “drudgery.”
+It was first used in the 1920 science fiction play `R.U.R.`_ (Rossum’s Universal Robots)
+by the Czech writer `Karel Čapek`_.
+In the play, robots were artificial workers created to serve humans, but they eventually rebelled.
+
+Over time, “robot” came to refer to machines or automated systems that can perform tasks,
+often with some level of intelligence or autonomy.
+
+Currently, 2 millions robots are working in the world, and the number is increasing every year.
+In South Korea, where the adoption of robots is particularly rapid,
+50 robots are operating per 1,000 people.
+
+.. _`R.U.R.`: https://thereader.mitpress.mit.edu/origin-word-robot-rur/
+.. _`Karel Čapek`: https://en.wikipedia.org/wiki/Karel_%C4%8Capek
+
+The History of Robots
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This timeline highlights key milestones in the history of robotics:
+
+Ancient and Early Concepts (Before 1500s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The idea of **automated machines** has existed for thousands of years.
+Ancient civilizations imagined mechanical beings:
+
+- **Ancient Greece (4th Century BC)** – Greek engineer `Hero of Alexandria`_ designed early **automata** (self-operating machines) powered by water or air.
+- **Chinese and Arabic Automata (9th–13th Century)** – Inventors like `Ismail Al-Jazari`_ created intricate mechanical devices, including water clocks and automated moving peacocks driven by hydropower.
+
+.. _`Hero of Alexandria`: https://en.wikipedia.org/wiki/Hero_of_Alexandria
+.. _`Ismail Al-Jazari`: https://en.wikipedia.org/wiki/Ismail_al-Jazari
+
+The Birth of Modern Robotics (1500s–1800s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- `Leonardo da Vinci’s Robot`_ (1495) – Designed a humanoid knight with mechanical movement.
+- `Jacques de Vaucanson’s Digesting Duck`_ (1738) – Created robotic figures like a mechanical duck that could "eat" and "digest."
+- `Industrial Revolution`_ (18th–19th Century) – Machines began replacing human labor in factories, setting the foundation for automation.
+
+.. _`Leonardo da Vinci’s Robot`: https://en.wikipedia.org/wiki/Leonardo%27s_robot
+.. _`Jacques de Vaucanson’s Digesting Duck`: https://en.wikipedia.org/wiki/Jacques_de_Vaucanson
+.. _`Industrial Revolution`: https://en.wikipedia.org/wiki/Industrial_Revolution
+
+The Rise of Industrial Robots (1900s–1950s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **The Term “Robot” (1921)** – Czech writer `Karel Čapek`_ introduced the word *“robot”* in his play `R.U.R.`_ (Rossum’s Universal Robots).
+- **Early Cybernetics (1940s–1950s)** – Scientists like `Norbert Wiener`_ developed theories of self-regulating machines, influencing modern robotics (Cybernetics).
+
+.. _`Norbert Wiener`: https://en.wikipedia.org/wiki/Norbert_Wiener
+
+The Birth of Modern Robotics (1950s–1980s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **First Industrial Robot (1961)** – `Unimate`_, created by `George Devol`_ and `Joseph Engelberger`_, was the first programmable robot used in a factory.
+- **Rise of AI & Autonomous Robots (1970s–1980s)** – Researchers developed mobile robots like `Shakey`_ (Stanford, 1966) and AI-based control systems.
+
+.. _`Unimate`: https://en.wikipedia.org/wiki/Unimate
+.. _`George Devol`: https://en.wikipedia.org/wiki/George_Devol
+.. _`Joseph Engelberger`: https://en.wikipedia.org/wiki/Joseph_Engelberger
+.. _`Shakey`: https://en.wikipedia.org/wiki/Shakey_the_robot
+
+Advanced Robotics and AI Integration (1990s–Present)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **Industrial Robots** – Advanced robots like `Baxter`_ and `Amazon Robotics`_ revolutionized manufacturing and logistics.
+- **Autonomous Vehicles** – Self-driving cars for robo taxi like `Waymo`_ and autonomous haulage system in mining like `AHS`_ became more advanced and bisiness-ready.
+- **Exploration Robots** – Used for planetary exploration (e.g., NASA’s `Mars rovers`_).
+- **Medical Robotics** – Robots like `da Vinci Surgical System`_ revolutionized healthcare.
+- **Personal Robots** – Devices like `Roomba`_ (vacuum robot) and `Sophia`_ (AI humanoid) became popular.
+- **Service Robots** - Assistive robots like serving robots in restaurants and hotels like `Bellabot`_.
+- **Collaborative Robots (Drones)** – Collaborative robots like UAV (Unmanned Aerial Vehicle) in drone shows and delivery services.
+
+.. _`Baxter`: https://en.wikipedia.org/wiki/Baxter_(robot)
+.. _`Amazon Robotics`: https://en.wikipedia.org/wiki/Amazon_Robotics
+.. _`Mars rovers`: https://en.wikipedia.org/wiki/Mars_rover
+.. _`Waymo`: https://waymo.com/
+.. _`AHS`: https://www.futurebridge.com/industry/perspectives-industrial-manufacturing/autonomous-haulage-systems-the-future-of-mining-operations/
+.. _`da Vinci Surgical System`: https://en.wikipedia.org/wiki/Da_Vinci_Surgical_System
+.. _`Roomba`: https://en.wikipedia.org/wiki/Roomba
+.. _`Sophia`: https://en.wikipedia.org/wiki/Sophia_(robot)
+.. _`Bellabot`: https://www.pudurobotics.com/en
+
+Key Components of Robotics
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Robotics consists of several essential components:
+
+#. Sensors – Gather information from the environment (e.g., Cameras, LiDAR, GNSS, Gyro, Accelerometer, Wheel encoders).
+#. Actuators – Enable movement and interaction with the world (e.g., Motors, Hydraulic systems).
+#. Computers – Process sensor data and make decisions (e.g., Micro-controllers, CPUs, GPUs).
+#. Power Supply – Provides energy to run the robot (e.g., Batteries, Solar power).
+#. Software & Algorithms – Allow the robot to function and make intelligent decisions (e.g., ROS, Machine learning models, Localization, Mapping, Path planning, Control).
+
+This project, `PythonRobotics`, focuses on the software and algorithms part of robotics.
+If you are interested in `Sensors` hardware, you can check :ref:`Internal Sensors for Robots` or :ref:`External Sensors for Robots`.
diff --git a/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst b/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst
new file mode 100644
index 0000000000..c47c122853
--- /dev/null
+++ b/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst
@@ -0,0 +1,135 @@
+Python for Robotics
+----------------------
+
+A programing language, Python is used for this `PythonRobotics` project
+to achieve the purposes of this project described in the :ref:`What is PythonRobotics?`.
+
+This section explains the Python itself and features for science computing and robotics.
+
+Python for general-purpose programming
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+`Python `_ is an general-purpose programming language developed by
+`Guido van Rossum `_ from the late 1980s.
+
+It features as follows:
+
+#. High-level
+#. Interpreted
+#. Dynamic type system (also type annotation is supported)
+#. Emphasizes code readability
+#. Rapid prototyping
+#. Batteries included
+#. Interoperability for C and Fortran
+
+Due to these features, Python is one of the most popular programming language
+for educational purposes for programming beginners.
+
+Python for Scientific Computing
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Python itself was not designed for scientific computing.
+However, scientists quickly recognized its strengths.
+For example,
+
+#. High-level and interpreted features enable scientists to focus on their problems without dealing with low-level programming tasks like memory management.
+#. Code readability, rapid prototyping, and batteries included features enables scientists who are not professional programmers, to solve their problems easily.
+#. The interoperability to wrap C and Fortran libraries enables scientists to access already existed powerful and optimized scientific computing libraries.
+
+To address the more needs of scientific computing, many fundamental scientific computation libraries have been developed based on the upper features.
+
+- `NumPy `_ is the fundamental package for scientific computing with Python.
+- `SciPy `_ is a library that builds on NumPy and provides a large number of functions that operate on NumPy arrays and are useful for different types of scientific and engineering applications.
+- `Matplotlib `_ is a plotting library for the Python programming language and its numerical mathematics extension NumPy.
+- `Pandas `_ is a fast, powerful, flexible, and easy-to-use open-source data analysis and data manipulation library built on top of NumPy.
+- `SymPy `_ is a Python library for symbolic mathematics.
+- `CVXPy `_ is a Python-embedded modeling language for convex optimization problems.
+
+Also, more domain-specific libraries have been developed based on these fundamental libraries:
+
+- `Scikit-learn `_ is a free software machine learning library for the Python programming language.
+- `Scikit-image `_ is a collection of algorithms for image processing.
+- `Networkx `_ is a package for the creation, manipulation for complex networks.
+- `SunPy `_ is a community-developed free and open-source software package for solar physics.
+- `Astropy `_ is a community-developed free and open-source software package for astronomy.
+
+Currently, Python is one of the most popular programming languages for scientific computing.
+
+Python for Robotics
+^^^^^^^^^^^^^^^^^^^^
+
+Python has become an increasingly popular language in robotics.
+
+These are advantages of Python for Robotics:
+
+Simplicity and Readability
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python's syntax is clear and concise, making it easier to learn and write code.
+This is crucial in robotics where complex algorithms and control logic are involved.
+
+
+Extensive libraries for scientific computation.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Scientific computation routine are fundamental for robotics.
+For example:
+
+- Matrix operation is needed for rigid body transformation, state estimation, and model based control.
+- Optimization is needed for optimization based SLAM, optimal path planning, and optimal control.
+- Visualization is needed for robot teleoperation, debugging, and simulation.
+
+ROS supports Python
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+`ROS`_ (Robot Operating System) is an open-source and widely used framework for robotics development.
+It is designed to help developping complicated robotic applications.
+ROS provides essential tools, libraries, and drivers to simplify robot programming and integration.
+
+Key Features of ROS:
+
+- Modular Architecture – Uses a node-based system where different components (nodes) communicate via messages.
+- Hardware Abstraction – Supports various robots, sensors, and actuators, making development more flexible.
+- Powerful Communication System – Uses topics, services, and actions for efficient data exchange between components.
+- Rich Ecosystem – Offers many pre-built packages for navigation, perception, and manipulation.
+- Multi-language Support – Primarily uses Python and C++, but also supports other languages.
+- Simulation & Visualization – Tools like Gazebo (for simulation) and RViz (for visualization) aid in development and testing.
+- Scalability & Community Support – Widely used in academia and industry, with a large open-source community.
+
+ROS has strong Python support (`rospy`_ for ROS1 and `rclpy`_ for ROS2).
+This allows developers to easily create nodes, manage communication between
+different parts of a robot system, and utilize various ROS tools.
+
+.. _`ROS`: https://www.ros.org/
+.. _`rospy`: http://wiki.ros.org/rospy
+.. _`rclpy`: https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html
+
+Cross-Platform Compatibility
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python code can run on various operating systems (Windows, macOS, Linux), providing flexibility in choosing hardware platforms for robotics projects.
+
+Large Community and Support
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python has a vast and active community, offering ample resources, tutorials, and support for developers. This is invaluable when tackling challenges in robotics development.
+
+Situations which Python is NOT suitable for Robotics
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+We explained the advantages of Python for robotics.
+However, Python is not always the best choice for robotics development.
+
+These are situations where Python is NOT suitable for robotics:
+
+High-speed real-time control
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python is an interpreted language, which means it is slower than compiled languages like C++.
+This can be a disadvantage when real-time control is required,
+such as in high-speed motion control or safety-critical systems.
+
+So, for these applications, we recommend to understand the each algorithm you
+needed using this project and implement it in other suitable languages like C++.
+
+Resource-constrained systems
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python is a high-level language that requires more memory and processing power
+compared to low-level languages.
+So, it is difficult to run Python on resource-constrained systems like
+microcontrollers or embedded devices.
+In such cases, C or C++ is more suitable for these applications.
diff --git a/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst
new file mode 100644
index 0000000000..0ed51e961b
--- /dev/null
+++ b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst
@@ -0,0 +1,68 @@
+Technologies for Robotics
+-------------------------
+
+The field of robotics needs wide areas of technologies such as mechanical engineering,
+electrical engineering, computer science, and artificial intelligence (AI).
+This project, `PythonRobotics`, only focus on computer science and artificial intelligence.
+
+The technologies for robotics are categorized as following 3 categories:
+
+#. `Autonomous Navigation`_
+#. `Manipulation`_
+#. `Robot type specific technologies`_
+
+.. _`Autonomous Navigation`:
+
+Autonomous Navigation
+^^^^^^^^^^^^^^^^^^^^^^^^
+Autonomous navigation is a capability that can move to a goal over long
+periods of time without any external control by an operator.
+
+To achieve autonomous navigation, the robot needs to have the following technologies:
+- It needs to know where it is (localization)
+- Where it is safe (mapping)
+- Where is is safe and where the robot is in the map (Simultaneous Localization and Mapping (SLAM))
+- Where and how to move (path planning)
+- How to control its motion (path following).
+
+The autonomous system would not work correctly if any of these technologies is missing.
+
+In recent years, autonomous navigation technologies have received huge
+attention in many fields.
+For example, self-driving cars, drones, and autonomous mobile robots in indoor and outdoor environments.
+
+In this project, we provide many algorithms, sample codes,
+and documentations for autonomous navigation.
+
+#. :ref:`Localization`
+#. :ref:`Mapping`
+#. :ref:`SLAM`
+#. :ref:`Path planning`
+#. :ref:`Path tracking`
+
+
+
+.. _`Manipulation`:
+
+Manipulation
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+#. :ref:`Arm Navigation`
+
+.. _`Robot type specific technologies`:
+
+Robot type specific technologies
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+#. :ref:`Aerial Navigation`
+#. :ref:`Bipedal`
+#. :ref:`Inverted Pendulum`
+
+
+Additional Information
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+#. :ref:`utils`
+#. :ref:`Appendix`
+
+
diff --git a/docs/modules/1_introduction/introduction_main.rst b/docs/modules/1_introduction/introduction_main.rst
new file mode 100644
index 0000000000..1871dfc3b1
--- /dev/null
+++ b/docs/modules/1_introduction/introduction_main.rst
@@ -0,0 +1,18 @@
+.. _Introduction:
+
+Introduction
+============
+
+PythonRobotics is composed of two words: "Python" and "Robotics".
+Therefore, I will first explain these two topics, Robotics and Python.
+After that, I will provide an overview of the robotics technologies
+covered in PythonRobotics.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Table of Contents
+
+ 1_definition_of_robotics/definition_of_robotics
+ 2_python_for_robotics/python_for_robotics
+ 3_technologies_for_robotics/technologies_for_robotics
+
diff --git a/docs/modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst b/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
similarity index 65%
rename from docs/modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
rename to docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
index 2543d1186a..214e645d10 100644
--- a/docs/modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
+++ b/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
@@ -5,3 +5,8 @@ Ensamble Kalman Filter Localization
This is a sensor fusion localization with Ensamble Kalman Filter(EnKF).
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Localization.ensemble_kalman_filter.ensemble_kalman_filter.enkf_localization
+
diff --git a/docs/modules/2_localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png b/docs/modules/2_localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png
new file mode 100644
index 0000000000..595b651bd2
Binary files /dev/null and b/docs/modules/2_localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png differ
diff --git a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png
similarity index 100%
rename from docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png
rename to docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png
diff --git a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
similarity index 51%
rename from docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
rename to docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
index 0ec920c9df..adb41e5e40 100644
--- a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
+++ b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
@@ -2,6 +2,9 @@
Extended Kalman Filter Localization
-----------------------------------
+Position Estimation Kalman Filter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
.. image:: extended_kalman_filter_localization_1_0.png
:width: 600px
@@ -9,6 +12,7 @@ Extended Kalman Filter Localization
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif
+
This is a sensor fusion localization with Extended Kalman Filter(EKF).
The blue line is true trajectory, the black line is dead reckoning
@@ -19,11 +23,40 @@ is estimated trajectory with EKF.
The red ellipse is estimated covariance ellipse with EKF.
-Code: `PythonRobotics/extended_kalman_filter.py at master ·
-AtsushiSakai/PythonRobotics `__
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.extended_kalman_filter.extended_kalman_filter.ekf_estimation
+
+Extended Kalman Filter algorithm
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Localization process using Extended Kalman Filter:EKF is
+
+=== Predict ===
+
+:math:`x_{Pred} = Fx_t+Bu_t`
+
+:math:`P_{Pred} = J_f P_t J_f^T + Q`
+
+=== Update ===
+
+:math:`z_{Pred} = Hx_{Pred}`
+
+:math:`y = z - z_{Pred}`
+
+:math:`S = J_g P_{Pred}.J_g^T + R`
+
+:math:`K = P_{Pred}.J_g^T S^{-1}`
+
+:math:`x_{t+1} = x_{Pred} + Ky`
+
+:math:`P_{t+1} = ( I - K J_g) P_{Pred}`
+
+
Filter design
-~~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In this simulation, the robot has a state vector includes 4 states at
time :math:`t`.
@@ -61,8 +94,9 @@ In the code, “observation” function generates the input and observation
vector with noise
`code `__
+
Motion Model
-~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~
The robot model is
@@ -100,7 +134,7 @@ Its Jacobian matrix is
Observation Model
~~~~~~~~~~~~~~~~~
-The robot can get x-y position infomation from GPS.
+The robot can get x-y position information from GPS.
So GPS Observation model is
@@ -120,32 +154,99 @@ Its Jacobian matrix is
:math:`\begin{equation*} = \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}`
-Extended Kalman Filter
-~~~~~~~~~~~~~~~~~~~~~~
-Localization process using Extended Kalman Filter:EKF is
+Kalman Filter with Speed Scale Factor Correction
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+This is a Extended kalman filter (EKF) localization with velocity correction.
-=== Predict ===
+This is for correcting the vehicle speed measured with scale factor errors due to factors such as wheel wear.
-:math:`x_{Pred} = Fx_t+Bu_t`
-:math:`P_{Pred} = J_f P_t J_f^T + Q`
+In this simulation, the robot has a state vector includes 5 states at
+time :math:`t`.
-=== Update ===
+.. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t, s_t]
-:math:`z_{Pred} = Hx_{Pred}`
+x, y are a 2D x-y position, :math:`\phi` is orientation, v is
+velocity, and s is a scale factor of velocity.
-:math:`y = z - z_{Pred}`
+In the code, “xEst” means the state vector.
+`code `__
-:math:`S = J_g P_{Pred}.J_g^T + R`
+The rest is the same as the Position Estimation Kalman Filter.
-:math:`K = P_{Pred}.J_g^T S^{-1}`
+.. image:: ekf_with_velocity_correction_1_0.png
+ :width: 600px
+
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.extended_kalman_filter.ekf_with_velocity_correction.ekf_estimation
+
+
+Motion Model
+~~~~~~~~~~~~
+
+The robot model is
+
+.. math:: \dot{x} = v s \cos(\phi)
+
+.. math:: \dot{y} = v s \sin(\phi)
+
+.. math:: \dot{\phi} = \omega
+
+So, the motion model is
+
+.. math:: \textbf{x}_{t+1} = f(\textbf{x}_t, \textbf{u}_t) = F\textbf{x}_t+B\textbf{u}_t
+
+where
+
+:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} B= \begin{bmatrix} cos(\phi) \Delta t s & 0\\ sin(\phi) \Delta t s & 0\\ 0 & \Delta t\\ 1 & 0\\ 0 & 0\\ \end{bmatrix} \end{equation*}`
+
+:math:`\Delta t` is a time interval.
+
+This is implemented at
+`code `__
+
+The motion function is that
+
+:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \\ w' \\ v' \end{bmatrix} = f(\textbf{x}, \textbf{u}) = \begin{bmatrix} x + v s \cos(\phi)\Delta t \\ y + v s \sin(\phi)\Delta t \\ \phi + \omega \Delta t \\ v \end{bmatrix} \end{equation*}`
+
+Its Jacobian matrix is
+
+:math:`\begin{equation*} J_f = \begin{bmatrix} \frac{\partial x'}{\partial x}& \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v} & \frac{\partial x'}{\partial s}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{\partial v} & \frac{\partial y'}{\partial s}\\ \frac{\partial \phi'}{\partial x}& \frac{\partial \phi'}{\partial y} & \frac{\partial \phi'}{\partial \phi} & \frac{\partial \phi'}{\partial v} & \frac{\partial \phi'}{\partial s}\\ \frac{\partial v'}{\partial x}& \frac{\partial v'}{\partial y} & \frac{\partial v'}{\partial \phi} & \frac{\partial v'}{\partial v} & \frac{\partial v'}{\partial s} \\ \frac{\partial s'}{\partial x}& \frac{\partial s'}{\partial y} & \frac{\partial s'}{\partial \phi} & \frac{\partial s'}{\partial v} & \frac{\partial s'}{\partial s} \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} = \begin{bmatrix} 1& 0 & -v s \sin(\phi) \Delta t & s \cos(\phi) \Delta t & \cos(\phi) v \Delta t\\ 0 & 1 & v s \cos(\phi) \Delta t & s \sin(\phi) \Delta t & v \sin(\phi) \Delta t\\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \end{bmatrix} \end{equation*}`
+
+
+Observation Model
+~~~~~~~~~~~~~~~~~
+
+The robot can get x-y position information from GPS.
+
+So GPS Observation model is
+
+.. math:: \textbf{z}_{t} = g(\textbf{x}_t) = H \textbf{x}_t
+
+where
+
+:math:`\begin{equation*} H = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}`
+
+The observation function states that
+
+:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \end{bmatrix} = g(\textbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}`
+
+Its Jacobian matrix is
+
+:math:`\begin{equation*} J_g = \begin{bmatrix} \frac{\partial x'}{\partial x} & \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v} & \frac{\partial x'}{\partial s}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{ \partial v} & \frac{\partial y'}{ \partial s}\\ \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} = \begin{bmatrix} 1& 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ \end{bmatrix} \end{equation*}`
-:math:`x_{t+1} = x_{Pred} + Ky`
-:math:`P_{t+1} = ( I - K J_g) P_{Pred}`
-Ref:
-~~~~
+Reference
+^^^^^^^^^^^
- `PROBABILISTIC-ROBOTICS.ORG `__
diff --git a/docs/modules/localization/histogram_filter_localization/1.png b/docs/modules/2_localization/histogram_filter_localization/1.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/1.png
rename to docs/modules/2_localization/histogram_filter_localization/1.png
diff --git a/docs/modules/localization/histogram_filter_localization/2.png b/docs/modules/2_localization/histogram_filter_localization/2.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/2.png
rename to docs/modules/2_localization/histogram_filter_localization/2.png
diff --git a/docs/modules/localization/histogram_filter_localization/3.png b/docs/modules/2_localization/histogram_filter_localization/3.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/3.png
rename to docs/modules/2_localization/histogram_filter_localization/3.png
diff --git a/docs/modules/localization/histogram_filter_localization/4.png b/docs/modules/2_localization/histogram_filter_localization/4.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/4.png
rename to docs/modules/2_localization/histogram_filter_localization/4.png
diff --git a/docs/modules/localization/histogram_filter_localization/histogram_filter_localization_main.rst b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst
similarity index 96%
rename from docs/modules/localization/histogram_filter_localization/histogram_filter_localization_main.rst
rename to docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst
index fafd578333..3a175b1316 100644
--- a/docs/modules/localization/histogram_filter_localization/histogram_filter_localization_main.rst
+++ b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst
@@ -16,6 +16,11 @@ The filter uses speed input and range observations from RFID for localization.
Initial position information is not needed.
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.histogram_filter.histogram_filter.histogram_filter_localization
+
Filtering algorithm
~~~~~~~~~~~~~~~~~~~~
@@ -107,7 +112,7 @@ There are two ways to calculate the final positions:
-References:
+Reference
~~~~~~~~~~~
- `_PROBABILISTIC ROBOTICS: `_
diff --git a/docs/modules/localization/localization_main.rst b/docs/modules/2_localization/localization_main.rst
similarity index 55%
rename from docs/modules/localization/localization_main.rst
rename to docs/modules/2_localization/localization_main.rst
index db6651f6b8..770a234b69 100644
--- a/docs/modules/localization/localization_main.rst
+++ b/docs/modules/2_localization/localization_main.rst
@@ -1,7 +1,8 @@
-.. _localization:
+.. _`Localization`:
Localization
============
+Localization is the ability of a robot to know its position and orientation with sensors such as Global Navigation Satellite System:GNSS etc. In localization, Bayesian filters such as Kalman filters, histogram filter, and particle filter are widely used[31]. Fig.2 shows localization simulations using histogram filter and particle filter.
.. toctree::
:maxdepth: 2
diff --git a/docs/modules/localization/particle_filter_localization/particle_filter_localization_main.rst b/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst
similarity index 87%
rename from docs/modules/localization/particle_filter_localization/particle_filter_localization_main.rst
rename to docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst
index c8652ce8a7..d648d8e080 100644
--- a/docs/modules/localization/particle_filter_localization/particle_filter_localization_main.rst
+++ b/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst
@@ -15,6 +15,12 @@ It is assumed that the robot can measure a distance from landmarks
This measurements are used for PF localization.
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.particle_filter.particle_filter.pf_localization
+
+
How to calculate covariance matrix from particles
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -30,8 +36,8 @@ The covariance matrix :math:`\Xi` from particle information is calculated by the
- :math:`\mu_j` is the :math:`j` th mean state of particles.
-References:
+Reference
~~~~~~~~~~~
- `_PROBABILISTIC ROBOTICS: `_
-- `Improving the particle filter in high dimensions using conjugate artificial process noise `_
+- `Improving the particle filter in high dimensions using conjugate artificial process noise `_
diff --git a/docs/modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
similarity index 81%
rename from docs/modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
rename to docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
index bb6b5b664b..a7a5aab61b 100644
--- a/docs/modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
+++ b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
@@ -7,7 +7,13 @@ This is a sensor fusion localization with Unscented Kalman Filter(UKF).
The lines and points are same meaning of the EKF simulation.
-References:
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.unscented_kalman_filter.unscented_kalman_filter.ukf_estimation
+
+
+Reference
~~~~~~~~~~~
- `Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization `_
diff --git a/docs/modules/mapping/circle_fitting/circle_fitting_main.rst b/docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst
similarity index 78%
rename from docs/modules/mapping/circle_fitting/circle_fitting_main.rst
rename to docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst
index 1892d1f8f7..e243529a9c 100644
--- a/docs/modules/mapping/circle_fitting/circle_fitting_main.rst
+++ b/docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst
@@ -11,3 +11,7 @@ The red crosses are observations from a ranging sensor.
The red circle is the estimated object shape using circle fitting.
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.circle_fitting.circle_fitting.circle_fitting
diff --git a/docs/modules/3_mapping/distance_map/distance_map.png b/docs/modules/3_mapping/distance_map/distance_map.png
new file mode 100644
index 0000000000..2d89252a70
Binary files /dev/null and b/docs/modules/3_mapping/distance_map/distance_map.png differ
diff --git a/docs/modules/3_mapping/distance_map/distance_map_main.rst b/docs/modules/3_mapping/distance_map/distance_map_main.rst
new file mode 100644
index 0000000000..ec60e752c9
--- /dev/null
+++ b/docs/modules/3_mapping/distance_map/distance_map_main.rst
@@ -0,0 +1,27 @@
+Distance Map
+------------
+
+This is an implementation of the Distance Map algorithm for path planning.
+
+The Distance Map algorithm computes the unsigned distance field (UDF) and signed distance field (SDF) from a boolean field representing obstacles.
+
+The UDF gives the distance from each point to the nearest obstacle. The SDF gives positive distances for points outside obstacles and negative distances for points inside obstacles.
+
+Example
+~~~~~~~
+
+The algorithm is demonstrated on a simple 2D grid with obstacles:
+
+.. image:: distance_map.png
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.DistanceMap.distance_map.compute_sdf
+
+.. autofunction:: Mapping.DistanceMap.distance_map.compute_udf
+
+References
+~~~~~~~~~~
+
+- `Distance Transforms of Sampled Functions `_ paper by Pedro F. Felzenszwalb and Daniel P. Huttenlocher.
\ No newline at end of file
diff --git a/docs/modules/mapping/gaussian_grid_map/gaussian_grid_map_main.rst b/docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst
similarity index 61%
rename from docs/modules/mapping/gaussian_grid_map/gaussian_grid_map_main.rst
rename to docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst
index b0f112a871..50033d2a20 100644
--- a/docs/modules/mapping/gaussian_grid_map/gaussian_grid_map_main.rst
+++ b/docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst
@@ -6,3 +6,9 @@ Gaussian grid map
This is a 2D Gaussian grid mapping example.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/gaussian_grid_map/animation.gif
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.gaussian_grid_map.gaussian_grid_map.generate_gaussian_grid_map
+
diff --git a/docs/modules/mapping/k_means_object_clustering/k_means_object_clustering_main.rst b/docs/modules/3_mapping/k_means_object_clustering/k_means_object_clustering_main.rst
similarity index 63%
rename from docs/modules/mapping/k_means_object_clustering/k_means_object_clustering_main.rst
rename to docs/modules/3_mapping/k_means_object_clustering/k_means_object_clustering_main.rst
index e098ca5409..0ece604009 100644
--- a/docs/modules/mapping/k_means_object_clustering/k_means_object_clustering_main.rst
+++ b/docs/modules/3_mapping/k_means_object_clustering/k_means_object_clustering_main.rst
@@ -4,3 +4,9 @@ k-means object clustering
This is a 2D object clustering with k-means algorithm.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/kmeans_clustering/animation.gif
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.kmeans_clustering.kmeans_clustering.kmeans_clustering
+
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/grid_map_example.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/grid_map_example.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/grid_map_example.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/grid_map_example.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
similarity index 98%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
index 1f62179efd..29f5878e48 100644
--- a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
+++ b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
@@ -196,3 +196,9 @@ Let’s use this flood fill on real data:
.. image:: lidar_to_grid_map_tutorial_14_1.png
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.lidar_to_grid_map.lidar_to_grid_map.main
+
+
diff --git a/docs/modules/3_mapping/mapping_main.rst b/docs/modules/3_mapping/mapping_main.rst
new file mode 100644
index 0000000000..34a3744893
--- /dev/null
+++ b/docs/modules/3_mapping/mapping_main.rst
@@ -0,0 +1,20 @@
+.. _`Mapping`:
+
+Mapping
+=======
+Mapping is the ability of a robot to understand its surroundings with external sensors such as LIDAR and camera. Robots have to recognize the position and shape of obstacles to avoid them. In mapping, grid mapping and machine learning algorithms are widely used[31][18]. Fig.3 shows mapping simulation results using grid mapping with 2D ray casting and 2D object clustering with k-means algorithm.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ gaussian_grid_map/gaussian_grid_map
+ ndt_map/ndt_map
+ ray_casting_grid_map/ray_casting_grid_map
+ lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial
+ point_cloud_sampling/point_cloud_sampling
+ k_means_object_clustering/k_means_object_clustering
+ circle_fitting/circle_fitting
+ rectangle_fitting/rectangle_fitting
+ normal_vector_estimation/normal_vector_estimation
+ distance_map/distance_map
diff --git a/docs/modules/3_mapping/ndt_map/grid_clustering.png b/docs/modules/3_mapping/ndt_map/grid_clustering.png
new file mode 100644
index 0000000000..c53590287a
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/grid_clustering.png differ
diff --git a/docs/modules/3_mapping/ndt_map/ndt_map1.png b/docs/modules/3_mapping/ndt_map/ndt_map1.png
new file mode 100644
index 0000000000..1f34b9a66a
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/ndt_map1.png differ
diff --git a/docs/modules/3_mapping/ndt_map/ndt_map2.png b/docs/modules/3_mapping/ndt_map/ndt_map2.png
new file mode 100644
index 0000000000..b997062979
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/ndt_map2.png differ
diff --git a/docs/modules/3_mapping/ndt_map/ndt_map_main.rst b/docs/modules/3_mapping/ndt_map/ndt_map_main.rst
new file mode 100644
index 0000000000..bfc3936314
--- /dev/null
+++ b/docs/modules/3_mapping/ndt_map/ndt_map_main.rst
@@ -0,0 +1,53 @@
+.. _ndt_map:
+
+Normal Distance Transform (NDT) map
+------------------------------------
+
+This is a NDT mapping example.
+
+Normal Distribution Transform (NDT) is a map representation that uses normal distribution for observation point modeling.
+
+Normal Distribution
+~~~~~~~~~~~~~~~~~~~~~
+
+Normal distribution consists of two parameters: mean :math:`\mu` and covariance :math:`\Sigma`.
+
+:math:`\mathbf{X} \sim \mathcal{N}(\boldsymbol{\mu}, \boldsymbol{\Sigma})`
+
+In the 2D case, :math:`\boldsymbol{\mu}` is a 2D vector and :math:`\boldsymbol{\Sigma}` is a 2x2 matrix.
+
+In the matrix form, the probability density function of thr normal distribution is:
+
+:math:`X=\frac{1}{\sqrt{(2 \pi)^2|\Sigma|}} \exp \left\{-\frac{1}{2}^t(x-\mu) \sum^{-1}(x-\mu)\right\}`
+
+Normal Distance Transform mapping steps
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+NDT mapping consists of two steps:
+
+When we have a new observation like this:
+
+.. figure:: raw_observations.png
+
+First, we need to cluster the observation points.
+This is done by using a grid based clustering algorithm.
+
+The result is:
+
+.. figure:: grid_clustering.png
+
+Then, we need to fit a normal distribution to each grid cluster.
+
+Black ellipse shows each NDT grid like this:
+
+.. figure:: ndt_map1.png
+
+.. figure:: ndt_map2.png
+
+API
+~~~~~
+
+.. autoclass:: Mapping.ndt_map.ndt_map.NDTMap
+ :members:
+ :class-doc-from: class
+
diff --git a/docs/modules/3_mapping/ndt_map/raw_observations.png b/docs/modules/3_mapping/ndt_map/raw_observations.png
new file mode 100644
index 0000000000..c1a0dd4778
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/raw_observations.png differ
diff --git a/docs/modules/3_mapping/normal_vector_estimation/normal_vector_calc.png b/docs/modules/3_mapping/normal_vector_estimation/normal_vector_calc.png
new file mode 100644
index 0000000000..63e2cdade7
Binary files /dev/null and b/docs/modules/3_mapping/normal_vector_estimation/normal_vector_calc.png differ
diff --git a/docs/modules/3_mapping/normal_vector_estimation/normal_vector_estimation_main.rst b/docs/modules/3_mapping/normal_vector_estimation/normal_vector_estimation_main.rst
new file mode 100644
index 0000000000..68a19e1534
--- /dev/null
+++ b/docs/modules/3_mapping/normal_vector_estimation/normal_vector_estimation_main.rst
@@ -0,0 +1,74 @@
+Normal vector estimation
+-------------------------
+
+
+Normal vector calculation of a 3D triangle
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+A 3D point is as a vector:
+
+.. math:: p = [x, y, z]
+
+When there are 3 points in 3D space, :math:`p_1, p_2, p_3`,
+
+we can calculate a normal vector n of a 3D triangle which is consisted of the points.
+
+.. math:: n = \frac{v1 \times v2}{|v1 \times v2|}
+
+where
+
+.. math:: v1 = p2 - p1
+
+.. math:: v2 = p3 - p1
+
+This is an example of normal vector calculation:
+
+.. figure:: normal_vector_calc.png
+
+Code Link
+==========
+
+.. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector
+
+Normal vector estimation with RANdam SAmpling Consensus(RANSAC)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Consider the problem of estimating the normal vector of a plane based on a
+set of N 3D points where a plane can be observed.
+
+There is a way that uses all point cloud data to estimate a plane and
+a normal vector using the `least-squares method `_
+
+However, this method is vulnerable to noise of the point cloud.
+
+In this document, we will use a method that uses
+`RANdam SAmpling Consensus(RANSAC) `_
+to estimate a plane and a normal vector.
+
+RANSAC is a robust estimation methods for data set with outliers.
+
+
+
+This RANSAC based normal vector estimation method is as follows:
+
+#. Select 3 points randomly from the point cloud.
+
+#. Calculate a normal vector of a plane which is consists of the sampled 3 points.
+
+#. Calculate the distance between the calculated plane and the all point cloud.
+
+#. If the distance is less than a threshold, the point is considered to be an inlier.
+
+#. Repeat the above steps until the inlier ratio is greater than a threshold.
+
+
+
+This is an example of RANSAC based normal vector estimation:
+
+.. figure:: ransac_normal_vector_estimation.png
+
+Code Link
+==========
+
+.. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation
+
diff --git a/docs/modules/3_mapping/normal_vector_estimation/ransac_normal_vector_estimation.png b/docs/modules/3_mapping/normal_vector_estimation/ransac_normal_vector_estimation.png
new file mode 100644
index 0000000000..e9b661e79c
Binary files /dev/null and b/docs/modules/3_mapping/normal_vector_estimation/ransac_normal_vector_estimation.png differ
diff --git a/docs/modules/3_mapping/point_cloud_sampling/farthest_point_sampling.png b/docs/modules/3_mapping/point_cloud_sampling/farthest_point_sampling.png
new file mode 100644
index 0000000000..6024a5f2d5
Binary files /dev/null and b/docs/modules/3_mapping/point_cloud_sampling/farthest_point_sampling.png differ
diff --git a/docs/modules/3_mapping/point_cloud_sampling/point_cloud_sampling_main.rst b/docs/modules/3_mapping/point_cloud_sampling/point_cloud_sampling_main.rst
new file mode 100644
index 0000000000..8cb08d4b78
--- /dev/null
+++ b/docs/modules/3_mapping/point_cloud_sampling/point_cloud_sampling_main.rst
@@ -0,0 +1,70 @@
+.. _point_cloud_sampling:
+
+Point cloud Sampling
+----------------------
+
+This sections explains point cloud sampling algorithms in PythonRobotics.
+
+Point clouds are two-dimensional and three-dimensional based data
+acquired by external sensors like LIDAR, cameras, etc.
+In general, Point Cloud data is very large in number of data.
+So, if you process all the data, computation time might become an issue.
+
+Point cloud sampling is a technique for solving this computational complexity
+issue by extracting only representative point data and thinning the point
+cloud data without compromising the performance of processing using the point
+cloud data.
+
+Voxel Point Sampling
+~~~~~~~~~~~~~~~~~~~~~~~~
+.. figure:: voxel_point_sampling.png
+
+Voxel grid sampling is a method of reducing point cloud data by using the
+`Voxel grids `_ which is regular grids
+in three-dimensional space.
+
+This method determines which each point is in a grid, and replaces the point
+clouds that are in the same Voxel with their average to reduce the number of
+points.
+
+Code Link
+==========
+
+.. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.voxel_point_sampling
+
+
+Farthest Point Sampling
+~~~~~~~~~~~~~~~~~~~~~~~~~
+.. figure:: farthest_point_sampling.png
+
+Farthest Point Sampling is a point cloud sampling method by a specified
+number of points so that the distance between points is as far from as
+possible.
+
+This method is useful for machine learning and other situations where
+you want to obtain a specified number of points from point cloud.
+
+API
+=====
+
+.. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.farthest_point_sampling
+
+Poisson Disk Sampling
+~~~~~~~~~~~~~~~~~~~~~~~~~
+.. figure:: poisson_disk_sampling.png
+
+Poisson disk sample is a point cloud sampling method by a specified
+number of points so that the algorithm selects points where the distance
+from selected points is greater than a certain distance.
+
+Although this method does not have good performance comparing the Farthest
+distance sample where each point is distributed farther from each other,
+this is suitable for real-time processing because of its fast computation time.
+
+Code Link
+==========
+
+.. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.poisson_disk_sampling
+
+
+
diff --git a/docs/modules/3_mapping/point_cloud_sampling/poisson_disk_sampling.png b/docs/modules/3_mapping/point_cloud_sampling/poisson_disk_sampling.png
new file mode 100644
index 0000000000..6863c9e046
Binary files /dev/null and b/docs/modules/3_mapping/point_cloud_sampling/poisson_disk_sampling.png differ
diff --git a/docs/modules/3_mapping/point_cloud_sampling/voxel_point_sampling.png b/docs/modules/3_mapping/point_cloud_sampling/voxel_point_sampling.png
new file mode 100644
index 0000000000..fe1d171f7c
Binary files /dev/null and b/docs/modules/3_mapping/point_cloud_sampling/voxel_point_sampling.png differ
diff --git a/docs/modules/3_mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst b/docs/modules/3_mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
new file mode 100644
index 0000000000..bee2f64193
--- /dev/null
+++ b/docs/modules/3_mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
@@ -0,0 +1,11 @@
+Ray casting grid map
+--------------------
+
+This is a 2D ray casting grid mapping example.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/raycasting_grid_map/animation.gif
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.ray_casting_grid_map.ray_casting_grid_map.generate_ray_casting_grid_map
diff --git a/docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst b/docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst
new file mode 100644
index 0000000000..06d85efe61
--- /dev/null
+++ b/docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst
@@ -0,0 +1,69 @@
+Object shape recognition using rectangle fitting
+------------------------------------------------
+
+This is an object shape recognition using rectangle fitting.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/rectangle_fitting/animation.gif
+
+This example code is based on this paper algorithm:
+
+- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University `_
+
+The algorithm consists of 2 steps as below.
+
+Step1: Adaptive range segmentation
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+In the first step, all range data points are segmented into some clusters.
+
+We calculate the distance between each range data and the nearest range data, and if this distance is below a certain threshold, it is judged to be in the same cluster.
+This distance threshold is determined in proportion to the distance from the sensor.
+This is taking advantage of the general model of distance sensors, which tends to have sparser data distribution as the distance from the sensor increases.
+
+The threshold range is calculated by:
+
+.. math:: r_{th} = R_0 + R_d * r_{origin}
+
+where
+
+- :math:`r_{th}`: Threashold range
+- :math:`R_0, R_d`: Constant parameters
+- :math:`r_{origin}`: Distance from the sensor for a range data.
+
+Step2: Rectangle search
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+In the second step, for each cluster calculated in the previous step, rectangular fittings will be applied.
+In this rectangular fitting, each cluster's distance data is rotated at certain angle intervals.
+It is evaluated by one of the three evaluation functions below, then best angle parameter one is selected as the rectangle shape.
+
+1. Rectangle Area Minimization criteria
+=========================================
+
+This evaluation function calculates the area of the smallest rectangle that includes all the points, derived from the difference between the maximum and minimum values on the x-y axis for all distance data points.
+This allows for fitting a rectangle in a direction that encompasses as much of the smallest rectangular shape as possible.
+
+
+2. Closeness criteria
+======================
+
+This evaluation function uses the distances between the top and bottom vertices on the right side of the rectangle and each point in the distance data as evaluation values.
+If there are points on the rectangle edges, this evaluation value decreases.
+
+3. Variance criteria
+=======================
+
+This evaluation function uses the squreed distances between the edges of the rectangle (horizontal and vertical) and each point.
+Calculating the squared error is the same as calculating the variance.
+The smaller this variance, the more it signifies that the points fit within the rectangle.
+
+Code Link
+~~~~~~~~~~~
+
+.. autoclass:: Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting
+ :members:
+
+References
+~~~~~~~~~~
+
+- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University `_
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png b/docs/modules/4_slam/FastSLAM1/FastSLAM1_12_0.png
similarity index 100%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_12_0.png
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png b/docs/modules/4_slam/FastSLAM1/FastSLAM1_12_1.png
similarity index 100%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_12_1.png
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png b/docs/modules/4_slam/FastSLAM1/FastSLAM1_1_0.png
similarity index 100%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_1_0.png
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_main.rst b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
similarity index 99%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_main.rst
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
index f19527c453..b6bafa0982 100644
--- a/docs/modules/slam/FastSLAM1/FastSLAM1_main.rst
+++ b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
@@ -22,6 +22,11 @@ The red points are particles of FastSLAM.
Black points are landmarks, blue crosses are estimated landmark
positions by FastSLAM.
+Code Link
+~~~~~~~~~~~~
+
+.. autofunction:: SLAM.FastSLAM1.fast_slam1.fast_slam1
+
Introduction
~~~~~~~~~~~~
@@ -65,7 +70,7 @@ represent the initial uncertainty. At each time step we do:
The following equations and code snippets we can see how the particles
distribution evolves in case we provide only the control :math:`(v,w)`,
-which are the linear and angular velocity repsectively.
+which are the linear and angular velocity respectively.
:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation*}`
diff --git a/docs/modules/slam/FastSLAM2/FastSLAM2_main.rst b/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
similarity index 85%
rename from docs/modules/slam/FastSLAM2/FastSLAM2_main.rst
rename to docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
index 9e79b496a3..59ed3b9f75 100644
--- a/docs/modules/slam/FastSLAM2/FastSLAM2_main.rst
+++ b/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
@@ -7,6 +7,12 @@ The animation has the same meanings as one of FastSLAM 1.0.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif
+Code Link
+~~~~~~~~~~~
+
+.. autofunction:: SLAM.FastSLAM2.fast_slam2.fast_slam2
+
+
References
~~~~~~~~~~
diff --git a/docs/modules/slam/ekf_slam/ekf_slam_1_0.png b/docs/modules/4_slam/ekf_slam/ekf_slam_1_0.png
similarity index 100%
rename from docs/modules/slam/ekf_slam/ekf_slam_1_0.png
rename to docs/modules/4_slam/ekf_slam/ekf_slam_1_0.png
diff --git a/docs/modules/slam/ekf_slam/ekf_slam_main.rst b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
similarity index 95%
rename from docs/modules/slam/ekf_slam/ekf_slam_main.rst
rename to docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
index 70a7d131ae..3967a7ae4d 100644
--- a/docs/modules/slam/ekf_slam/ekf_slam_main.rst
+++ b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
@@ -21,6 +21,12 @@ This is a simulation of EKF SLAM.
- Blue line: ground truth
- Red line: EKF SLAM position estimation
+Code Link
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. autofunction:: SLAM.EKFSLAM.ekf_slam.ekf_slam
+
+
Introduction
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -63,27 +69,27 @@ Take care to note the difference between :math:`X` (state) and :math:`x`
original author: Atsushi Sakai (@Atsushi_twi)
notebook author: Andrew Tu (drewtu2)
"""
-
+
import math
import numpy as np
%matplotlib notebook
import matplotlib.pyplot as plt
-
-
+
+
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Change in covariance
-
+
# Simulation parameter
Qsim = np.diag([0.2, np.deg2rad(1.0)])**2 # Sensor Noise
Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 # Process Noise
-
+
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3 # State size [x,y,yaw]
LM_SIZE = 2 # LM state size [x,y]
-
+
show_animation = True
Algorithm Walk through
@@ -97,23 +103,22 @@ the estimated state and measurements
def ekf_slam(xEst, PEst, u, z):
"""
- Performs an iteration of EKF SLAM from the available information.
-
+ Performs an iteration of EKF SLAM from the available information.
+
:param xEst: the belief in last position
:param PEst: the uncertainty in last position
- :param u: the control function applied to the last position
+ :param u: the control function applied to the last position
:param z: measurements at this step
:returns: the next estimated position and associated covariance
"""
- S = STATE_SIZE
-
+
# Predict
- xEst, PEst, G, Fx = predict(xEst, PEst, u)
+ xEst, PEst = predict(xEst, PEst, u)
initP = np.eye(2)
-
+
# Update
- xEst, PEst = update(xEst, PEst, u, z, initP)
-
+ xEst, PEst = update(xEst, PEst, z, initP)
+
return xEst, PEst
@@ -170,26 +175,25 @@ the landmarks.
def predict(xEst, PEst, u):
"""
Performs the prediction step of EKF SLAM
-
+
:param xEst: nx1 state vector
:param PEst: nxn covariance matrix
:param u: 2x1 control vector
:returns: predicted state vector, predicted covariance, jacobian of control vector, transition fx
"""
- S = STATE_SIZE
- G, Fx = jacob_motion(xEst[0:S], u)
- xEst[0:S] = motion_model(xEst[0:S], u)
+ G, Fx = jacob_motion(xEst, u)
+ xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
# Fx is an an identity matrix of size (STATE_SIZE)
# sigma = G*sigma*G.T + Noise
- PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
- return xEst, PEst, G, Fx
+ PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
+ return xEst, PEst
.. code:: ipython3
def motion_model(x, u):
"""
Computes the motion model based on current state and input function.
-
+
:param x: 3x1 pose estimation
:param u: 2x1 control input [v; w]
:returns: the resulting state after the control function is applied
@@ -197,11 +201,11 @@ the landmarks.
F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
-
+
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])
-
+
x = (F @ x) + (B @ u)
return x
@@ -261,22 +265,21 @@ the changing uncertainty.
.. code:: ipython3
- def update(xEst, PEst, u, z, initP):
+ def update(xEst, PEst, z, initP):
"""
Performs the update step of EKF SLAM
-
+
:param xEst: nx1 the predicted pose of the system and the pose of the landmarks
:param PEst: nxn the predicted covariance
- :param u: 2x1 the control function
:param z: the measurements read at new position
:param initP: 2x2 an identity matrix acting as the initial covariance
:returns: the updated state and covariance for the system
"""
for iz in range(len(z[:, 0])): # for each observation
minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) # associate to a known landmark
-
+
nLM = calc_n_LM(xEst) # number of landmarks we currently know about
-
+
if minid == nLM: # Landmark is a NEW landmark
print("New LM")
# Extend state and covariance matrix
@@ -285,14 +288,14 @@ the changing uncertainty.
np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
xEst = xAug
PEst = PAug
-
+
lm = get_LM_Pos_from_state(xEst, minid)
y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
-
+
K = (PEst @ H.T) @ np.linalg.inv(S) # Calculate Kalman Gain
xEst = xEst + (K @ y)
PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst
-
+
xEst[2] = pi_2_pi(xEst[2])
return xEst, PEst
@@ -302,7 +305,7 @@ the changing uncertainty.
def calc_innovation(lm, xEst, PEst, z, LMid):
"""
Calculates the innovation based on expected position and landmark position
-
+
:param lm: landmark position
:param xEst: estimated position/state
:param PEst: estimated covariance
@@ -315,19 +318,19 @@ the changing uncertainty.
zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
# zp is the expected measurement based on xEst and the expected landmark position
-
+
y = (z - zp).T # y = innovation
y[1] = pi_2_pi(y[1])
-
+
H = jacobH(q, delta, xEst, LMid + 1)
S = H @ PEst @ H.T + Cx[0:2, 0:2]
-
+
return y, S, H
-
+
def jacobH(q, delta, x, i):
"""
Calculates the jacobian of the measurement function
-
+
:param q: the range from the system pose to the landmark
:param delta: the difference between a landmark position and the estimated system position
:param x: the state, including the estimated system position
@@ -337,17 +340,17 @@ the changing uncertainty.
sq = math.sqrt(q)
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
[delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])
-
+
G = G / q
nLM = calc_n_LM(x)
F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))
F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),
np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))
-
+
F = np.vstack((F1, F2))
-
+
H = G @ F
-
+
return H
Observation Step
@@ -370,17 +373,17 @@ reckoning and control functions are passed along here as well.
:param xd: the current noisy estimate of the system
:param u: the current control input
:param RFID: the true position of the landmarks
-
- :returns: Computes the true position, observations, dead reckoning (noisy) position,
+
+ :returns: Computes the true position, observations, dead reckoning (noisy) position,
and noisy control function
"""
xTrue = motion_model(xTrue, u)
-
+
# add noise to gps x-y
z = np.zeros((0, 3))
-
+
for i in range(len(RFID[:, 0])): # Test all beacons, only add the ones we can see (within MAX_RANGE)
-
+
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
@@ -390,12 +393,12 @@ reckoning and control functions are passed along here as well.
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise
zi = np.array([dn, anglen, i])
z = np.vstack((z, zi))
-
+
# add noise to input
ud = np.array([[
u[0, 0] + np.random.randn() * Rsim[0, 0],
u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T
-
+
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
@@ -409,32 +412,30 @@ reckoning and control functions are passed along here as well.
"""
n = int((len(x) - STATE_SIZE) / LM_SIZE)
return n
-
-
+
+
def jacob_motion(x, u):
"""
- Calculates the jacobian of motion model.
-
+ Calculates the jacobian of motion model.
+
:param x: The state, including the estimated position of the system
:param u: The control function
:returns: G: Jacobian
Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix
"""
-
+
# [eye(3) [0 x y; 0 x y; 0 x y]]
Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
(STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
-
+
jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]],dtype=object)
-
+
G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
if calc_n_LM(x) > 0:
print(Fx.shape)
return G, Fx,
-
-
.. code:: ipython3
@@ -442,68 +443,68 @@ reckoning and control functions are passed along here as well.
def calc_LM_Pos(x, z):
"""
Calculates the pose in the world coordinate frame of a landmark at the given measurement.
-
+
:param x: [x; y; theta]
:param z: [range; bearing]
:returns: [x; y] for given measurement
"""
zp = np.zeros((2, 1))
-
+
zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
#zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
#zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
-
+
return zp
-
-
+
+
def get_LM_Pos_from_state(x, ind):
"""
Returns the position of a given landmark
-
+
:param x: The state containing all landmark positions
:param ind: landmark id
:returns: The position of the landmark
"""
lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]
-
+
return lm
-
-
+
+
def search_correspond_LM_ID(xAug, PAug, zi):
"""
Landmark association with Mahalanobis distance.
-
- If this landmark is at least M_DIST_TH units away from all known landmarks,
+
+ If this landmark is at least M_DIST_TH units away from all known landmarks,
it is a NEW landmark.
-
+
:param xAug: The estimated state
:param PAug: The estimated covariance
:param zi: the read measurements of specific landmark
:returns: landmark id
"""
-
+
nLM = calc_n_LM(xAug)
-
+
mdist = []
-
+
for i in range(nLM):
lm = get_LM_Pos_from_state(xAug, i)
y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
mdist.append(y.T @ np.linalg.inv(S) @ y)
-
+
mdist.append(M_DIST_TH) # new landmark
-
+
minid = mdist.index(min(mdist))
-
+
return minid
-
+
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
return u
-
+
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
@@ -511,53 +512,53 @@ reckoning and control functions are passed along here as well.
def main():
print(" start!!")
-
+
time = 0.0
-
+
# RFID positions [x, y]
RFID = np.array([[10.0, -2.0],
[15.0, 10.0],
[3.0, 15.0],
[-5.0, 20.0]])
-
+
# State Vector [x y yaw v]'
xEst = np.zeros((STATE_SIZE, 1))
xTrue = np.zeros((STATE_SIZE, 1))
PEst = np.eye(STATE_SIZE)
-
+
xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
-
+
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
-
+
while SIM_TIME >= time:
time += DT
u = calc_input()
-
+
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
-
+
xEst, PEst = ekf_slam(xEst, PEst, ud, z)
-
+
x_state = xEst[0:STATE_SIZE]
-
+
# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
-
+
if show_animation: # pragma: no cover
plt.cla()
-
+
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(xEst[0], xEst[1], ".r")
-
+
# plot landmark
for i in range(calc_n_LM(xEst)):
plt.plot(xEst[STATE_SIZE + i * 2],
xEst[STATE_SIZE + i * 2 + 1], "xg")
-
+
plt.plot(hxTrue[0, :],
hxTrue[1, :], "-b")
plt.plot(hxDR[0, :],
@@ -583,9 +584,7 @@ reckoning and control functions are passed along here as well.
.. image:: ekf_slam_1_0.png
-References:
+Reference
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- `PROBABILISTIC ROBOTICS `_
-
-
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst
similarity index 97%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst
index 491320512b..15963aff79 100644
--- a/docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst
+++ b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst
@@ -165,7 +165,7 @@ different data sources into a single optimization problem.
6 215.8405 -0.000000
-.. figure:: graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/Graph_SLAM_optimization.gif
.. code:: ipython3
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc.rst b/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc.rst
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc.rst
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_formulation.rst b/docs/modules/4_slam/graph_slam/graphSLAM_formulation.rst
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_formulation.rst
rename to docs/modules/4_slam/graph_slam/graphSLAM_formulation.rst
diff --git a/docs/modules/slam/graph_slam/graph_slam_main.rst b/docs/modules/4_slam/graph_slam/graph_slam_main.rst
similarity index 84%
rename from docs/modules/slam/graph_slam/graph_slam_main.rst
rename to docs/modules/4_slam/graph_slam/graph_slam_main.rst
index 987eed385c..2ef17e4179 100644
--- a/docs/modules/slam/graph_slam/graph_slam_main.rst
+++ b/docs/modules/4_slam/graph_slam/graph_slam_main.rst
@@ -13,11 +13,17 @@ The black stars are landmarks for graph edge generation.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/animation.gif
+Code Link
+~~~~~~~~~~~~
+
+.. autofunction:: SLAM.GraphBasedSLAM.graph_based_slam.graph_based_slam
+
+
.. include:: graphSLAM_doc.rst
.. include:: graphSLAM_formulation.rst
.. include:: graphSLAM_SE2_example.rst
-References:
+Reference
~~~~~~~~~~~
- `A Tutorial on Graph-Based SLAM `_
diff --git a/docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst b/docs/modules/4_slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
similarity index 78%
rename from docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
rename to docs/modules/4_slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
index a30b1fc99b..772fe62889 100644
--- a/docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
+++ b/docs/modules/4_slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
@@ -10,7 +10,13 @@ points to points.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: SLAM.ICPMatching.icp_matching.icp_matching
+
+
References
-~~~~~~~~~~
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- `Introduction to Mobile Robotics: Iterative Closest Point Algorithm `_
diff --git a/docs/modules/4_slam/slam_main.rst b/docs/modules/4_slam/slam_main.rst
new file mode 100644
index 0000000000..98211986c2
--- /dev/null
+++ b/docs/modules/4_slam/slam_main.rst
@@ -0,0 +1,18 @@
+.. _`SLAM`:
+
+SLAM
+====
+
+Simultaneous Localization and Mapping(SLAM) examples
+Simultaneous Localization and Mapping (SLAM) is an ability to estimate the pose of a robot and the map of the environment at the same time. The SLAM problem is hard to
+solve, because a map is needed for localization and localization is needed for mapping. In this way, SLAM is often said to be similar to a ‘chicken-and-egg’ problem. Popular SLAM solution methods include the extended Kalman filter, particle filter, and Fast SLAM algorithm[31]. Fig.4 shows SLAM simulation results using extended Kalman filter and results using FastSLAM2.0[31].
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ iterative_closest_point_matching/iterative_closest_point_matching
+ ekf_slam/ekf_slam
+ FastSLAM1/FastSLAM1
+ FastSLAM2/FastSLAM2
+ graph_slam/graph_slam
diff --git a/docs/modules/path_planning/bezier_path/Figure_1.png b/docs/modules/5_path_planning/bezier_path/Figure_1.png
similarity index 100%
rename from docs/modules/path_planning/bezier_path/Figure_1.png
rename to docs/modules/5_path_planning/bezier_path/Figure_1.png
diff --git a/docs/modules/path_planning/bezier_path/Figure_2.png b/docs/modules/5_path_planning/bezier_path/Figure_2.png
similarity index 100%
rename from docs/modules/path_planning/bezier_path/Figure_2.png
rename to docs/modules/5_path_planning/bezier_path/Figure_2.png
diff --git a/docs/modules/path_planning/bezier_path/bezier_path_main.rst b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
similarity index 58%
rename from docs/modules/path_planning/bezier_path/bezier_path_main.rst
rename to docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
index fbba6a4496..19fb89a1b1 100644
--- a/docs/modules/path_planning/bezier_path/bezier_path_main.rst
+++ b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
@@ -13,8 +13,15 @@ You can get different Beizer course:
.. image:: Figure_2.png
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.BezierPath.bezier_path.calc_4points_bezier_path
+
+
+Reference
+~~~~~~~~~~~~~~~
- `Continuous Curvature Path Generation Based on Bezier Curves for
Autonomous
- Vehicles `__
+ Vehicles `__
diff --git a/docs/modules/5_path_planning/bspline_path/approx_and_curvature.png b/docs/modules/5_path_planning/bspline_path/approx_and_curvature.png
new file mode 100644
index 0000000000..49f260bb60
Binary files /dev/null and b/docs/modules/5_path_planning/bspline_path/approx_and_curvature.png differ
diff --git a/docs/modules/5_path_planning/bspline_path/approximation1.png b/docs/modules/5_path_planning/bspline_path/approximation1.png
new file mode 100644
index 0000000000..d5f00551eb
Binary files /dev/null and b/docs/modules/5_path_planning/bspline_path/approximation1.png differ
diff --git a/docs/modules/5_path_planning/bspline_path/basis_functions.png b/docs/modules/5_path_planning/bspline_path/basis_functions.png
new file mode 100644
index 0000000000..65075c8015
Binary files /dev/null and b/docs/modules/5_path_planning/bspline_path/basis_functions.png differ
diff --git a/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst b/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst
new file mode 100644
index 0000000000..00e5ef2fdb
--- /dev/null
+++ b/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst
@@ -0,0 +1,146 @@
+B-Spline planning
+-----------------
+
+.. image:: bspline_path_planning.png
+
+This is a B-Spline path planning routines.
+
+If you input waypoints, it generates a smooth path with B-Spline curve.
+
+This codes provide two types of B-Spline curve generations:
+
+1. Interpolation: generate a curve that passes through all waypoints.
+
+2. Approximation: generate a curve that approximates the waypoints. (Not passing through all waypoints)
+
+Bspline basics
+~~~~~~~~~~~~~~
+
+BSpline (Basis-Spline) is a piecewise polynomial spline curve.
+
+It is expressed by the following equation.
+
+:math:`\mathbf{S}(x)=\sum_{i=k-p}^k \mathbf{c}_i B_{i, p}(x)`
+
+here:
+
+* :math:`S(x)` is the curve point on the spline at x.
+* :math:`c_i` is the representative point generating the spline, called the control point.
+* :math:`p+1` is the dimension of the BSpline.
+* :math:`k` is the number of knots.
+* :math:`B_{i,p}(x)` is a function called Basis Function.
+
+The the basis function can be calculated by the following `De Boor recursion formula `_:
+
+:math:`B_{i, 0}(x):= \begin{cases}1 & \text { if } \quad t_i \leq x`_.
+
+.. code-block:: python
+
+ from scipy.interpolate import BSpline
+
+ def B_orig(x, k, i, t):
+ if k == 0:
+ return 1.0 if t[i] <= x < t[i + 1] else 0.0
+ if t[i + k] == t[i]:
+ c1 = 0.0
+ else:
+ c1 = (x - t[i]) / (t[i + k] - t[i]) * B(x, k - 1, i, t)
+
+ if t[i + k + 1] == t[i + 1]:
+ c2 = 0.0
+ else:
+ c2 = (t[i + k + 1] - x) / (t[i + k + 1] - t[i + 1]) * B(x, k - 1, i + 1, t)
+ return c1 + c2
+
+
+ def B(x, k, i, t):
+ c = np.zeros_like(t)
+ c[i] = 1
+ return BSpline(t, c, k)(x)
+
+
+ def main():
+ k = 3 # degree of the spline
+ t = [0, 1, 2, 3, 4, 5] # knots vector
+
+ x = np.linspace(0, 5, 1000, endpoint=False)
+ t = np.r_[[np.min(t)]*k, t, [np.max(t)]*k]
+
+ n = len(t) - k - 1
+ for i in range(n):
+ y = np.array([B(ix, k, i, t) for ix in x])
+ plt.plot(x, y, label=f'i = {i}')
+
+ plt.title(f'Basis functions (k = {k}, knots = {t})')
+ plt.show()
+
+Bspline interpolation planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+:meth:`PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path` generates a curve that passes through all waypoints.
+
+This is a simple example of the interpolation planning:
+
+.. image:: interpolation1.png
+
+This figure also shows curvatures of each path point using :ref:`utils.plot.plot_curvature `.
+
+The default spline degree is 3, so curvature changes smoothly.
+
+.. image:: interp_and_curvature.png
+
+Code link
+++++++++++
+
+.. autofunction:: PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path
+
+
+Bspline approximation planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+:meth:`PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path`
+generates a curve that approximates the waypoints, which means that
+the curve might not pass through waypoints.
+
+Users can adjust path smoothness by the smoothing parameter `s`. If this
+value is bigger, the path will be smoother, but it will be less accurate.
+If this value is smaller, the path will be more accurate, but it will be
+less smooth.
+
+This is a simple example of the approximation planning:
+
+.. image:: approximation1.png
+
+This figure also shows curvatures of each path point using :ref:`utils.plot.plot_curvature `.
+
+The default spline degree is 3, so curvature changes smoothly.
+
+.. image:: approx_and_curvature.png
+
+Code Link
+++++++++++
+
+.. autofunction:: PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path
+
+
+References
+~~~~~~~~~~
+
+- `B-spline - Wikipedia `__
+- `scipy.interpolate.UnivariateSpline `__
\ No newline at end of file
diff --git a/docs/modules/5_path_planning/bspline_path/bspline_path_planning.png b/docs/modules/5_path_planning/bspline_path/bspline_path_planning.png
new file mode 100644
index 0000000000..a3c1e621f9
Binary files /dev/null and b/docs/modules/5_path_planning/bspline_path/bspline_path_planning.png differ
diff --git a/docs/modules/5_path_planning/bspline_path/interp_and_curvature.png b/docs/modules/5_path_planning/bspline_path/interp_and_curvature.png
new file mode 100644
index 0000000000..54e291741c
Binary files /dev/null and b/docs/modules/5_path_planning/bspline_path/interp_and_curvature.png differ
diff --git a/docs/modules/5_path_planning/bspline_path/interpolation1.png b/docs/modules/5_path_planning/bspline_path/interpolation1.png
new file mode 100644
index 0000000000..fb280343c9
Binary files /dev/null and b/docs/modules/5_path_planning/bspline_path/interpolation1.png differ
diff --git a/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst
new file mode 100644
index 0000000000..e1cd2fe353
--- /dev/null
+++ b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst
@@ -0,0 +1,16 @@
+Bug planner
+-----------
+
+This is a 2D planning with Bug algorithm.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BugPlanner/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.BugPlanning.bug.main
+
+Reference
+~~~~~~~~~~~~
+
+- `ECE452 Bug Algorithms `_
diff --git a/docs/modules/5_path_planning/catmull_rom_spline/blending_functions.png b/docs/modules/5_path_planning/catmull_rom_spline/blending_functions.png
new file mode 100644
index 0000000000..928946df46
Binary files /dev/null and b/docs/modules/5_path_planning/catmull_rom_spline/blending_functions.png differ
diff --git a/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_path_planning.png b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_path_planning.png
new file mode 100644
index 0000000000..1d0ff001e6
Binary files /dev/null and b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_path_planning.png differ
diff --git a/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst
new file mode 100644
index 0000000000..fa2a2ff72b
--- /dev/null
+++ b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst
@@ -0,0 +1,103 @@
+Catmull-Rom Spline Planning
+----------------------------
+
+.. image:: catmull_rom_path_planning.png
+
+This is a Catmull-Rom spline path planning routine.
+
+If you provide waypoints, the Catmull-Rom spline generates a smooth path that always passes through the control points,
+exhibits local control, and maintains 𝐶1 continuity.
+
+
+Catmull-Rom Spline Fundamentals
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Catmull-Rom splines are a type of cubic spline that passes through a given set of points, known as control points.
+
+They are defined by the following equation for calculating a point on the spline:
+
+:math:`P(t) = 0.5 \times \left( 2P_1 + (-P_0 + P_2)t + (2P_0 - 5P_1 + 4P_2 - P_3)t^2 + (-P_0 + 3P_1 - 3P_2 + P_3)t^3 \right)`
+
+Where:
+
+* :math:`P(t)` is the point on the spline at parameter :math:`t`.
+* :math:`P_0, P_1, P_2, P_3` are the control points surrounding the parameter :math:`t`.
+
+Types of Catmull-Rom Splines
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+There are different types of Catmull-Rom splines based on the choice of the **tau** parameter, which influences how the curve
+behaves in relation to the control points:
+
+1. **Uniform Catmull-Rom Spline**:
+ The standard implementation where the parameterization is uniform. Each segment of the spline is treated equally,
+ regardless of the distances between control points.
+
+
+2. **Chordal Catmull-Rom Spline**:
+ This spline type takes into account the distance between control points. The parameterization is based on the actual distance
+ along the spline, ensuring smoother transitions. The equation can be modified to include the chord length :math:`L_i` between
+ points :math:`P_i` and :math:`P_{i+1}`:
+
+ .. math::
+ \tau_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
+
+3. **Centripetal Catmull-Rom Spline**:
+ This variation improves upon the chordal spline by using the square root of the distance to determine the parameterization,
+ which avoids oscillations and creates a more natural curve. The parameter :math:`t_i` is adjusted using the following relation:
+
+ .. math::
+ t_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
+
+
+Blending Functions
+~~~~~~~~~~~~~~~~~~~~~
+
+In Catmull-Rom spline interpolation, blending functions are used to calculate the influence of each control point on the spline at a
+given parameter :math:`t`. The blending functions ensure that the spline is smooth and passes through the control points while
+maintaining continuity. The four blending functions used in Catmull-Rom splines are defined as follows:
+
+1. **Blending Function 1**:
+
+ .. math::
+ b_1(t) = -t + 2t^2 - t^3
+
+2. **Blending Function 2**:
+
+ .. math::
+ b_2(t) = 2 - 5t^2 + 3t^3
+
+3. **Blending Function 3**:
+
+ .. math::
+ b_3(t) = t + 4t^2 - 3t^3
+
+4. **Blending Function 4**:
+
+ .. math::
+ b_4(t) = -t^2 + t^3
+
+The blending functions are combined in the spline equation to create a smooth curve that reflects the influence of each control point.
+
+The following figure illustrates the blending functions over the interval :math:`[0, 1]`:
+
+.. image:: blending_functions.png
+
+Catmull-Rom Spline API
+~~~~~~~~~~~~~~~~~~~~~~~
+
+This section provides an overview of the functions used for Catmull-Rom spline path planning.
+
+Code Link
+++++++++++
+
+.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_point
+
+.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_spline
+
+
+References
+~~~~~~~~~~~~~~
+
+- `Catmull-Rom Spline - Wikipedia `__
+- `Catmull-Rom Splines `__
\ No newline at end of file
diff --git a/docs/modules/path_planning/clothoid_path/clothoid_path_main.rst b/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst
similarity index 95%
rename from docs/modules/path_planning/clothoid_path/clothoid_path_main.rst
rename to docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst
index 1e8cee694a..16d0ec03c1 100644
--- a/docs/modules/path_planning/clothoid_path/clothoid_path_main.rst
+++ b/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst
@@ -73,6 +73,11 @@ The final clothoid path can be calculated with the path parameters and Fresnel i
&y(s)=y_{0}+\int_{0}^{s} \sin \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau
\end{aligned}
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.ClothoidPath.clothoid_path_planner.generate_clothoid_path
+
References
~~~~~~~~~~
diff --git a/docs/modules/path_planning/coverage_path/coverage_path_main.rst b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst
similarity index 73%
rename from docs/modules/path_planning/coverage_path/coverage_path_main.rst
rename to docs/modules/5_path_planning/coverage_path/coverage_path_main.rst
index 828342a294..eaa876c80b 100644
--- a/docs/modules/path_planning/coverage_path/coverage_path_main.rst
+++ b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst
@@ -8,6 +8,11 @@ This is a 2D grid based sweep coverage path planner simulation:
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/GridBasedSweepCPP/animation.gif
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.GridBasedSweepCPP.grid_based_sweep_coverage_path_planner.planning
+
Spiral Spanning Tree
~~~~~~~~~~~~~~~~~~~~
@@ -17,6 +22,14 @@ This is a 2D grid based spiral spanning tree coverage path planner simulation:
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation2.gif
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation3.gif
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.SpiralSpanningTreeCPP.spiral_spanning_tree_coverage_path_planner.main
+
+Reference
++++++++++++++
+
- `Spiral-STC: An On-Line Coverage Algorithm of Grid Environments by a Mobile Robot `_
@@ -29,6 +42,14 @@ This is a 2D grid based wavefront coverage path planner simulation:
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation2.gif
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation3.gif
-- `Planning paths of complete coverage of an unstructured environment by a mobile robot `_
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.WavefrontCPP.wavefront_coverage_path_planner.wavefront
+
+Reference
++++++++++++++
+
+- `Planning paths of complete coverage of an unstructured environment by a mobile robot `_
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_1d.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_1d.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_1d.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_1d.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_2d_curvature.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_curvature.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_2d_curvature.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_curvature.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_2d_path.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_path.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_2d_path.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_path.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_2d_yaw.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_yaw.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_2d_yaw.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_yaw.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_main.rst b/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst
similarity index 99%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_main.rst
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst
index 33471f7c85..a110217a2e 100644
--- a/docs/modules/path_planning/cubic_spline/cubic_spline_main.rst
+++ b/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst
@@ -171,8 +171,8 @@ the second derivative by:
These equations can be calculated by differentiating the cubic polynomial.
-API
-===
+Code Link
+==========
This is the 1D cubic spline class API:
@@ -199,8 +199,8 @@ Curvature of each point can be also calculated analytically by:
:math:`\kappa=\frac{y^{\prime \prime} x^{\prime}-x^{\prime \prime} y^{\prime}}{\left(x^{\prime2}+y^{\prime2}\right)^{\frac{2}{3}}}`
-API
-===
+Code Link
+==========
.. autoclass:: PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline2D
:members:
diff --git a/docs/modules/path_planning/cubic_spline/spline.png b/docs/modules/5_path_planning/cubic_spline/spline.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/spline.png
rename to docs/modules/5_path_planning/cubic_spline/spline.png
diff --git a/docs/modules/path_planning/cubic_spline/spline_continuity.png b/docs/modules/5_path_planning/cubic_spline/spline_continuity.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/spline_continuity.png
rename to docs/modules/5_path_planning/cubic_spline/spline_continuity.png
diff --git a/docs/modules/path_planning/dubins_path/RLR.jpg b/docs/modules/5_path_planning/dubins_path/RLR.jpg
similarity index 100%
rename from docs/modules/path_planning/dubins_path/RLR.jpg
rename to docs/modules/5_path_planning/dubins_path/RLR.jpg
diff --git a/docs/modules/path_planning/dubins_path/RSR.jpg b/docs/modules/5_path_planning/dubins_path/RSR.jpg
similarity index 100%
rename from docs/modules/path_planning/dubins_path/RSR.jpg
rename to docs/modules/5_path_planning/dubins_path/RSR.jpg
diff --git a/docs/modules/path_planning/dubins_path/dubins_path.jpg b/docs/modules/5_path_planning/dubins_path/dubins_path.jpg
similarity index 100%
rename from docs/modules/path_planning/dubins_path/dubins_path.jpg
rename to docs/modules/5_path_planning/dubins_path/dubins_path.jpg
diff --git a/docs/modules/path_planning/dubins_path/dubins_path_main.rst b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst
similarity index 97%
rename from docs/modules/path_planning/dubins_path/dubins_path_main.rst
rename to docs/modules/5_path_planning/dubins_path/dubins_path_main.rst
index 516638d83d..5a3d14464b 100644
--- a/docs/modules/path_planning/dubins_path/dubins_path_main.rst
+++ b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst
@@ -62,7 +62,7 @@ You can generate a path from these information and the maximum curvature informa
A path type which has minimum course length among 6 types is selected,
and then a path is constructed based on the selected type and its distances.
-API
+Code Link
~~~~~~~~~~~~~~~~~~~~
.. autofunction:: PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path
@@ -72,5 +72,5 @@ Reference
~~~~~~~~~~~~~~~~~~~~
- `On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents `__
- `Dubins path - Wikipedia `__
-- `15.3.1 Dubins Curves `__
+- `15.3.1 Dubins Curves `__
- `A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths `__
diff --git a/docs/modules/path_planning/dynamic_window_approach/dynamic_window_approach_main.rst b/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
similarity index 74%
rename from docs/modules/path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
rename to docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
index d645838597..ac5322df94 100644
--- a/docs/modules/path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
+++ b/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
@@ -5,7 +5,17 @@ Dynamic Window Approach
This is a 2D navigation sample code with Dynamic Window Approach.
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif
+
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.DynamicWindowApproach.dynamic_window_approach.dwa_control
+
+
+Reference
+~~~~~~~~~~~~
+
- `The Dynamic Window Approach to Collision
Avoidance `__
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif
diff --git a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
new file mode 100644
index 0000000000..d0109d4ec3
--- /dev/null
+++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
@@ -0,0 +1,79 @@
+Elastic Bands
+-------------
+
+This is a path planning with Elastic Bands.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ElasticBands/animation.gif
+
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.ElasticBands.elastic_bands.ElasticBands
+
+
+Core Concept
+~~~~~~~~~~~~
+- **Elastic Band**: A dynamically deformable collision-free path initialized by a global planner.
+- **Objective**:
+
+ * Shorten and smooth the path.
+ * Maximize obstacle clearance.
+ * Maintain global path connectivity.
+
+Bubble Representation
+~~~~~~~~~~~~~~~~~~~~~~~~
+- **Definition**: A local free-space region around configuration :math:`b`:
+
+ .. math::
+ B(b) = \{ q: \|q - b\| < \rho(b) \},
+
+ where :math:`\rho(b)` is the radius of the bubble.
+
+
+Force-Based Deformation
+~~~~~~~~~~~~~~~~~~~~~~~
+The elastic band deforms under artificial forces:
+
+Internal Contraction Force
+++++++++++++++++++++++++++
+- **Purpose**: Reduces path slack and length.
+- **Formula**: For node :math:`b_i`:
+
+ .. math::
+ f_c(b_i) = k_c \left( \frac{b_{i-1} - b_i}{\|b_{i-1} - b_i\|} + \frac{b_{i+1} - b_i}{\|b_{i+1} - b_i\|} \right)
+
+ where :math:`k_c` is the contraction gain.
+
+External Repulsion Force
++++++++++++++++++++++++++
+- **Purpose**: Pushes the path away from obstacles.
+- **Formula**: For node :math:`b_i`:
+
+ .. math::
+ f_r(b_i) = \begin{cases}
+ k_r (\rho_0 - \rho(b_i)) \nabla \rho(b_i) & \text{if } \rho(b_i) < \rho_0, \\
+ 0 & \text{otherwise}.
+ \end{cases}
+
+ where :math:`k_r` is the repulsion gain, :math:`\rho_0` is the maximum distance for applying repulsion force, and :math:`\nabla \rho(b_i)` is approximated via finite differences:
+
+ .. math::
+ \frac{\partial \rho}{\partial x} \approx \frac{\rho(b_i + h) - \rho(b_i - h)}{2h}.
+
+Dynamic Path Maintenance
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+1. **Node Update**:
+
+ .. math::
+ b_i^{\text{new}} = b_i^{\text{old}} + \alpha (f_c + f_r),
+
+ where :math:`\alpha` is a step-size parameter, which often proportional to :math:`\rho(b_i^{\text{old}})`
+
+2. **Overlap Enforcement**:
+- Insert new nodes if adjacent nodes are too far apart
+- Remove redundant nodes if adjacent nodes are too close
+
+References
++++++++++++++
+
+- `Elastic Bands: Connecting Path Planning and Control `__
diff --git a/docs/modules/path_planning/eta3_spline/eta3_spline_main.rst b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst
similarity index 71%
rename from docs/modules/path_planning/eta3_spline/eta3_spline_main.rst
rename to docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst
index ffc3cc6451..9ee343e8a7 100644
--- a/docs/modules/path_planning/eta3_spline/eta3_spline_main.rst
+++ b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst
@@ -7,7 +7,14 @@ Eta^3 Spline path planning
This is a path planning with Eta^3 spline.
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autoclass:: PathPlanning.Eta3SplineTrajectory.eta3_spline_trajectory.Eta3SplineTrajectory
+
+
+Reference
+~~~~~~~~~~~~~~~
- `\\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile
Robots `__
diff --git a/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst
new file mode 100644
index 0000000000..0f84d381ea
--- /dev/null
+++ b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst
@@ -0,0 +1,52 @@
+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 predicted path.
+
+Code Link
+~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.FrenetOptimalTrajectory.frenet_optimal_trajectory.main
+
+
+High Speed and Velocity Keeping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/high_speed_and_velocity_keeping_frenet_path.gif
+
+This scenario shows how the trajectory is maintained at high speeds while keeping a consistent velocity.
+
+High Speed and Merging and Stopping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/high_speed_and_merging_and_stopping_frenet_path.gif
+
+This scenario demonstrates the trajectory planning at high speeds with merging and stopping behaviors.
+
+Low Speed and Velocity Keeping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/low_speed_and_velocity_keeping_frenet_path.gif
+
+This scenario demonstrates how the trajectory is managed at low speeds while maintaining a steady velocity.
+
+Low Speed and Merging and Stopping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/low_speed_and_merging_and_stopping_frenet_path.gif
+
+This scenario illustrates the trajectory planning at low speeds with merging and stopping behaviors.
+
+Reference
+
+- `Optimal Trajectory Generation for Dynamic Street Scenarios in a
+ Frenet
+ Frame `__
+
+- `Optimal trajectory generation for dynamic street scenarios in a
+ Frenet Frame `__
+
diff --git a/docs/modules/path_planning/grid_base_search/grid_base_search_main.rst b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst
similarity index 75%
rename from docs/modules/path_planning/grid_base_search/grid_base_search_main.rst
rename to docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst
index 1644ed00cc..c4aa6882aa 100644
--- a/docs/modules/path_planning/grid_base_search/grid_base_search_main.rst
+++ b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst
@@ -10,6 +10,12 @@ This is a 2D grid based path planning with Breadth first search algorithm.
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.BreadthFirstSearch.breadth_first_search.BreadthFirstSearchPlanner
+
+
Depth First Search
~~~~~~~~~~~~~~~~~~~~
@@ -19,6 +25,12 @@ This is a 2D grid based path planning with Depth first search algorithm.
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.DepthFirstSearch.depth_first_search.DepthFirstSearchPlanner
+
+
.. _dijkstra:
Dijkstra algorithm
@@ -30,6 +42,12 @@ This is a 2D grid based shortest path planning with Dijkstra's algorithm.
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.Dijkstra.dijkstra.DijkstraPlanner
+
+
.. _a*-algorithm:
A\* algorithm
@@ -43,6 +61,12 @@ In the animation, cyan points are searched nodes.
Its heuristic is 2D Euclid distance.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.AStar.a_star.AStarPlanner
+
+
Bidirectional A\* algorithm
~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -52,6 +76,12 @@ This is a 2D grid based shortest path planning with bidirectional A star algorit
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.BidirectionalAStar.bidirectional_a_star.BidirectionalAStarPlanner
+
+
.. _D*-algorithm:
D\* algorithm
@@ -63,7 +93,14 @@ This is a 2D grid based shortest path planning with D star algorithm.
The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm.
-Ref:
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.DStar.dstar.Dstar
+
+
+Reference
+++++++++++++
- `D* search Wikipedia `__
@@ -74,7 +111,13 @@ This is a 2D grid based path planning and replanning with D star lite algorithm.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStarLite/animation.gif
-Ref:
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.DStarLite.d_star_lite.DStarLite
+
+Reference
+++++++++++++
- `Improved Fast Replanning for Robot Navigation in Unknown Terrain `_
@@ -88,7 +131,14 @@ 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.
-Ref:
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.PotentialFieldPlanning.potential_field_planning.potential_field_planning
+
+
+Reference
+++++++++++++
- `Robotic Motion Planning:Potential
Functions `__
diff --git a/docs/modules/path_planning/hybridastar/hybridastar_main.rst b/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst
similarity index 66%
rename from docs/modules/path_planning/hybridastar/hybridastar_main.rst
rename to docs/modules/5_path_planning/hybridastar/hybridastar_main.rst
index a9876fa3d4..36f340e0c2 100644
--- a/docs/modules/path_planning/hybridastar/hybridastar_main.rst
+++ b/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst
@@ -4,3 +4,8 @@ Hybrid a star
This is a simple vehicle model based hybrid A\* path planner.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/HybridAStar/animation.gif
+
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.HybridAStar.hybrid_a_star.hybrid_a_star_planning
diff --git a/docs/modules/path_planning/lqr_path/lqr_path_main.rst b/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst
similarity index 73%
rename from docs/modules/path_planning/lqr_path/lqr_path_main.rst
rename to docs/modules/5_path_planning/lqr_path/lqr_path_main.rst
index 788442ff63..1eb1e4d840 100644
--- a/docs/modules/path_planning/lqr_path/lqr_path_main.rst
+++ b/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst
@@ -4,3 +4,8 @@ LQR based path planning
A sample code using LQR based path planning for double integrator model.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true
+
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.LQRPlanner.lqr_planner.LQRPlanner
diff --git a/docs/modules/path_planning/model_predictive_trajectory_generator/lookup_table.png b/docs/modules/5_path_planning/model_predictive_trajectory_generator/lookup_table.png
similarity index 100%
rename from docs/modules/path_planning/model_predictive_trajectory_generator/lookup_table.png
rename to docs/modules/5_path_planning/model_predictive_trajectory_generator/lookup_table.png
diff --git a/docs/modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
similarity index 70%
rename from docs/modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
rename to docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
index 0fc997898e..76472a6792 100644
--- a/docs/modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
+++ b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
@@ -6,6 +6,12 @@ generator.
This algorithm is used for state lattice planner.
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.ModelPredictiveTrajectoryGenerator.trajectory_generator.optimize_trajectory
+
+
Path optimization sample
~~~~~~~~~~~~~~~~~~~~~~~~
@@ -16,7 +22,8 @@ Lookup table generation sample
.. image:: lookup_table.png
-Ref:
+Reference
+~~~~~~~~~~~~
- `Optimal rough terrain trajectory generation for wheeled mobile
- robots `__
+ robots `__
diff --git a/docs/modules/path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst
similarity index 57%
rename from docs/modules/path_planning/path_planning_main.rst
rename to docs/modules/5_path_planning/path_planning_main.rst
index a3237f16f1..0c84a19c22 100644
--- a/docs/modules/path_planning/path_planning_main.rst
+++ b/docs/modules/5_path_planning/path_planning_main.rst
@@ -1,8 +1,10 @@
-.. _path_planning:
+.. _`Path Planning`:
Path Planning
=============
+Path planning is the ability of a robot to search feasible and efficient path to the goal. The path has to satisfy some constraints based on the robot’s motion model and obstacle positions, and optimize some objective functions such as time to goal and distance to obstacle. In path planning, dynamic programming based approaches and sampling based approaches are widely used[22]. Fig.5 shows simulation results of potential field path planning and LQRRRT* path planning[27].
+
.. toctree::
:maxdepth: 2
:caption: Contents
@@ -10,6 +12,7 @@ Path Planning
dynamic_window_approach/dynamic_window_approach
bugplanner/bugplanner
grid_base_search/grid_base_search
+ time_based_grid_search/time_based_grid_search
model_predictive_trajectory_generator/model_predictive_trajectory_generator
state_lattice_planner/state_lattice_planner
prm_planner/prm_planner
@@ -18,6 +21,7 @@ Path Planning
rrt/rrt
cubic_spline/cubic_spline
bspline_path/bspline_path
+ catmull_rom_spline/catmull_rom_spline
clothoid_path/clothoid_path
eta3_spline/eta3_spline
bezier_path/bezier_path
@@ -28,3 +32,4 @@ Path Planning
hybridastar/hybridastar
frenet_frame_path/frenet_frame_path
coverage_path/coverage_path
+ elastic_bands/elastic_bands
\ No newline at end of file
diff --git a/docs/modules/path_planning/prm_planner/prm_planner_main.rst b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
similarity index 79%
rename from docs/modules/path_planning/prm_planner/prm_planner_main.rst
rename to docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
index 0628719176..d58d1e2633 100644
--- a/docs/modules/path_planning/prm_planner/prm_planner_main.rst
+++ b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
@@ -13,7 +13,14 @@ Cyan crosses means searched points with Dijkstra method,
The red line is the final path of PRM.
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.ProbabilisticRoadMap.probabilistic_road_map.prm_planning
+
+
+Reference
+~~~~~~~~~~~
- `Probabilistic roadmap -
Wikipedia `__
diff --git a/docs/modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
similarity index 94%
rename from docs/modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
rename to docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
index feec345bae..c7bc3fb55c 100644
--- a/docs/modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
+++ b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
@@ -9,6 +9,11 @@ Motion planning with quintic polynomials.
It can calculate 2D path, velocity, and acceleration profile based on
quintic polynomials.
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.QuinticPolynomialsPlanner.quintic_polynomials_planner.quintic_polynomials_planner
+
Quintic polynomials for one dimensional robot motion
@@ -97,10 +102,10 @@ Each velocity and acceleration boundary condition can be calculated with each or
:math:`v_{xe}=v_ecos(\theta_e), v_{ye}=v_esin(\theta_e)`
-References:
+Reference
~~~~~~~~~~~
- `Local Path Planning And Motion Control For Agv In
- Positioning `__
+ Positioning `__
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LR_L.png b/docs/modules/5_path_planning/reeds_shepp_path/LR_L.png
new file mode 100644
index 0000000000..1e64da57f2
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LR_L.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LR_LR.png b/docs/modules/5_path_planning/reeds_shepp_path/LR_LR.png
new file mode 100644
index 0000000000..e2c9883d8e
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LR_LR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSL.png b/docs/modules/5_path_planning/reeds_shepp_path/LSL.png
new file mode 100644
index 0000000000..6785ad3f8c
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSL.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSL90xR.png b/docs/modules/5_path_planning/reeds_shepp_path/LSL90xR.png
new file mode 100644
index 0000000000..54e892ba46
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSL90xR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSR.png b/docs/modules/5_path_planning/reeds_shepp_path/LSR.png
new file mode 100644
index 0000000000..8acc0de69f
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSR90_L.png b/docs/modules/5_path_planning/reeds_shepp_path/LSR90_L.png
new file mode 100644
index 0000000000..58d381010d
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSR90_L.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL.png
new file mode 100644
index 0000000000..c305c0be6e
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL90_R.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL90_R.png
new file mode 100644
index 0000000000..f2b38329da
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL90_R.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R90SR.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SR.png
new file mode 100644
index 0000000000..4323a9fe3b
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_RL.png b/docs/modules/5_path_planning/reeds_shepp_path/L_RL.png
new file mode 100644
index 0000000000..ad58b8ffea
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_RL.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_RL_R.png b/docs/modules/5_path_planning/reeds_shepp_path/L_RL_R.png
new file mode 100644
index 0000000000..db4aaf6af3
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_RL_R.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R_L.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R_L.png
new file mode 100644
index 0000000000..0d3082aeaf
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R_L.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
new file mode 100644
index 0000000000..4dd54d7c97
--- /dev/null
+++ b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
@@ -0,0 +1,399 @@
+Reeds Shepp planning
+--------------------
+
+A sample code with Reeds Shepp path planning.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true
+
+Code Link
+==============
+
+.. autofunction:: PathPlanning.ReedsSheppPath.reeds_shepp_path_planning.reeds_shepp_path_planning
+
+
+Mathematical Description of Individual Path Types
+=================================================
+Here is an overview of mathematical derivations of formulae for individual path types.
+
+In all the derivations below, radius of curvature of the vehicle is assumed to be of unit length and start pose is considered to be at origin. (*In code we are removing the offset due to start position and normalising the lengths before passing the values to these functions.*)
+
+Also, (t, u, v) respresent the measure of each motion requried. Thus, in case of a turning maneuver, they represent the angle inscribed at the centre of turning circle and in case of straight maneuver, they represent the distance to be travelled.
+
+1. **Left-Straight-Left**
+
+.. image:: LSL.png
+
+We can deduce the following facts using geometry.
+
+- AGHC is a rectangle.
+- :math:`∠LAC = ∠BAG = t`
+- :math:`t + v = φ`
+- :math:`C(x - sin(φ), y + cos(φ))`
+- :math:`A(0, 1)`
+- :math:`u, t = polar(vector)`
+
+Hence, we have:
+
+- :math:`u, t = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`v = φ - t`
+
+
+2. **Left-Straight-Right**
+
+.. image:: LSR.png
+
+With followng notations:
+
+- :math:`∠MBD = t1`
+- :math:`∠BDF = θ`
+- :math:`BC = u1`
+
+We can deduce the following facts using geometry.
+
+- D is mid-point of BC and FG.
+- :math:`t - v = φ`
+- :math:`C(x + sin(φ), y - cos(φ))`
+- :math:`A(0, 1)`
+- :math:`u1, t1 = polar(vector)`
+- :math:`\frac{u1^2}{4} = 1 + \frac{u^2}{4}`
+- :math:`BF = 1` [Radius Of Curvature]
+- :math:`FD = \frac{u}{2}`
+- :math:`θ = arctan(\frac{BF}{FD})`
+- :math:`t1 + θ = t`
+
+Hence, we have:
+
+- :math:`u1, t1 = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = \sqrt{u1^2 - 4}`
+- :math:`θ = arctan(\frac{2}{u})`
+- :math:`t = t1 + θ`
+- :math:`v = t - φ`
+
+3. **LeftxRightxLeft**
+
+.. image:: L_R_L.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + u + v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`A = arccos(\frac{BD/2}{CD})`
+- :math:`u = (π - 2*A)`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD + ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`A = arccos(\frac{u1/2}{2})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`u = (π - 2*A)`
+- :math:`v = (φ - t - u)`
+
+4. **LeftxRight-Left**
+
+.. image:: L_RL.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + u - v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`A = arccos(\frac{BD/2}{CD})`
+- :math:`u = (π - 2*A)`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD + ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`A = arccos(\frac{u1/2}{2})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`u = (π - 2*A)`
+- :math:`v = (-φ + t + u)`
+
+5. **Left-RightxLeft**
+
+.. image:: LR_L.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - u - v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BC = CD = 2` [2 * radius of curvature]
+- :math:`cos(2π - u) = \frac{BC^2 + CD^2 - BD^2}{2 * BC * CD}` [Cosine Rule]
+- :math:`\frac{sin(A)}{BC} = \frac{sin(u)}{u1}` [Sine Rule]
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD - ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(1 - \frac{u1^2}{8})`
+- :math:`A = arcsin(\frac{sin(u)}{u1}*2)`
+- :math:`t = \frac{π}{2} + θ - A`
+- :math:`v = (t - u - φ)`
+
+6. **Left-RightxLeft-Right**
+
+.. image:: LR_LR.png
+
+With followng notations:
+
+- :math:`∠CLG = ∠BCL = ∠CBG = ∠LGB = A = u` [BGCL is an isoceles trapezium]
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - 2u + v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BC = CL = LG = 2` [2 * radius of curvature]
+- :math:`CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)` [Cosine rule in LGC]
+- :math:`CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)` [Cosine rule in LGC]
+- From the previous two equations: :math:`A = arccos(\frac{u1 + 2}{4})`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG + ∠GBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = arccos(\frac{u1 + 2}{4})`
+- :math:`t = \frac{π}{2} + θ + u`
+- :math:`v = (φ - t + 2u)`
+
+7. **LeftxRight-LeftxRight**
+
+.. image:: L_RL_R.png
+
+With followng notations:
+
+- :math:`∠GBC = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BC = CL = LG = 2` [2 * radius of curvature]
+- :math:`CD = 1` [radius of curvature]
+- D is midpoint of BG
+- :math:`BD = \frac{u1}{2}`
+- :math:`cos(u) = \frac{BC^2 + CD^2 - BD^2}{2*BC*CD}` [Cosine rule in BCD]
+- :math:`sin(A) = CD*\frac{sin(u)}{BD}` [Sine rule in BCD]
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG + ∠GBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = arccos(\frac{20 - u1^2}{16})`
+- :math:`A = arcsin(2*\frac{sin(u)}{u1})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ)`
+
+
+8. **LeftxRight90-Straight-Left**
+
+.. image:: L_R90SL.png
+
+With followng notations:
+
+- :math:`∠FBM = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBF = θ`
+- :math:`BF = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + \frac{π}{2} - v = φ`
+- :math:`F(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BM = CB = 2` [2 * radius of curvature]
+- :math:`MD = CD = 1` [CGDM is a rectangle]
+- :math:`MC = GD = u` [CGDM is a rectangle]
+- :math:`MF = MD + DF = 2`
+- :math:`BM = \sqrt{BF^2 - MF^2}` [Pythagoras theorem on BFM]
+- :math:`tan(A) = \frac{MF}{BM}`
+- :math:`u = MC = BM - CB`
+- :math:`t = ∠ABK + ∠KBF + ∠FBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(\sqrt{u1^2 - 4} - 2)`
+- :math:`A = arctan(\frac{2}{\sqrt{u1^2 - 4}})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ + \frac{π}{2})`
+
+
+9. **Left-Straight-Right90xLeft**
+
+.. image:: LSR90_L.png
+
+With followng notations:
+
+- :math:`∠MBH = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBH = θ`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - \frac{π}{2} - v = φ`
+- :math:`H(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`GH = 2` [2 * radius of curvature]
+- :math:`CM = DG = 1` [CGDM is a rectangle]
+- :math:`CD = MG = u` [CGDM is a rectangle]
+- :math:`BM = BC + CM = 2`
+- :math:`MH = \sqrt{BH^2 - BM^2}` [Pythagoras theorem on BHM]
+- :math:`tan(A) = \frac{HM}{BM}`
+- :math:`u = MC = BM - CB`
+- :math:`t = ∠ABK + ∠KBH - ∠HBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(\sqrt{u1^2 - 4} - 2)`
+- :math:`A = arctan(\frac{2}{\sqrt{u1^2 - 4}})`
+- :math:`t = \frac{π}{2} + θ - A`
+- :math:`v = (t - φ - \frac{π}{2})`
+
+
+10. **LeftxRight90-Straight-Right**
+
+.. image:: L_R90SR.png
+
+With followng notations:
+
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - \frac{π}{2} - v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BD = 2` [2 * radius of curvature]
+- :math:`DG = EF = u` [DGFE is a rectangle]
+- :math:`DG = BG - BD = 2`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = u1 - 2`
+- :math:`t = \frac{π}{2} + θ`
+- :math:`v = (t - φ - \frac{π}{2})`
+
+
+11. **Left-Straight-Left90xRight**
+
+.. image:: LSL90xR.png
+
+With followng notations:
+
+- :math:`∠KBH = θ`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + \frac{π}{2} + v = φ`
+- :math:`H(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`GH = 2` [2 * radius of curvature]
+- :math:`DC = BG = u` [DGBC is a rectangle]
+- :math:`BG = BH - GH`
+- :math:`∠ABC= ∠KBH`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = u1 - 2`
+- :math:`t = θ`
+- :math:`v = (φ - t - \frac{π}{2})`
+
+
+12. **LeftxRight90-Straight-Left90xRight**
+
+.. image:: L_R90SL90_R.png
+
+With followng notations:
+
+- :math:`∠KBH = θ`
+- :math:`∠HBM = A`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - v = φ`
+- :math:`H(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`GF = ED = 1` [radius of curvature]
+- :math:`BD = GH = 2` [2 * radius of curvature]
+- :math:`FN = GH = 2` [ENMD is a rectangle]
+- :math:`NH = GF = 1` [FNHG is a rectangle]
+- :math:`MN = ED = 1` [ENMD is a rectangle]
+- :math:`DO = EF = u` [DOFE is a rectangle]
+- :math:`MH = MN + NH = 2`
+- :math:`BM = \sqrt{BH^2 - MH^2}` [Pythagoras theorem on BHM]
+- :math:`DO = BM - BD - OM`
+- :math:`tan(A) = \frac{MH}{BM}`
+- :math:`∠ABC = ∠ABK + ∠KBH + ∠HBM`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = /sqrt{u1^2 - 4} - 4`
+- :math:`A = arctan(\frac{2}{u1^2 - 4})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ)`
+
+
+Reference
+=============
+
+- `15.3.2 Reeds-Shepp
+ Curves `__
+
+- `optimal paths for a car that goes both forwards and
+ backwards `__
+
+- `ghliu/pyReedsShepp: Implementation of Reeds Shepp
+ curve. `__
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_1.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_1.png
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_3.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_3.png
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_4.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_4.png
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_5.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_5.png
diff --git a/docs/modules/path_planning/rrt/figure_1.png b/docs/modules/5_path_planning/rrt/figure_1.png
similarity index 100%
rename from docs/modules/path_planning/rrt/figure_1.png
rename to docs/modules/5_path_planning/rrt/figure_1.png
diff --git a/docs/modules/path_planning/rrt/rrt_main.rst b/docs/modules/5_path_planning/rrt/rrt_main.rst
similarity index 74%
rename from docs/modules/path_planning/rrt/rrt_main.rst
rename to docs/modules/5_path_planning/rrt/rrt_main.rst
index 5a3a30dcff..1bd5497f4c 100644
--- a/docs/modules/path_planning/rrt/rrt_main.rst
+++ b/docs/modules/5_path_planning/rrt/rrt_main.rst
@@ -14,6 +14,12 @@ This is a simple path planning code with Rapidly-Exploring Random Trees
Black circles are obstacles, green line is a searched tree, red crosses
are start and goal positions.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRT.rrt.RRT
+
+
.. include:: rrt_star.rst
@@ -24,6 +30,12 @@ RRT with dubins path
Path planning for a car robot with RRT and dubins path planner.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTDubins.rrt_dubins.RRTDubins
+
+
.. _rrt*-with-dubins-path:
RRT\* with dubins path
@@ -33,6 +45,12 @@ RRT\* with dubins path
Path planning for a car robot with RRT\* and dubins path planner.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTStarDubins.rrt_star_dubins.RRTStarDubins
+
+
.. _rrt*-with-reeds-sheep-path:
RRT\* with reeds-sheep path
@@ -42,6 +60,12 @@ RRT\* with reeds-sheep path
Path planning for a car robot with RRT\* and reeds sheep path planner.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTStarReedsShepp.rrt_star_reeds_shepp.RRTStarReedsShepp
+
+
.. _informed-rrt*:
Informed RRT\*
@@ -53,11 +77,18 @@ This is a path planning code with Informed RRT*.
The cyan ellipse is the heuristic sampling domain of Informed RRT*.
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.InformedRRTStar.informed_rrt_star.InformedRRTStar
+
+
+Reference
+^^^^^^^^^^
- `Informed RRT\*: Optimal Sampling-based Path Planning Focused via
Direct Sampling of an Admissible Ellipsoidal
- Heuristic `__
+ Heuristic `__
.. _batch-informed-rrt*:
@@ -68,12 +99,20 @@ Batch Informed RRT\*
This is a path planning code with Batch Informed RRT*.
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.BatchInformedRRTStar.batch_informed_rrt_star.BITStar
+
+
+Reference
+^^^^^^^^^^^
- `Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the
Heuristically Guided Search of Implicit Random Geometric
Graphs `__
+
.. _closed-loop-rrt*:
Closed Loop RRT\*
@@ -87,17 +126,25 @@ In this code, pure-pursuit algorithm is used for steering control,
PID is used for speed control.
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.ClosedLoopRRTStar.closed_loop_rrt_star_car.ClosedLoopRRTStar
+
+
+Reference
+^^^^^^^^^^^^
- `Motion Planning in Complex Environments using Closed-loop
- Prediction `__
+ Prediction `__
- `Real-time Motion Planning with Applications to Autonomous Urban
- Driving `__
+ Driving `__
- `[1601.06326] Sampling-based Algorithms for Optimal Motion Planning
Using Closed-loop Prediction `__
+
.. _lqr-rrt*:
LQR-RRT\*
@@ -109,10 +156,17 @@ A double integrator motion model is used for LQR local planner.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.LQRRRTStar.lqr_rrt_star.LQRRRTStar
+
+
+Reference
+~~~~~~~~~~~~~
- `LQR-RRT\*: Optimal Sampling-Based Motion Planning with Automatically
Derived Extension
- Heuristics `__
+ Heuristics `__
- `MahanFathi/LQR-RRTstar: LQR-RRT\* method is used for random motion planning of a simple pendulum in its phase plot `__
\ No newline at end of file
diff --git a/docs/modules/path_planning/rrt/rrt_star.rst b/docs/modules/5_path_planning/rrt/rrt_star.rst
similarity index 82%
rename from docs/modules/path_planning/rrt/rrt_star.rst
rename to docs/modules/5_path_planning/rrt/rrt_star.rst
index d36eaac70e..960b9976d5 100644
--- a/docs/modules/path_planning/rrt/rrt_star.rst
+++ b/docs/modules/5_path_planning/rrt/rrt_star.rst
@@ -7,6 +7,12 @@ 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.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTStar.rrt_star.RRTStar
+
+
Simulation
^^^^^^^^^^
@@ -16,6 +22,6 @@ Simulation
Ref
^^^
-- `Sampling-based Algorithms for Optimal Motion Planning `__
+- `Sampling-based Algorithms for Optimal Motion Planning `__
- `Incremental Sampling-based Algorithms for Optimal Motion Planning `__
diff --git a/docs/modules/path_planning/rrt/rrt_star/rrt_star_1_0.png b/docs/modules/5_path_planning/rrt/rrt_star/rrt_star_1_0.png
similarity index 100%
rename from docs/modules/path_planning/rrt/rrt_star/rrt_star_1_0.png
rename to docs/modules/5_path_planning/rrt/rrt_star/rrt_star_1_0.png
diff --git a/docs/modules/path_planning/rrt/rrt_star_reeds_shepp/figure_1.png b/docs/modules/5_path_planning/rrt/rrt_star_reeds_shepp/figure_1.png
similarity index 100%
rename from docs/modules/path_planning/rrt/rrt_star_reeds_shepp/figure_1.png
rename to docs/modules/5_path_planning/rrt/rrt_star_reeds_shepp/figure_1.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_1.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_1.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_1.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_1.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_2.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_2.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_2.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_2.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_3.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_3.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_3.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_3.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_4.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_4.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_4.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_4.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_5.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_5.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_5.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_5.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_6.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_6.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_6.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_6.png
diff --git a/docs/modules/path_planning/state_lattice_planner/state_lattice_planner_main.rst b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst
similarity index 61%
rename from docs/modules/path_planning/state_lattice_planner/state_lattice_planner_main.rst
rename to docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst
index 3ef0c7eca2..9f8cc0c50f 100644
--- a/docs/modules/path_planning/state_lattice_planner/state_lattice_planner_main.rst
+++ b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst
@@ -12,22 +12,39 @@ Uniform polar sampling
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif
+Code Link
+^^^^^^^^^^^^^
+
+.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_uniform_polar_states
+
+
Biased polar sampling
~~~~~~~~~~~~~~~~~~~~~
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/BiasedPolarSampling.gif
+Code Link
+^^^^^^^^^^^^^
+.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_biased_polar_states
+
+
Lane sampling
~~~~~~~~~~~~~
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif
-Ref:
+Code Link
+^^^^^^^^^^^^^
+.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_lane_states
+
+
+Reference
+~~~~~~~~~~~~~~~
- `Optimal rough terrain trajectory generation for wheeled mobile
- robots `__
+ robots `__
- `State Space Sampling of Feasible Motions for High-Performance Mobile
Robot Navigation in Complex
- Environments `__
+ Environments `__
diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst
new file mode 100644
index 0000000000..a44dd20a97
--- /dev/null
+++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst
@@ -0,0 +1,108 @@
+Time based grid search
+----------------------
+
+Space-time astar
+~~~~~~~~~~~~~~~~~~~~~~
+
+This is an extension of A* algorithm that supports planning around dynamic obstacles.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar/path_animation.gif
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar/path_animation2.gif
+
+The key difference of this algorithm compared to vanilla A* is that the cost and heuristic are now time-based instead of distance-based.
+Using a time-based cost and heuristic ensures the path found is optimal in terms of time to reach the goal.
+
+The cost is the amount of time it takes to reach a given node, and the heuristic is the minimum amount of time it could take to reach the goal from that node, disregarding all obstacles.
+For a simple scenario where the robot can move 1 cell per time step and stop and go as it pleases, the heuristic for time is equivalent to the heuristic for distance.
+
+One optimization that was added in `this PR `__ was to add an expanded set to the algorithm. The algorithm will not expand nodes that are already in that set. This greatly reduces the number of node expansions needed to find a path, since no duplicates are expanded. It also helps to reduce the amount of memory the algorithm uses.
+
+Before::
+
+ Found path to goal after 204490 expansions
+ Planning took: 1.72464 seconds
+ Memory usage (RSS): 68.19 MB
+
+
+After::
+
+ Found path to goal after 2348 expansions
+ Planning took: 0.01550 seconds
+ Memory usage (RSS): 64.85 MB
+
+When starting at (1, 11) in the structured obstacle arrangement (second of the two gifs above).
+
+Code Link
+^^^^^^^^^^^^^
+.. autoclass:: PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar.SpaceTimeAStar
+
+
+Safe Interval Path Planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The safe interval path planning algorithm is described in this paper:
+
+`SIPP: Safe Interval Path Planning for Dynamic Environments `__
+
+It is faster than space-time A* because it pre-computes the intervals of time that are unoccupied in each cell. This allows it to reduce the number of successor node it generates by avoiding nodes within the same interval.
+
+**Comparison with SpaceTime A*:**
+
+Arrangement 1 starting at (1, 18)
+
+SafeInterval planner::
+
+ Found path to goal after 322 expansions
+ Planning took: 0.00730 seconds
+
+SpaceTime A*::
+
+ Found path to goal after 2717154 expansions
+ Planning took: 20.51330 seconds
+
+**Benchmarking the Safe Interval Path Planner:**
+
+250 random obstacles::
+
+ Found path to goal after 764 expansions
+ Planning took: 0.60596 seconds
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/SafeIntervalPathPlanner/path_animation.gif
+
+Arrangement 1 starting at (1, 18)::
+
+ Found path to goal after 322 expansions
+ Planning took: 0.00730 seconds
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/SafeIntervalPathPlanner/path_animation2.gif
+
+Code Link
+^^^^^^^^^^^^^
+.. autoclass:: PathPlanning.TimeBasedPathPlanning.SafeInterval.SafeIntervalPathPlanner
+
+Multi-Agent Path Planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Priority Based Planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The planner generates an order to plan in, and generates plans for the robots in that order. Each planned path is reserved in the grid, and all future plans must avoid that path. The robots are planned for in descending order of distance from start to goal.
+
+This is a sub-optimal algorithm because no replanning happens once paths are found. It does not guarantee the shortest path is found for any particular robot, nor that the final set of paths found contains the lowest possible amount of time or movement.
+
+Static Obstacles:
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner/priority_planner2.gif
+
+Dynamic Obstacles:
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner/priority_planner.gif
+
+
+References
+~~~~~~~~~~~
+
+- `Cooperative Pathfinding `__
+- `SIPP: Safe Interval Path Planning for Dynamic Environments `__
+- `Prioritized Planning Algorithms for Trajectory Coordination of Multiple Mobile Robots `__
\ No newline at end of file
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step0.png b/docs/modules/5_path_planning/visibility_road_map_planner/step0.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step0.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step0.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step1.png b/docs/modules/5_path_planning/visibility_road_map_planner/step1.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step1.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step1.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step2.png b/docs/modules/5_path_planning/visibility_road_map_planner/step2.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step2.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step2.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step3.png b/docs/modules/5_path_planning/visibility_road_map_planner/step3.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step3.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step3.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst b/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
similarity index 94%
rename from docs/modules/path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
rename to docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
index 3c9b7c008c..aac96d6e19 100644
--- a/docs/modules/path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
+++ b/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
@@ -13,6 +13,11 @@ red crosses are visibility nodes, and blue lines area collision free visibility
The red line is the final path searched by dijkstra algorithm frm the visibility graphs.
+Code Link
+~~~~~~~~~~~~
+.. autoclass:: PathPlanning.VisibilityRoadMap.visibility_road_map.VisibilityRoadMap
+
+
Algorithms
~~~~~~~~~~
@@ -64,7 +69,7 @@ The red line is searched path in the figure:
You can find the details of Dijkstra algorithm in :ref:`dijkstra`.
References
-^^^^^^^^^^
+~~~~~~~~~~~~
- `Visibility graph - Wikipedia `_
diff --git a/docs/modules/path_planning/vrm_planner/vrm_planner_main.rst b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
similarity index 79%
rename from docs/modules/path_planning/vrm_planner/vrm_planner_main.rst
rename to docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
index 92e729ab29..a9a41aab74 100644
--- a/docs/modules/path_planning/vrm_planner/vrm_planner_main.rst
+++ b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
@@ -11,7 +11,13 @@ Cyan crosses mean searched points with Dijkstra method,
The red line is the final path of Vornoi Road-Map.
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+.. autoclass:: PathPlanning.VoronoiRoadMap.voronoi_road_map.VoronoiRoadMapPlanner
+
+
+Reference
+~~~~~~~~~~~~
- `Robotic Motion Planning `__
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
similarity index 92%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
index a1980ba17e..914a4555ff 100644
--- a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
+++ b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
@@ -17,6 +17,11 @@ Nonlinear Model Predictive Control with C-GMRES
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif
:alt: gif
+Code Link
+~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.cgmres_nmpc.cgmres_nmpc.NMPCControllerCGMRES
+
Mathematical Formulation
~~~~~~~~~~~~~~~~~~~~~~~~
@@ -60,7 +65,7 @@ Ref
- `Shunichi09/nonlinear_control: Implementing the nonlinear model
predictive control, sliding mode
- control `__
+ control `__
- `非線形モデル予測制御におけるCGMRES法をpythonで実装する -
Qiita `__
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
new file mode 100644
index 0000000000..b0ba9e0d45
--- /dev/null
+++ b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
@@ -0,0 +1,145 @@
+.. _linearquadratic-regulator-(lqr)-speed-and-steering-control:
+
+Linear–quadratic regulator (LQR) speed and steering control
+-----------------------------------------------------------
+
+Path tracking simulation with LQR speed and steering control.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif
+
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.lqr_speed_steer_control.lqr_speed_steer_control.lqr_speed_steering_control
+
+
+Overview
+~~~~~~~~
+
+The LQR (Linear Quadratic Regulator) speed and steering control model implemented in `lqr_speed_steer_control.py` provides a simulation
+for an autonomous vehicle to track:
+
+1. A desired speed by adjusting acceleration based on feedback from the current state and the desired speed.
+
+2. A desired trajectory by adjusting steering angle based on feedback from the current state and the desired trajectory.
+
+by only using one LQT controller.
+
+Vehicle motion Model
+~~~~~~~~~~~~~~~~~~~~~
+
+The below figure shows the geometric model of the vehicle used in this simulation:
+
+.. image:: lqr_steering_control_model.jpg
+ :width: 600px
+
+The `e`, :math:`{\theta}`, and :math:`\upsilon` represent the lateral error, orientation error, and velocity error, respectively, with respect to the desired trajectory and speed.
+And :math:`\dot{e}` and :math:`\dot{\theta}` represent the rates of change of these errors.
+
+The :math:`e_t` and :math:`\theta_t`, and :math:`\upsilon` are the updated values of `e`, :math:`\theta`, :math:`\upsilon` and at time `t`, respectively, and can be calculated using the following kinematic equations:
+
+.. math:: e_t = e_{t-1} + \dot{e}_{t-1} dt
+
+.. math:: \theta_t = \theta_{t-1} + \dot{\theta}_{t-1} dt
+
+.. math:: \upsilon_t = \upsilon_{t-1} + a_{t-1} dt
+
+Where `dt` is the time difference and :math:`a_t` is the acceleration at the time `t`.
+
+The change rate of the `e` can be calculated as:
+
+.. math:: \dot{e}_t = V \sin(\theta_{t-1})
+
+Where `V` is the current vehicle speed.
+
+If the :math:`\theta` is small,
+
+.. math:: \theta \approx 0
+
+the :math:`\sin(\theta)` can be approximated as :math:`\theta`:
+
+.. math:: \sin(\theta) = \theta
+
+So, the change rate of the `e` can be approximated as:
+
+.. math:: \dot{e}_t = V \theta_{t-1}
+
+The change rate of the :math:`\theta` can be calculated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \tan(\delta)
+
+where `L` is the wheelbase of the vehicle and :math:`\delta` is the steering angle.
+
+If the :math:`\delta` is small,
+
+.. math:: \delta \approx 0
+
+the :math:`\tan(\delta)` can be approximated as :math:`\delta`:
+
+.. math:: \tan(\delta) = \delta
+
+So, the change rate of the :math:`\theta` can be approximated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \delta
+
+The above equations can be used to update the state of the vehicle at each time step.
+
+Control Model
+~~~~~~~~~~~~~~
+
+To formulate the state-space representation of the vehicle dynamics as a linear model,
+the state vector `x` and control input vector `u` are defined as follows:
+
+.. math:: x_t = [e_t, \dot{e}_t, \theta_t, \dot{\theta}_t, \upsilon_t]^T
+
+.. math:: u_t = [\delta_t, a_t]^T
+
+The linear state transition equation can be represented as:
+
+.. math:: x_{t+1} = A x_t + B u_t
+
+where:
+
+:math:`\begin{equation*} A = \begin{bmatrix} 1 & dt & 0 & 0 & 0\\ 0 & 0 & v & 0 & 0\\ 0 & 0 & 1 & dt & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\\end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} B = \begin{bmatrix} 0 & 0\\ 0 & 0\\ 0 & 0\\ \frac{v}{L} & 0\\ 0 & dt \\ \end{bmatrix} \end{equation*}`
+
+LQR controller
+~~~~~~~~~~~~~~~
+
+The Linear Quadratic Regulator (LQR) controller is used to calculate the optimal control input `u` that minimizes the quadratic cost function:
+
+:math:`J = \sum_{t=0}^{N} (x_t^T Q x_t + u_t^T R u_t)`
+
+where `Q` and `R` are the weighting matrices for the state and control input, respectively.
+
+for the linear model:
+
+:math:`x_{t+1} = A x_t + B u_t`
+
+The optimal control input `u` can be calculated as:
+
+:math:`u_t = -K x_t`
+
+where `K` is the feedback gain matrix obtained by solving the Riccati equation.
+
+Simulation results
+~~~~~~~~~~~~~~~~~~~
+
+
+.. image:: x-y.png
+ :width: 600px
+
+.. image:: yaw.png
+ :width: 600px
+
+.. image:: speed.png
+ :width: 600px
+
+
+
+Reference
+~~~~~~~~~~~
+
+- `Towards fully autonomous driving: Systems and algorithms `__
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg
new file mode 100644
index 0000000000..83754d5bb0
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg differ
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/speed.png b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/speed.png
new file mode 100644
index 0000000000..ae99eb7ea3
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/speed.png differ
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/x-y.png b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/x-y.png
new file mode 100644
index 0000000000..bff3f1a786
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/x-y.png differ
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/yaw.png b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/yaw.png
new file mode 100644
index 0000000000..7f3d9c1d10
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/yaw.png differ
diff --git a/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst
new file mode 100644
index 0000000000..fc8f2a33aa
--- /dev/null
+++ b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst
@@ -0,0 +1,125 @@
+.. _linearquadratic-regulator-(lqr)-steering-control:
+
+Linear–quadratic regulator (LQR) steering control
+-------------------------------------------------
+
+Path tracking simulation with LQR steering control and PID speed
+control.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.lqr_steer_control.lqr_steer_control.lqr_steering_control
+
+
+Overview
+~~~~~~~~
+
+The LQR (Linear Quadratic Regulator) steering control model implemented in lqr_steer_control.py provides a simulation
+for an autonomous vehicle to track a desired trajectory by adjusting steering angle based on feedback from the current state and the desired trajectory.
+This model utilizes a combination of PID speed control and LQR steering control to achieve smooth and accurate trajectory tracking.
+
+Vehicle motion Model
+~~~~~~~~~~~~~~~~~~~~~
+
+The below figure shows the geometric model of the vehicle used in this simulation:
+
+.. image:: lqr_steering_control_model.jpg
+ :width: 600px
+
+The `e` and :math:`\theta` represent the lateral error and orientation error, respectively, with respect to the desired trajectory.
+And :math:`\dot{e}` and :math:`\dot{\theta}` represent the rates of change of these errors.
+
+The :math:`e_t` and :math:`\theta_t` are the updated values of `e` and :math:`\theta` at time `t`, respectively, and can be calculated using the following kinematic equations:
+
+.. math:: e_t = e_{t-1} + \dot{e}_{t-1} dt
+
+.. math:: \theta_t = \theta_{t-1} + \dot{\theta}_{t-1} dt
+
+Where `dt` is the time difference.
+
+The change rate of the `e` can be calculated as:
+
+.. math:: \dot{e}_t = V \sin(\theta_{t-1})
+
+Where `V` is the current vehicle speed.
+
+If the :math:`\theta` is small,
+
+.. math:: \theta \approx 0
+
+the :math:`\sin(\theta)` can be approximated as :math:`\theta`:
+
+.. math:: \sin(\theta) = \theta
+
+So, the change rate of the `e` can be approximated as:
+
+.. math:: \dot{e}_t = V \theta_{t-1}
+
+The change rate of the :math:`\theta` can be calculated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \tan(\delta)
+
+where `L` is the wheelbase of the vehicle and :math:`\delta` is the steering angle.
+
+If the :math:`\delta` is small,
+
+.. math:: \delta \approx 0
+
+the :math:`\tan(\delta)` can be approximated as :math:`\delta`:
+
+.. math:: \tan(\delta) = \delta
+
+So, the change rate of the :math:`\theta` can be approximated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \delta
+
+The above equations can be used to update the state of the vehicle at each time step.
+
+Control Model
+~~~~~~~~~~~~~~
+
+To formulate the state-space representation of the vehicle dynamics as a linear model,
+the state vector `x` and control input vector `u` are defined as follows:
+
+.. math:: x_t = [e_t, \dot{e}_t, \theta_t, \dot{\theta}_t]^T
+
+.. math:: u_t = \delta_t
+
+The linear state transition equation can be represented as:
+
+.. math:: x_{t+1} = A x_t + B u_t
+
+where:
+
+:math:`\begin{equation*} A = \begin{bmatrix} 1 & dt & 0 & 0\\ 0 & 0 & v & 0\\ 0 & 0 & 1 & dt\\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} B = \begin{bmatrix} 0\\ 0\\ 0\\ \frac{v}{L} \\ \end{bmatrix} \end{equation*}`
+
+LQR controller
+~~~~~~~~~~~~~~~
+
+The Linear Quadratic Regulator (LQR) controller is used to calculate the optimal control input `u` that minimizes the quadratic cost function:
+
+:math:`J = \sum_{t=0}^{N} (x_t^T Q x_t + u_t^T R u_t)`
+
+where `Q` and `R` are the weighting matrices for the state and control input, respectively.
+
+for the linear model:
+
+:math:`x_{t+1} = A x_t + B u_t`
+
+The optimal control input `u` can be calculated as:
+
+:math:`u_t = -K x_t`
+
+where `K` is the feedback gain matrix obtained by solving the Riccati equation.
+
+Reference
+~~~~~~~~~~~
+- `ApolloAuto/apollo: An open autonomous driving platform `_
+
+- `Linear Quadratic Regulator (LQR) `_
+
diff --git a/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_model.jpg b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_model.jpg
new file mode 100644
index 0000000000..83754d5bb0
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_model.jpg differ
diff --git a/docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst b/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
similarity index 92%
rename from docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
rename to docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
index 59a7166674..76a6819a46 100644
--- a/docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
+++ b/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
@@ -5,13 +5,6 @@ Model predictive speed and steering control
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif?raw=true
:alt: Model predictive speed and steering control
- Model predictive speed and steering control
-
-code:
-
-`PythonRobotics/model_predictive_speed_and_steer_control.py at master ·
-AtsushiSakai/PythonRobotics `__
-
This is a path tracking simulation using model predictive control (MPC).
The MPC controller controls vehicle speed and steering base on
@@ -22,6 +15,12 @@ This code uses cvxpy as an optimization modeling tool.
- `Welcome to CVXPY 1.0 — CVXPY 1.0.6
documentation `__
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.model_predictive_speed_and_steer_control.model_predictive_speed_and_steer_control.iterative_linear_mpc_control
+
+
MPC modeling
~~~~~~~~~~~~
@@ -133,5 +132,5 @@ Reference
- `Vehicle Dynamics and Control \| Rajesh Rajamani \|
Springer `__
-- `MPC Course Material - MPC Lab @
- UC-Berkeley `__
+- `MPC Book - MPC Lab @
+ UC-Berkeley `__
diff --git a/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control_main.rst b/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst
similarity index 97%
rename from docs/modules/control/move_to_a_pose_control/move_to_a_pose_control_main.rst
rename to docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst
index 77ec682a30..19bb0e4c14 100644
--- a/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control_main.rst
+++ b/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst
@@ -3,7 +3,13 @@ Move to a Pose Control
In this section, we present the logic of PathFinderController that drives a car from a start pose (x, y, theta) to a goal pose. A simulation of moving to a pose control is presented below.
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/move_to_pose/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.move_to_pose.move_to_pose.move_to_pose
+
Position Control of non-Holonomic Systems
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
diff --git a/docs/modules/6_path_tracking/path_tracking_main.rst b/docs/modules/6_path_tracking/path_tracking_main.rst
new file mode 100644
index 0000000000..742cc807e6
--- /dev/null
+++ b/docs/modules/6_path_tracking/path_tracking_main.rst
@@ -0,0 +1,19 @@
+.. _`Path Tracking`:
+
+Path Tracking
+=============
+
+Path tracking is the ability of a robot to follow the reference path generated by a path planner while simultaneously stabilizing the robot. The path tracking controller may need to account for modeling error and other forms of uncertainty. In path tracking, feedback control techniques and optimization based control techniques are widely used[22]. Fig.6 shows simulations using rear wheel feedback steering control and PID speed control, and iterative linear model predictive path tracking control[27].
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ pure_pursuit_tracking/pure_pursuit_tracking
+ stanley_control/stanley_control
+ rear_wheel_feedback_control/rear_wheel_feedback_control
+ lqr_steering_control/lqr_steering_control
+ lqr_speed_and_steering_control/lqr_speed_and_steering_control
+ model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control
+ cgmres_nmpc/cgmres_nmpc
+ move_to_a_pose/move_to_a_pose
diff --git a/docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
similarity index 80%
rename from docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
rename to docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
index 5c7bcef85f..ff6749bbbe 100644
--- a/docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
+++ b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
@@ -9,7 +9,13 @@ speed control.
The red line is a target course, the green cross means the target point
for pure pursuit control, the blue line is the tracking.
-References:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.pure_pursuit.pure_pursuit.pure_pursuit_steer_control
+
+
+Reference
~~~~~~~~~~~
- `A Survey of Motion Planning and Control Techniques for Self-driving
diff --git a/docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst b/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
similarity index 72%
rename from docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
rename to docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
index 70875fdc6c..56d0db63ad 100644
--- a/docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
+++ b/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
@@ -6,7 +6,13 @@ PID speed control.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif
-References:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.rear_wheel_feedback_control.rear_wheel_feedback_control.rear_wheel_feedback_control
+
+
+Reference
~~~~~~~~~~~
- `A Survey of Motion Planning and Control Techniques for Self-driving
Urban Vehicles `__
diff --git a/docs/modules/path_tracking/stanley_control/stanley_control_main.rst b/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst
similarity index 82%
rename from docs/modules/path_tracking/stanley_control/stanley_control_main.rst
rename to docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst
index fe325b0102..3c491804f6 100644
--- a/docs/modules/path_tracking/stanley_control/stanley_control_main.rst
+++ b/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst
@@ -6,7 +6,13 @@ control.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif
-References:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.stanley_control.stanley_control.stanley_control
+
+
+Reference
~~~~~~~~~~~
- `Stanley: The robot that won the DARPA grand
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png
diff --git a/docs/modules/arm_navigation/arm_navigation_main.rst b/docs/modules/7_arm_navigation/arm_navigation_main.rst
similarity index 88%
rename from docs/modules/arm_navigation/arm_navigation_main.rst
rename to docs/modules/7_arm_navigation/arm_navigation_main.rst
index bbbc872c58..7acd3ee7d3 100644
--- a/docs/modules/arm_navigation/arm_navigation_main.rst
+++ b/docs/modules/7_arm_navigation/arm_navigation_main.rst
@@ -1,4 +1,4 @@
-.. _arm_navigation:
+.. _`Arm Navigation`:
Arm Navigation
==============
diff --git a/docs/modules/arm_navigation/n_joint_arm_to_point_control_main.rst b/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst
similarity index 76%
rename from docs/modules/arm_navigation/n_joint_arm_to_point_control_main.rst
rename to docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst
index cc6279f681..56900acde1 100644
--- a/docs/modules/arm_navigation/n_joint_arm_to_point_control_main.rst
+++ b/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst
@@ -11,3 +11,10 @@ plotting area.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/n_joint_arm_to_point_control/animation.gif
In this simulation N = 10, however, you can change it.
+
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: ArmNavigation.n_joint_arm_to_point_control.n_joint_arm_to_point_control.main
+
diff --git a/docs/modules/arm_navigation/obstacle_avoidance_arm_navigation_main.rst b/docs/modules/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst
similarity index 52%
rename from docs/modules/arm_navigation/obstacle_avoidance_arm_navigation_main.rst
rename to docs/modules/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst
index 899a64a5cd..4433ab531b 100644
--- a/docs/modules/arm_navigation/obstacle_avoidance_arm_navigation_main.rst
+++ b/docs/modules/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst
@@ -3,4 +3,9 @@ Arm navigation with obstacle avoidance
Arm navigation with obstacle avoidance simulation.
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/arm_obstacle_navigation/animation.gif
\ No newline at end of file
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/arm_obstacle_navigation/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: ArmNavigation.arm_obstacle_navigation.arm_obstacle_navigation.main
diff --git a/docs/modules/arm_navigation/planar_two_link_ik_main.rst b/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst
similarity index 98%
rename from docs/modules/arm_navigation/planar_two_link_ik_main.rst
rename to docs/modules/7_arm_navigation/planar_two_link_ik_main.rst
index 2da2b0dc30..5b2712eb48 100644
--- a/docs/modules/arm_navigation/planar_two_link_ik_main.rst
+++ b/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst
@@ -11,6 +11,12 @@ This is a interactive simulation.
You can set the goal position of the end effector with left-click on the plotting area.
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: ArmNavigation.two_joint_arm_to_point_control.two_joint_arm_to_point_control.main
+
+
Inverse Kinematics for a Planar Two-Link Robotic Arm
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
diff --git a/docs/modules/aerial_navigation/aerial_navigation_main.rst b/docs/modules/8_aerial_navigation/aerial_navigation_main.rst
similarity index 89%
rename from docs/modules/aerial_navigation/aerial_navigation_main.rst
rename to docs/modules/8_aerial_navigation/aerial_navigation_main.rst
index b2ccb071af..7f76689770 100644
--- a/docs/modules/aerial_navigation/aerial_navigation_main.rst
+++ b/docs/modules/8_aerial_navigation/aerial_navigation_main.rst
@@ -1,4 +1,4 @@
-.. _aerial_navigation:
+.. _`Aerial Navigation`:
Aerial Navigation
=================
diff --git a/docs/modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst b/docs/modules/8_aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
similarity index 66%
rename from docs/modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
rename to docs/modules/8_aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
index c3bdc33cdc..1805bb3f6d 100644
--- a/docs/modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
+++ b/docs/modules/8_aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
@@ -4,3 +4,8 @@ Drone 3d trajectory following
This is a 3d trajectory following simulation for a quadrotor.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/drone_3d_trajectory_following/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: AerialNavigation.drone_3d_trajectory_following.drone_3d_trajectory_following.quad_sim
diff --git a/docs/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst b/docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
similarity index 96%
rename from docs/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
rename to docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
index 6c90d2b44e..4bf5117500 100644
--- a/docs/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
+++ b/docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
@@ -1,6 +1,14 @@
Rocket powered landing
-----------------------------
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/rocket_powered_landing/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: AerialNavigation.rocket_powered_landing.rocket_powered_landing.main
+
+
Simulation
~~~~~~~~~~
diff --git a/docs/modules/bipedal/bipedal_main.rst b/docs/modules/9_bipedal/bipedal_main.rst
similarity index 88%
rename from docs/modules/bipedal/bipedal_main.rst
rename to docs/modules/9_bipedal/bipedal_main.rst
index fc5b933055..dc387dc4e8 100644
--- a/docs/modules/bipedal/bipedal_main.rst
+++ b/docs/modules/9_bipedal/bipedal_main.rst
@@ -1,4 +1,4 @@
-.. _bipedal:
+.. _`Bipedal`:
Bipedal
=================
diff --git a/docs/modules/bipedal/bipedal_planner/bipedal_planner_main.rst b/docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst
similarity index 67%
rename from docs/modules/bipedal/bipedal_planner/bipedal_planner_main.rst
rename to docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst
index 2ee5971e7a..6253715cc1 100644
--- a/docs/modules/bipedal/bipedal_planner/bipedal_planner_main.rst
+++ b/docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst
@@ -4,3 +4,8 @@ Bipedal Planner
Bipedal Walking with modifying designated footsteps
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Bipedal/bipedal_planner/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: Bipedal.bipedal_planner.bipedal_planner.BipedalPlanner
diff --git a/docs/modules/control/control_main.rst b/docs/modules/control/control_main.rst
deleted file mode 100644
index cee2aa9e8e..0000000000
--- a/docs/modules/control/control_main.rst
+++ /dev/null
@@ -1,12 +0,0 @@
-.. _control:
-
-Control
-=================
-
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- inverted_pendulum_control/inverted_pendulum_control
- move_to_a_pose_control/move_to_a_pose_control
-
diff --git a/docs/modules/introduction_main.rst b/docs/modules/introduction_main.rst
deleted file mode 100644
index f9fb487297..0000000000
--- a/docs/modules/introduction_main.rst
+++ /dev/null
@@ -1,42 +0,0 @@
-
-Introduction
-============
-
-TBD
-
-Definition Of Robotics
-----------------------
-
-TBD
-
-History Of Robotics
-----------------------
-
-TBD
-
-Application Of Robotics
-------------------------
-
-TBD
-
-Software for Robotics
-----------------------
-
-TBD
-
-Software for Robotics
-----------------------
-
-TBD
-
-Python for Robotics
-----------------------
-
-TBD
-
-Learning Robotics Algorithms
-----------------------------
-
-TBD
-
-
diff --git a/docs/modules/mapping/mapping_main.rst b/docs/modules/mapping/mapping_main.rst
deleted file mode 100644
index 0f7ec549dc..0000000000
--- a/docs/modules/mapping/mapping_main.rst
+++ /dev/null
@@ -1,15 +0,0 @@
-.. _mapping:
-
-Mapping
-=======
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- gaussian_grid_map/gaussian_grid_map
- ray_casting_grid_map/ray_casting_grid_map
- lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial
- k_means_object_clustering/k_means_object_clustering
- circle_fitting/circle_fitting
- rectangle_fitting/rectangle_fitting
-
diff --git a/docs/modules/mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst b/docs/modules/mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
deleted file mode 100644
index cc5a1a1c5b..0000000000
--- a/docs/modules/mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
+++ /dev/null
@@ -1,6 +0,0 @@
-Ray casting grid map
---------------------
-
-This is a 2D ray casting grid mapping example.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/raycasting_grid_map/animation.gif
\ No newline at end of file
diff --git a/docs/modules/mapping/rectangle_fitting/rectangle_fitting_main.rst b/docs/modules/mapping/rectangle_fitting/rectangle_fitting_main.rst
deleted file mode 100644
index c0949ac4c3..0000000000
--- a/docs/modules/mapping/rectangle_fitting/rectangle_fitting_main.rst
+++ /dev/null
@@ -1,7 +0,0 @@
-Object shape recognition using rectangle fitting
-------------------------------------------------
-
-This is an object shape recognition using rectangle fitting.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/rectangle_fitting/animation.gif
-
diff --git a/docs/modules/path_planning/bspline_path/Figure_1.png b/docs/modules/path_planning/bspline_path/Figure_1.png
deleted file mode 100644
index 539854ac29..0000000000
Binary files a/docs/modules/path_planning/bspline_path/Figure_1.png and /dev/null differ
diff --git a/docs/modules/path_planning/bspline_path/bspline_path_main.rst b/docs/modules/path_planning/bspline_path/bspline_path_main.rst
deleted file mode 100644
index d3523a1918..0000000000
--- a/docs/modules/path_planning/bspline_path/bspline_path_main.rst
+++ /dev/null
@@ -1,14 +0,0 @@
-B-Spline planning
------------------
-
-.. image:: Figure_1.png
-
-This is a path planning with B-Spline curse.
-
-If you input waypoints, it generates a smooth path with B-Spline curve.
-
-The final course should be on the first and last waypoints.
-
-Ref:
-
-- `B-spline - Wikipedia `__
diff --git a/docs/modules/path_planning/bugplanner/bugplanner_main.rst b/docs/modules/path_planning/bugplanner/bugplanner_main.rst
deleted file mode 100644
index 72cc46709f..0000000000
--- a/docs/modules/path_planning/bugplanner/bugplanner_main.rst
+++ /dev/null
@@ -1,8 +0,0 @@
-Bug planner
------------
-
-This is a 2D planning with Bug algorithm.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BugPlanner/animation.gif
-
-- `ECE452 Bug Algorithms `_
diff --git a/docs/modules/path_planning/frenet_frame_path/frenet_frame_path_main.rst b/docs/modules/path_planning/frenet_frame_path/frenet_frame_path_main.rst
deleted file mode 100644
index e9d9041e0e..0000000000
--- a/docs/modules/path_planning/frenet_frame_path/frenet_frame_path_main.rst
+++ /dev/null
@@ -1,20 +0,0 @@
-Optimal Trajectory in a Frenet Frame
-------------------------------------
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif
-
-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 predicted path.
-
-Ref:
-
-- `Optimal Trajectory Generation for Dynamic Street Scenarios in a
- Frenet
- Frame `__
-
-- `Optimal trajectory generation for dynamic street scenarios in a
- Frenet Frame `__
-
diff --git a/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst b/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
deleted file mode 100644
index 81e565888e..0000000000
--- a/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
+++ /dev/null
@@ -1,17 +0,0 @@
-Reeds Shepp planning
---------------------
-
-A sample code with Reeds Shepp path planning.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true
-
-Ref:
-
-- `15.3.2 Reeds-Shepp
- Curves `__
-
-- `optimal paths for a car that goes both forwards and
- backwards `__
-
-- `ghliu/pyReedsShepp: Implementation of Reeds Shepp
- curve. `__
diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst b/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
deleted file mode 100644
index 68ea9c88b2..0000000000
--- a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
+++ /dev/null
@@ -1,13 +0,0 @@
-.. _linearquadratic-regulator-(lqr)-speed-and-steering-control:
-
-Linear–quadratic regulator (LQR) speed and steering control
------------------------------------------------------------
-
-Path tracking simulation with LQR speed and steering control.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif
-
-References:
-~~~~~~~~~~~
-
-- `Towards fully autonomous driving: Systems and algorithms `__
diff --git a/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst b/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst
deleted file mode 100644
index bf6d6b5854..0000000000
--- a/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst
+++ /dev/null
@@ -1,14 +0,0 @@
-.. _linearquadratic-regulator-(lqr)-steering-control:
-
-Linear–quadratic regulator (LQR) steering control
--------------------------------------------------
-
-Path tracking simulation with LQR steering control and PID speed
-control.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif
-
-References:
-~~~~~~~~~~~
-- `ApolloAuto/apollo: An open autonomous driving platform `_
-
diff --git a/docs/modules/path_tracking/path_tracking_main.rst b/docs/modules/path_tracking/path_tracking_main.rst
deleted file mode 100644
index 504ba08795..0000000000
--- a/docs/modules/path_tracking/path_tracking_main.rst
+++ /dev/null
@@ -1,16 +0,0 @@
-.. _path_tracking:
-
-Path Tracking
-=============
-
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- pure_pursuit_tracking/pure_pursuit_tracking
- stanley_control/stanley_control
- rear_wheel_feedback_control/rear_wheel_feedback_control
- lqr_steering_control/lqr_steering_control
- lqr_speed_and_steering_control/lqr_speed_and_steering_control
- model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control
- cgmres_nmpc/cgmres_nmpc
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif
deleted file mode 100644
index 2fabeaafc9..0000000000
Binary files a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif and /dev/null differ
diff --git a/docs/modules/slam/slam_main.rst b/docs/modules/slam/slam_main.rst
deleted file mode 100644
index 86befa6e35..0000000000
--- a/docs/modules/slam/slam_main.rst
+++ /dev/null
@@ -1,16 +0,0 @@
-.. _slam:
-
-SLAM
-====
-
-Simultaneous Localization and Mapping(SLAM) examples
-
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- iterative_closest_point_matching/iterative_closest_point_matching
- ekf_slam/ekf_slam
- FastSLAM1/FastSLAM1
- FastSLAM2/FastSLAM2
- graph_slam/graph_slam
diff --git a/requirements/environment.yml b/requirements/environment.yml
index 109fc57789..023a3d75bf 100644
--- a/requirements/environment.yml
+++ b/requirements/environment.yml
@@ -2,10 +2,9 @@ name: python_robotics
channels:
- conda-forge
dependencies:
- - python=3.10
+ - python=3.13
- pip
- scipy
- numpy
- - pandas
- cvxpy
- matplotlib
diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 6aee1a3d9e..050bd1e57c 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,9 +1,9 @@
-numpy == 1.23.2
-scipy == 1.9.1
-pandas == 1.4.3
-matplotlib == 3.5.3
-cvxpy == 1.2.1
-pytest == 7.1.2 # For unit test
-pytest-xdist == 2.5.0 # For unit test
-mypy == 0.971 # For unit test
-flake8 == 5.0.4 # For unit test
+numpy == 2.2.4
+scipy == 1.15.2
+matplotlib == 3.10.1
+cvxpy == 1.7.3
+ecos == 2.0.14
+pytest == 8.4.2 # For unit test
+pytest-xdist == 3.8.0 # For unit test
+mypy == 1.18.2 # For unit test
+ruff == 0.13.2 # For unit test
diff --git a/ruff.toml b/ruff.toml
new file mode 100644
index 0000000000..d76b715a06
--- /dev/null
+++ b/ruff.toml
@@ -0,0 +1,18 @@
+line-length = 88
+
+select = ["F", "E", "W", "UP"]
+ignore = ["E501", "E741", "E402"]
+exclude = [
+]
+
+# Assume Python 3.13
+target-version = "py313"
+
+[per-file-ignores]
+
+[mccabe]
+# Unlike Flake8, default to a complexity level of 10.
+max-complexity = 10
+
+[pydocstyle]
+convention = "numpy"
diff --git a/runtests.sh b/runtests.sh
index a2a683e571..c4460c8aa7 100755
--- a/runtests.sh
+++ b/runtests.sh
@@ -1,8 +1,9 @@
#!/usr/bin/env bash
+cd "$(dirname "$0")" || exit 1
echo "Run test suites! "
# === pytest based test runner ===
# -Werror: warning as error
# --durations=0: show ranking of test durations
# -l (--showlocals); show local variables when test failed
-pytest -n auto tests -l -Werror --durations=0
+pytest tests -l -Werror --durations=0
diff --git a/tests/conftest.py b/tests/conftest.py
index 3485fe8150..b707b22d00 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -10,4 +10,4 @@
def run_this_test(file):
- pytest.main([os.path.abspath(file)])
+ pytest.main(args=["-W", "error", "-Werror", "--pythonwarnings=error", os.path.abspath(file)])
diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py
index b535b99baf..be12195eac 100644
--- a/tests/test_LQR_planner.py
+++ b/tests/test_LQR_planner.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from PathPlanning.LQRPlanner import LQRplanner as m
+from PathPlanning.LQRPlanner import lqr_planner as m
def test_1():
diff --git a/tests/test_batch_informed_rrt_star.py b/tests/test_batch_informed_rrt_star.py
index 3ad78bdb23..cf0a9827a8 100644
--- a/tests/test_batch_informed_rrt_star.py
+++ b/tests/test_batch_informed_rrt_star.py
@@ -1,7 +1,7 @@
import random
import conftest
-from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m
+from PathPlanning.BatchInformedRRTStar import batch_informed_rrt_star as m
def test_1():
diff --git a/tests/test_behavior_tree.py b/tests/test_behavior_tree.py
new file mode 100644
index 0000000000..0898690448
--- /dev/null
+++ b/tests/test_behavior_tree.py
@@ -0,0 +1,207 @@
+import pytest
+import conftest
+
+from MissionPlanning.BehaviorTree.behavior_tree import (
+ BehaviorTreeFactory,
+ Status,
+ ActionNode,
+)
+
+
+def test_sequence_node1():
+ xml_string = """
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+ bt.tick()
+ bt.tick()
+ assert bt.root.status == Status.FAILURE
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_sequence_node2():
+ xml_string = """
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick_while_running()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_selector_node1():
+ xml_string = """
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.FAILURE
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_selector_node2():
+ xml_string = """
+
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick_while_running()
+ assert bt.root.status == Status.FAILURE
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+
+
+def test_while_do_else_node():
+ xml_string = """
+
+
+
+
+
+ """
+
+ class CountNode(ActionNode):
+ def __init__(self, name, count_threshold):
+ super().__init__(name)
+ self.count = 0
+ self.count_threshold = count_threshold
+
+ def tick(self):
+ self.count += 1
+ if self.count >= self.count_threshold:
+ return Status.FAILURE
+ else:
+ return Status.SUCCESS
+
+ bt_factory = BehaviorTreeFactory()
+ bt_factory.register_node_builder(
+ "Count",
+ lambda node: CountNode(
+ node.attrib.get("name", CountNode.__name__),
+ int(node.attrib["count_threshold"]),
+ ),
+ )
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is Status.SUCCESS
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is Status.SUCCESS
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_node_children():
+ # ControlNode Must have children
+ xml_string = """
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # DecoratorNode Must have child
+ xml_string = """
+
+
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # DecoratorNode Must have only one child
+ xml_string = """
+
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # ActionNode Must have no children
+ xml_string = """
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # WhileDoElse Must have exactly 2 or 3 children
+ xml_string = """
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+
+ xml_string = """
+
+
+
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_bspline_path.py b/tests/test_bspline_path.py
new file mode 100644
index 0000000000..4918c9810f
--- /dev/null
+++ b/tests/test_bspline_path.py
@@ -0,0 +1,74 @@
+import conftest
+import numpy as np
+import pytest
+from PathPlanning.BSplinePath import bspline_path
+
+
+def test_list_input():
+ way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]
+ way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]
+ n_course_point = 50 # sampling number
+
+ rax, ray, heading, curvature = bspline_path.approximate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, s=0.5)
+
+ assert len(rax) == len(ray) == len(heading) == len(curvature)
+
+ rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path(
+ way_point_x, way_point_y, n_course_point)
+
+ assert len(rix) == len(riy) == len(heading) == len(curvature)
+
+
+def test_array_input():
+ way_point_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0])
+ way_point_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0])
+ n_course_point = 50 # sampling number
+
+ rax, ray, heading, curvature = bspline_path.approximate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, s=0.5)
+
+ assert len(rax) == len(ray) == len(heading) == len(curvature)
+
+ rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path(
+ way_point_x, way_point_y, n_course_point)
+
+ assert len(rix) == len(riy) == len(heading) == len(curvature)
+
+
+def test_degree_change():
+ way_point_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0])
+ way_point_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0])
+ n_course_point = 50 # sampling number
+
+ rax, ray, heading, curvature = bspline_path.approximate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, s=0.5, degree=4)
+
+ assert len(rax) == len(ray) == len(heading) == len(curvature)
+
+ rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, degree=4)
+
+ assert len(rix) == len(riy) == len(heading) == len(curvature)
+
+ rax, ray, heading, curvature = bspline_path.approximate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, s=0.5, degree=2)
+
+ assert len(rax) == len(ray) == len(heading) == len(curvature)
+
+ rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, degree=2)
+
+ assert len(rix) == len(riy) == len(heading) == len(curvature)
+
+ with pytest.raises(ValueError):
+ bspline_path.approximate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, s=0.5, degree=1)
+
+ with pytest.raises(ValueError):
+ bspline_path.interpolate_b_spline_path(
+ way_point_x, way_point_y, n_course_point, degree=1)
+
+
+if __name__ == '__main__':
+ conftest.run_this_test(__file__)
diff --git a/tests/test_catmull_rom_spline.py b/tests/test_catmull_rom_spline.py
new file mode 100644
index 0000000000..41a73588c3
--- /dev/null
+++ b/tests/test_catmull_rom_spline.py
@@ -0,0 +1,16 @@
+import conftest
+from PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path import catmull_rom_spline
+
+def test_catmull_rom_spline():
+ way_points = [[0, 0], [1, 2], [2, 0], [3, 3]]
+ num_points = 100
+
+ spline_x, spline_y = catmull_rom_spline(way_points, num_points)
+
+ assert spline_x.size > 0, "Spline X coordinates should not be empty"
+ assert spline_y.size > 0, "Spline Y coordinates should not be empty"
+
+ assert spline_x.shape == spline_y.shape, "Spline X and Y coordinates should have the same shape"
+
+if __name__ == '__main__':
+ conftest.run_this_test(__file__)
diff --git a/tests/test_diff_codestyle.py b/tests/test_codestyle.py
similarity index 78%
rename from tests/test_diff_codestyle.py
rename to tests/test_codestyle.py
index 89c228e285..55e558c184 100644
--- a/tests/test_diff_codestyle.py
+++ b/tests/test_codestyle.py
@@ -1,8 +1,8 @@
"""
-Diff based code style checker with flake8
+Diff code style checker with ruff
This code come from:
-https://github.com/scipy/scipy/blob/main/tools/lint_diff.py
+https://github.com/scipy/scipy/blob/main/tools/lint.py
Scipy's licence: https://github.com/scipy/scipy/blob/main/LICENSE.txt
Copyright (c) 2001-2002 Enthought, Inc. 2003-2022, SciPy Developers.
@@ -37,9 +37,30 @@
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import conftest
+import os
import subprocess
+CONFIG = os.path.join(
+ os.path.abspath(os.path.dirname(os.path.dirname(__file__))),
+ 'ruff.toml',
+)
+
+ROOT_DIR = os.path.abspath(os.path.dirname(os.path.dirname(__file__)))
+
+
+def run_ruff(files, fix):
+ if not files:
+ return 0, ""
+ args = ['--fix'] if fix else []
+ res = subprocess.run(
+ ['ruff', 'check', f'--config={CONFIG}'] + args + files,
+ stdout=subprocess.PIPE,
+ encoding='utf-8'
+ )
+ return res.returncode, res.stdout
+
+
def rev_list(branch, num_commits):
"""List commits in reverse chronological order.
Only the first `num_commits` are shown.
@@ -89,23 +110,23 @@ def find_diff(sha):
return res.stdout
-def run_flake8(diff):
- """Run flake8 on the given diff."""
+def diff_files(sha):
+ """Find the diff since the given SHA."""
res = subprocess.run(
- ['flake8', '--diff', '--ignore',
- 'E402' # top level import for sys.path.append
- ],
- input=diff,
+ ['git', 'diff', '--name-only', '--diff-filter=ACMR', '-z', sha, '--',
+ '*.py', '*.pyx', '*.pxd', '*.pxi'],
stdout=subprocess.PIPE,
- encoding='utf-8',
+ encoding='utf-8'
)
- return res.returncode, res.stdout
+ res.check_returncode()
+ return [os.path.join(ROOT_DIR, f) for f in res.stdout.split('\0') if f]
def test():
branch_commit = find_branch_point("origin/master")
- diff = find_diff(branch_commit)
- rc, errors = run_flake8(diff)
+ files = diff_files(branch_commit)
+ print(files)
+ rc, errors = run_ruff(files, fix=True)
if errors:
print(errors)
else:
diff --git a/tests/test_distance_map.py b/tests/test_distance_map.py
new file mode 100644
index 0000000000..df6e394e2c
--- /dev/null
+++ b/tests/test_distance_map.py
@@ -0,0 +1,118 @@
+import conftest # noqa
+import numpy as np
+from Mapping.DistanceMap import distance_map as m
+
+
+def test_compute_sdf():
+ """Test the computation of Signed Distance Field (SDF)"""
+ # Create a simple obstacle map for testing
+ obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]])
+
+ sdf = m.compute_sdf(obstacles)
+
+ # Verify basic properties of SDF
+ assert sdf.shape == obstacles.shape, "SDF should have the same shape as input map"
+ assert np.all(np.isfinite(sdf)), "SDF should not contain infinite values"
+
+ # Verify SDF value is negative at obstacle position
+ assert sdf[1, 1] < 0, "SDF value should be negative at obstacle position"
+
+ # Verify SDF value is positive in free space
+ assert sdf[0, 0] > 0, "SDF value should be positive in free space"
+
+
+def test_compute_udf():
+ """Test the computation of Unsigned Distance Field (UDF)"""
+ # Create obstacle map for testing
+ obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]])
+
+ udf = m.compute_udf(obstacles)
+
+ # Verify basic properties of UDF
+ assert udf.shape == obstacles.shape, "UDF should have the same shape as input map"
+ assert np.all(np.isfinite(udf)), "UDF should not contain infinite values"
+ assert np.all(udf >= 0), "All UDF values should be non-negative"
+
+ # Verify UDF value is 0 at obstacle position
+ assert np.abs(udf[1, 1]) < 1e-10, "UDF value should be 0 at obstacle position"
+
+ # Verify UDF value is 1 for adjacent cells
+ assert np.abs(udf[0, 1] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+ assert np.abs(udf[1, 0] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+ assert np.abs(udf[1, 2] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+ assert np.abs(udf[2, 1] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+
+
+def test_dt():
+ """Test the computation of 1D distance transform"""
+ # Create test data
+ d = np.array([m.INF, 0, m.INF])
+ m.dt(d)
+
+ # Verify distance transform results
+ assert np.all(np.isfinite(d)), (
+ "Distance transform result should not contain infinite values"
+ )
+ assert d[1] == 0, "Distance at obstacle position should be 0"
+ assert d[0] == 1, "Distance at adjacent position should be 1"
+ assert d[2] == 1, "Distance at adjacent position should be 1"
+
+
+def test_compute_sdf_empty():
+ """Test SDF computation with empty map"""
+ # Test with empty map (no obstacles)
+ empty_map = np.zeros((5, 5))
+ sdf = m.compute_sdf(empty_map)
+
+ assert np.all(sdf > 0), "All SDF values should be positive for empty map"
+ assert sdf.shape == empty_map.shape, "Output shape should match input shape"
+
+
+def test_compute_sdf_full():
+ """Test SDF computation with fully occupied map"""
+ # Test with fully occupied map
+ full_map = np.ones((5, 5))
+ sdf = m.compute_sdf(full_map)
+
+ assert np.all(sdf < 0), "All SDF values should be negative for fully occupied map"
+ assert sdf.shape == full_map.shape, "Output shape should match input shape"
+
+
+def test_compute_udf_invalid_input():
+ """Test UDF computation with invalid input values"""
+ # Test with invalid values (not 0 or 1)
+ invalid_map = np.array([[0, 2, 0], [0, -1, 0], [0, 0.5, 0]])
+
+ try:
+ m.compute_udf(invalid_map)
+ assert False, "Should raise ValueError for invalid input values"
+ except ValueError:
+ pass
+
+
+def test_compute_udf_empty():
+ """Test UDF computation with empty map"""
+ # Test with empty map
+ empty_map = np.zeros((5, 5))
+ udf = m.compute_udf(empty_map)
+
+ assert np.all(udf > 0), "All UDF values should be positive for empty map"
+ assert np.all(np.isfinite(udf)), "UDF should not contain infinite values"
+
+
+def test_main():
+ """Test the execution of main function"""
+ m.ENABLE_PLOT = False
+ m.main()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_elastic_bands.py b/tests/test_elastic_bands.py
new file mode 100644
index 0000000000..ad4e13af1a
--- /dev/null
+++ b/tests/test_elastic_bands.py
@@ -0,0 +1,23 @@
+import conftest
+import numpy as np
+from PathPlanning.ElasticBands.elastic_bands import ElasticBands
+
+
+def test_1():
+ path = np.load("PathPlanning/ElasticBands/path.npy")
+ obstacles_points = np.load("PathPlanning/ElasticBands/obstacles.npy")
+ obstacles = np.zeros((500, 500))
+ for x, y in obstacles_points:
+ size = 30 # Side length of the square
+ half_size = size // 2
+ x_start = max(0, x - half_size)
+ x_end = min(obstacles.shape[0], x + half_size)
+ y_start = max(0, y - half_size)
+ y_end = min(obstacles.shape[1], y + half_size)
+ obstacles[x_start:x_end, y_start:y_end] = 1
+ elastic_bands = ElasticBands(path, obstacles)
+ elastic_bands.update_bubbles()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_frenet_optimal_trajectory.py b/tests/test_frenet_optimal_trajectory.py
index 72bb7a8341..b8761ff4a6 100644
--- a/tests/test_frenet_optimal_trajectory.py
+++ b/tests/test_frenet_optimal_trajectory.py
@@ -1,12 +1,48 @@
import conftest
from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m
+from PathPlanning.FrenetOptimalTrajectory.frenet_optimal_trajectory import (
+ LateralMovement,
+ LongitudinalMovement,
+)
-def test1():
+def default_scenario_test():
m.show_animation = False
m.SIM_LOOP = 5
m.main()
-if __name__ == '__main__':
+def high_speed_and_merging_and_stopping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.HIGH_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.MERGING_AND_STOPPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+def high_speed_and_velocity_keeping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.HIGH_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.VELOCITY_KEEPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+def low_speed_and_velocity_keeping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.LOW_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.VELOCITY_KEEPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+def low_speed_and_merging_and_stopping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.LOW_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.MERGING_AND_STOPPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+if __name__ == "__main__":
conftest.run_this_test(__file__)
diff --git a/tests/test_grid_map_lib.py b/tests/test_grid_map_lib.py
index 8ae142b003..670b357ad3 100644
--- a/tests/test_grid_map_lib.py
+++ b/tests/test_grid_map_lib.py
@@ -1,4 +1,5 @@
from Mapping.grid_map_lib.grid_map_lib import GridMap
+import conftest
import numpy as np
@@ -24,5 +25,16 @@ def test_polygon_set():
1.0, inside=False)
+def test_xy_and_grid_index_conversion():
+ grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
+
+ for x_ind in range(grid_map.width):
+ for y_ind in range(grid_map.height):
+ grid_ind = grid_map.calc_grid_index_from_xy_index(x_ind, y_ind)
+ x_ind_2, y_ind_2 = grid_map.calc_xy_index_from_grid_index(grid_ind)
+ assert x_ind == x_ind_2
+ assert y_ind == y_ind_2
+
+
if __name__ == '__main__':
conftest.run_this_test(__file__)
diff --git a/tests/test_inverted_pendulum_lqr_control.py b/tests/test_inverted_pendulum_lqr_control.py
index cbbabb93b1..62afda71c3 100644
--- a/tests/test_inverted_pendulum_lqr_control.py
+++ b/tests/test_inverted_pendulum_lqr_control.py
@@ -1,5 +1,5 @@
import conftest
-from Control.inverted_pendulum import inverted_pendulum_lqr_control as m
+from InvertedPendulum import inverted_pendulum_lqr_control as m
def test_1():
diff --git a/tests/test_inverted_pendulum_mpc_control.py b/tests/test_inverted_pendulum_mpc_control.py
index 800aefd7d5..94859c2e0a 100644
--- a/tests/test_inverted_pendulum_mpc_control.py
+++ b/tests/test_inverted_pendulum_mpc_control.py
@@ -1,6 +1,6 @@
import conftest
-from Control.inverted_pendulum import inverted_pendulum_mpc_control as m
+from InvertedPendulum import inverted_pendulum_mpc_control as m
def test1():
diff --git a/tests/test_iterative_closest_point.py b/tests/test_iterative_closest_point.py
index 3e726b5649..cdfa89cc55 100644
--- a/tests/test_iterative_closest_point.py
+++ b/tests/test_iterative_closest_point.py
@@ -1,5 +1,5 @@
import conftest
-from SLAM.iterative_closest_point import iterative_closest_point as m
+from SLAM.ICPMatching import icp_matching as m
def test_1():
diff --git a/tests/test_move_to_pose.py b/tests/test_move_to_pose.py
index 8bc11a8d24..e06d801555 100644
--- a/tests/test_move_to_pose.py
+++ b/tests/test_move_to_pose.py
@@ -1,13 +1,81 @@
+import itertools
+import numpy as np
import conftest # Add root path to sys.path
-from Control.move_to_pose import move_to_pose as m
+from PathTracking.move_to_pose import move_to_pose as m
-def test_1():
+def test_random():
m.show_animation = False
m.main()
-def test_2():
+def test_stability():
+ """
+ This unit test tests the move_to_pose.py program for stability
+ """
+ m.show_animation = False
+ x_start = 5
+ y_start = 5
+ theta_start = 0
+ x_goal = 1
+ y_goal = 4
+ theta_goal = 0
+ _, _, v_traj, w_traj = m.move_to_pose(
+ x_start, y_start, theta_start, x_goal, y_goal, theta_goal
+ )
+
+ def v_is_change(current, previous):
+ return abs(current - previous) > m.MAX_LINEAR_SPEED
+
+ def w_is_change(current, previous):
+ return abs(current - previous) > m.MAX_ANGULAR_SPEED
+
+ # Check if the speed is changing too much
+ window_size = 10
+ count_threshold = 4
+ v_change = [v_is_change(v_traj[i], v_traj[i - 1]) for i in range(1, len(v_traj))]
+ w_change = [w_is_change(w_traj[i], w_traj[i - 1]) for i in range(1, len(w_traj))]
+ for i in range(len(v_change) - window_size + 1):
+ v_window = v_change[i : i + window_size]
+ w_window = w_change[i : i + window_size]
+
+ v_unstable = sum(v_window) > count_threshold
+ w_unstable = sum(w_window) > count_threshold
+
+ assert not v_unstable, (
+ f"v_unstable in window [{i}, {i + window_size}], unstable count: {sum(v_window)}"
+ )
+ assert not w_unstable, (
+ f"w_unstable in window [{i}, {i + window_size}], unstable count: {sum(w_window)}"
+ )
+
+
+def test_reach_goal():
+ """
+ This unit test tests the move_to_pose.py program for reaching the goal
+ """
+ m.show_animation = False
+ x_start = 5
+ y_start = 5
+ theta_start_list = [0, np.pi / 2, np.pi, 3 * np.pi / 2]
+ x_goal_list = [0, 5, 10]
+ y_goal_list = [0, 5, 10]
+ theta_goal = 0
+ for theta_start, x_goal, y_goal in itertools.product(
+ theta_start_list, x_goal_list, y_goal_list
+ ):
+ x_traj, y_traj, _, _ = m.move_to_pose(
+ x_start, y_start, theta_start, x_goal, y_goal, theta_goal
+ )
+ x_diff = x_goal - x_traj[-1]
+ y_diff = y_goal - y_traj[-1]
+ rho = np.hypot(x_diff, y_diff)
+ assert rho < 0.001, (
+ f"start:[{x_start}, {y_start}, {theta_start}], goal:[{x_goal}, {y_goal}, {theta_goal}], rho: {rho} is too large"
+ )
+
+
+def test_max_speed():
"""
This unit test tests the move_to_pose.py program for a MAX_LINEAR_SPEED and
MAX_ANGULAR_SPEED
@@ -18,5 +86,5 @@ def test_2():
m.main()
-if __name__ == '__main__':
+if __name__ == "__main__":
conftest.run_this_test(__file__)
diff --git a/tests/test_move_to_pose_robot.py b/tests/test_move_to_pose_robot.py
index a93b44d198..7a82f98556 100644
--- a/tests/test_move_to_pose_robot.py
+++ b/tests/test_move_to_pose_robot.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from Control.move_to_pose import move_to_pose as m
+from PathTracking.move_to_pose import move_to_pose as m
def test_1():
diff --git a/tests/test_mypy_type_check.py b/tests/test_mypy_type_check.py
index 07afb40afd..6b933c1011 100644
--- a/tests/test_mypy_type_check.py
+++ b/tests/test_mypy_type_check.py
@@ -7,12 +7,12 @@
"AerialNavigation",
"ArmNavigation",
"Bipedal",
- "Control",
"Localization",
"Mapping",
"PathPlanning",
"PathTracking",
"SLAM",
+ "InvertedPendulum"
]
diff --git a/tests/test_normal_vector_estimation.py b/tests/test_normal_vector_estimation.py
new file mode 100644
index 0000000000..7612f22aa7
--- /dev/null
+++ b/tests/test_normal_vector_estimation.py
@@ -0,0 +1,19 @@
+import conftest # Add root path to sys.path
+from Mapping.normal_vector_estimation import normal_vector_estimation as m
+import random
+
+random.seed(12345)
+
+
+def test_1():
+ m.show_animation = False
+ m.main1()
+
+
+def test_2():
+ m.show_animation = False
+ m.main2()
+
+
+if __name__ == '__main__':
+ conftest.run_this_test(__file__)
diff --git a/tests/test_point_cloud_sampling.py b/tests/test_point_cloud_sampling.py
new file mode 100644
index 0000000000..8f6447c69c
--- /dev/null
+++ b/tests/test_point_cloud_sampling.py
@@ -0,0 +1,15 @@
+import conftest # Add root path to sys.path
+from Mapping.point_cloud_sampling import point_cloud_sampling as m
+
+
+def test_1(capsys):
+ m.do_plot = False
+ m.main()
+ captured = capsys.readouterr()
+ assert "voxel_sampling_points.shape=(27, 3)" in captured.out
+ assert "farthest_sampling_points.shape=(20, 3)" in captured.out
+ assert "poisson_disk_points.shape=(20, 3)" in captured.out
+
+
+if __name__ == '__main__':
+ conftest.run_this_test(__file__)
diff --git a/tests/test_priority_based_planner.py b/tests/test_priority_based_planner.py
new file mode 100644
index 0000000000..e2ff602d88
--- /dev/null
+++ b/tests/test_priority_based_planner.py
@@ -0,0 +1,39 @@
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ NodePath,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal
+from PathPlanning.TimeBasedPathPlanning import PriorityBasedPlanner as m
+from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
+import numpy as np
+import conftest
+
+
+def test_1():
+ grid_side_length = 21
+
+ start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+ obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=250,
+ obstacle_avoid_points=obstacle_avoid_points,
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ m.show_animation = False
+
+ start_and_goals: list[StartAndGoal]
+ paths: list[NodePath]
+ start_and_goals, paths = m.PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, False)
+
+ # All paths should start at the specified position and reach the goal
+ for i, start_and_goal in enumerate(start_and_goals):
+ assert paths[i].path[0].position == start_and_goal.start
+ assert paths[i].path[-1].position == start_and_goal.goal
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_pure_pursuit.py b/tests/test_pure_pursuit.py
index 0e0b83bf6c..89c1829514 100644
--- a/tests/test_pure_pursuit.py
+++ b/tests/test_pure_pursuit.py
@@ -6,6 +6,10 @@ def test1():
m.show_animation = False
m.main()
+def test_backward():
+ m.show_animation = False
+ m.is_reverse_mode = True
+ m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)
diff --git a/tests/test_raycasting_grid_map.py b/tests/test_ray_casting_grid_map.py
similarity index 71%
rename from tests/test_raycasting_grid_map.py
rename to tests/test_ray_casting_grid_map.py
index f08ae9277e..2d192c9310 100644
--- a/tests/test_raycasting_grid_map.py
+++ b/tests/test_ray_casting_grid_map.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from Mapping.raycasting_grid_map import raycasting_grid_map as m
+from Mapping.ray_casting_grid_map import ray_casting_grid_map as m
def test1():
diff --git a/tests/test_rear_wheel_feedback.py b/tests/test_rear_wheel_feedback.py
index 895eb188b3..eccde52358 100644
--- a/tests/test_rear_wheel_feedback.py
+++ b/tests/test_rear_wheel_feedback.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from PathTracking.rear_wheel_feedback import rear_wheel_feedback as m
+from PathTracking.rear_wheel_feedback_control import rear_wheel_feedback_control as m
def test1():
diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py
index cb6b586b33..11157aa57a 100644
--- a/tests/test_rrt_star_reeds_shepp.py
+++ b/tests/test_rrt_star_reeds_shepp.py
@@ -6,6 +6,41 @@ def test1():
m.show_animation = False
m.main(max_iter=5)
+obstacleList = [
+ (5, 5, 1),
+ (4, 6, 1),
+ (4, 8, 1),
+ (4, 10, 1),
+ (6, 5, 1),
+ (7, 5, 1),
+ (8, 6, 1),
+ (8, 8, 1),
+ (8, 10, 1)
+] # [x,y,size(radius)]
+
+start = [0.0, 0.0, m.np.deg2rad(0.0)]
+goal = [6.0, 7.0, m.np.deg2rad(90.0)]
+
+def test2():
+ step_size = 0.2
+ rrt_star_reeds_shepp = m.RRTStarReedsShepp(start, goal,
+ obstacleList, [-2.0, 15.0],
+ max_iter=100, step_size=step_size)
+ rrt_star_reeds_shepp.set_random_seed(seed=8)
+ path = rrt_star_reeds_shepp.planning(animation=False)
+ for i in range(len(path)-1):
+ # + 0.00000000000001 for acceptable errors arising from the planning process
+ assert m.math.dist(path[i][0:2], path[i+1][0:2]) < step_size + 0.00000000000001
+
+def test_too_big_step_size():
+ step_size = 20
+ rrt_star_reeds_shepp = m.RRTStarReedsShepp(start, goal,
+ obstacleList, [-2.0, 15.0],
+ max_iter=100, step_size=step_size)
+ rrt_star_reeds_shepp.set_random_seed(seed=8)
+ path = rrt_star_reeds_shepp.planning(animation=False)
+ assert path is None
+
if __name__ == '__main__':
conftest.run_this_test(__file__)
diff --git a/tests/test_rrt_with_pathsmoothing_radius.py b/tests/test_rrt_with_pathsmoothing_radius.py
new file mode 100644
index 0000000000..a1159255b5
--- /dev/null
+++ b/tests/test_rrt_with_pathsmoothing_radius.py
@@ -0,0 +1,48 @@
+import conftest
+import math
+
+from PathPlanning.RRT import rrt_with_pathsmoothing as rrt_module
+
+def test_smoothed_path_safety():
+ # Define test environment
+ obstacle_list = [
+ (5, 5, 1.0),
+ (3, 6, 2.0),
+ (3, 8, 2.0),
+ (3, 10, 2.0),
+ (7, 5, 2.0),
+ (9, 5, 2.0)
+ ]
+ robot_radius = 0.5
+
+ # Disable animation for testing
+ rrt_module.show_animation = False
+
+ # Create RRT planner
+ rrt = rrt_module.RRT(
+ start=[0, 0],
+ goal=[6, 10],
+ rand_area=[-2, 15],
+ obstacle_list=obstacle_list,
+ robot_radius=robot_radius
+ )
+
+ # Run RRT
+ path = rrt.planning(animation=False)
+
+ # Smooth the path
+ smoothed = rrt_module.path_smoothing(path, max_iter=1000,
+ obstacle_list=obstacle_list,
+ robot_radius=robot_radius)
+
+ # Check if all points on the smoothed path are safely distant from obstacles
+ for x, y in smoothed:
+ for ox, oy, obs_radius in obstacle_list:
+ d = math.hypot(x - ox, y - oy)
+ min_safe_dist = obs_radius + robot_radius
+ assert d > min_safe_dist, \
+ f"Point ({x:.2f}, {y:.2f}) too close to obstacle at ({ox}, {oy})"
+
+
+if __name__ == '__main__':
+ conftest.run_this_test(__file__)
diff --git a/tests/test_safe_interval_path_planner.py b/tests/test_safe_interval_path_planner.py
new file mode 100644
index 0000000000..bbcd4e427c
--- /dev/null
+++ b/tests/test_safe_interval_path_planner.py
@@ -0,0 +1,31 @@
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning import SafeInterval as m
+import numpy as np
+import conftest
+
+
+def test_1():
+ start = Position(1, 11)
+ goal = Position(19, 19)
+ grid_side_length = 21
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ m.show_animation = False
+ path = m.SafeIntervalPathPlanner.plan(grid, start, goal)
+
+ # path should have 31 entries
+ assert len(path.path) == 31
+
+ # path should end at the goal
+ assert path.path[-1].position == goal
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py
new file mode 100644
index 0000000000..1194c61d2e
--- /dev/null
+++ b/tests/test_space_time_astar.py
@@ -0,0 +1,32 @@
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning import SpaceTimeAStar as m
+import numpy as np
+import conftest
+
+
+def test_1():
+ start = Position(1, 11)
+ goal = Position(19, 19)
+ grid_side_length = 21
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ m.show_animation = False
+ path = m.SpaceTimeAStar.plan(grid, start, goal)
+
+ # path should have 28 entries
+ assert len(path.path) == 31
+
+ # path should end at the goal
+ assert path.path[-1].position == goal
+
+ assert path.expanded_node_count < 1000
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_stanley_controller.py b/tests/test_stanley_controller.py
index a1d8073764..bd590cb51a 100644
--- a/tests/test_stanley_controller.py
+++ b/tests/test_stanley_controller.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from PathTracking.stanley_controller import stanley_controller as m
+from PathTracking.stanley_control import stanley_control as m
def test1():
diff --git a/tests/test_state_machine.py b/tests/test_state_machine.py
new file mode 100644
index 0000000000..e36a8120fd
--- /dev/null
+++ b/tests/test_state_machine.py
@@ -0,0 +1,51 @@
+import conftest
+
+from MissionPlanning.StateMachine.state_machine import StateMachine
+
+
+def test_transition():
+ sm = StateMachine("state_machine")
+ sm.add_transition(src_state="idle", event="start", dst_state="running")
+ sm.set_current_state("idle")
+ sm.process("start")
+ assert sm.get_current_state().name == "running"
+
+
+def test_guard():
+ class Model:
+ def can_start(self):
+ return False
+
+ sm = StateMachine("state_machine", Model())
+ sm.add_transition(
+ src_state="idle", event="start", dst_state="running", guard="can_start"
+ )
+ sm.set_current_state("idle")
+ sm.process("start")
+ assert sm.get_current_state().name == "idle"
+
+
+def test_action():
+ class Model:
+ def on_start(self):
+ self.start_called = True
+
+ model = Model()
+ sm = StateMachine("state_machine", model)
+ sm.add_transition(
+ src_state="idle", event="start", dst_state="running", action="on_start"
+ )
+ sm.set_current_state("idle")
+ sm.process("start")
+ assert model.start_called
+
+
+def test_plantuml():
+ sm = StateMachine("state_machine")
+ sm.add_transition(src_state="idle", event="start", dst_state="running")
+ sm.set_current_state("idle")
+ assert sm.generate_plantuml()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/users_comments.md b/users_comments.md
index 66e8b4022e..c814e74c4b 100644
--- a/users_comments.md
+++ b/users_comments.md
@@ -6,24 +6,18 @@ This is an electric wheelchair control demo by [Katsushun89](https://github.com/
[WHILL Model CR](https://whill.jp/model-cr) is the control target, [M5Stack](https://m5stack.com/) is used for the controller, and [toio](https://toio.io/) is used for the control input device.
-[Move to a Pose Control — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/control/move_to_a_pose_control/move_to_a_pose_control.html) is used for its control algorithm ([code link](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/move_to_pose/move_to_pose.py)).
+[Move to a Pose Control — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/move_to_a_pose/move_to_a_pose.html) is used for its control algorithm ([code link](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/move_to_pose/move_to_pose.py)).
-Ref:
+Reference:
- [toioと同じように動くWHILL Model CR (in Japanese)](https://qiita.com/KatsuShun89/items/68fde7544ecfe36096d2)
# Educational users
-This is a list of users who are using PythonRobotics for education.
-
-If you found other users, please make an issue to let me know.
-
-- [CSCI/ARTI 4530/6530: Introduction to Robotics (Fall 2018), University of Georgia ](http://cobweb.cs.uga.edu/~ramviyas/csci_x530.html)
-
-- [CIT Modules & Programmes \- COMP9073 \- Automation with Python](https://courses.cit.ie/index.cfm/page/module/moduleId/14416)
+If you found users who are using PythonRobotics for education, please make an issue to let me know.
# Stargazers location map
@@ -107,7 +101,7 @@ Thank you!
---
-Dear AtsushiSakai, Thanks a lot for the wonderful collection of projects.It was really useful. I really appreciate your time in maintaing and building this repository.It will be really great if I get a chance to meet you in person sometime.I got really inspired looking at your work. All the very best for all your future endeavors! Thanks once again,
+Dear AtsushiSakai, Thanks a lot for the wonderful collection of projects.It was really useful. I really appreciate your time in maintaining and building this repository.It will be really great if I get a chance to meet you in person sometime.I got really inspired looking at your work. All the very best for all your future endeavors! Thanks once again,
---Ezhil Bharathi
@@ -169,7 +163,7 @@ so kind of you can you make videos of it.
---
-Dear AtshshiSakai, You are very wise and smart that you created this library for students wanting to learn Probabilistic Robotics and actually perform robot control. Hats off to you and your work. I am also reading your book titled : Python Robotics
+Dear AtsushiSakai, You are very wise and smart that you created this library for students wanting to learn Probabilistic Robotics and actually perform robot control. Hats off to you and your work. I am also reading your book titled : Python Robotics
--Himanshu
@@ -269,7 +263,7 @@ Hey Atsushi We are working on a multiagent simulation game and this project
---
-Thanks a lot for this amazing set of very useful librarires!
+Thanks a lot for this amazing set of very useful libraries!
--Razvan Viorel Mihai
@@ -281,7 +275,7 @@ Dear Atsushi Sakai,
This is probably one of the best open-source roboti
---
-hanks frnd, for ur contibusion
+Thanks friend, for your contribution
—--
@@ -386,14 +380,14 @@ Dear Atsushi Sakai, Thank you so much for creating PythonRobotics and docume
1. B. Blaga, M. Deac, R. W. Y. Al-doori, M. Negru and R. Dǎnescu, "Miniature Autonomous Vehicle Development on Raspberry Pi," 2018 IEEE 14th International Conference on Intelligent Computer Communication and Processing (ICCP), Cluj-Napoca, Romania, 2018, pp. 229-236.
doi: 10.1109/ICCP.2018.8516589
keywords: {Automobiles;Task analysis;Autonomous vehicles;Path planning;Global Positioning System;Cameras;miniature autonomous vehicle;path planning;navigation;parking assist;lane detection and tracking;traffic sign recognition},
-URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8516589&isnumber=8516425
+URL: https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8516589&isnumber=8516425
2. Peggy (Yuchun) Wang and Caitlin Hogan, "Path Planning with Dynamic Obstacle Avoidance for a Jumping-Enabled Robot", AA228/CS238 class report, Department of Computer Science, Stanford University, URL: https://web.stanford.edu/class/aa228/reports/2018/final113.pdf
-3. Welburn, E, Hakim Khalili, H, Gupta, A, Watson, S & Carrasco, J 2019, A Navigational System for Quadcopter Remote Inspection of Offshore Substations. in The Fifteenth International Conference on Autonomic and Autonomous Systems 2019. URL:https://www.research.manchester.ac.uk/portal/files/107169964/ICAS19_A_Navigational_System_for_Quadcopter_Remote_Inspection_of_Offshore_Substations.pdf
+3. Welburn, E, Hakim Khalili, H, Gupta, A, Watson, S & Carrasco, J 2019, A Navigational System for Quadcopter Remote Inspection of Offshore Substations. in The Fifteenth International Conference on Autonomic and Autonomous Systems 2019. URL:https://research.manchester.ac.uk/portal/files/107169964/ICAS19_A_Navigational_System_for_Quadcopter_Remote_Inspection_of_Offshore_Substations.pdf
4. E. Horváth, C. Hajdu, C. Radu and Á. Ballagi, "Range Sensor-based Occupancy Grid Mapping with Signatures," 2019 20th International Carpathian Control Conference (ICCC), Krakow-Wieliczka, Poland, 2019, pp. 1-5.
-URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8765684&isnumber=8765679
+URL: https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8765684&isnumber=8765679
5. Josie Hughes, Masaru Shimizu, and Arnoud Visser, "A Review of Robot Rescue Simulation Platforms for Robotics Education"
URL: https://2019.robocup.org/downloads/program/HughesEtAl2019.pdf
@@ -408,7 +402,7 @@ URL: https://arxiv.org/abs/1910.01557
URL: https://pdfs.semanticscholar.org/5c06/f3cb9542a51e1bf1a32523c1bc7fea6cecc5.pdf
9. Brijen Thananjeyan, et al. "ABC-LMPC: Safe Sample-Based Learning MPC for Stochastic Nonlinear Dynamical Systems with Adjustable Boundary Conditions"
-URL: https://arxiv.org/pdf/2003.01410.pdf
+URL: https://arxiv.org/pdf/2003.01410
# Others
diff --git a/utils/plot.py b/utils/plot.py
index 2c70a60bbe..eb5aa7ad04 100644
--- a/utils/plot.py
+++ b/utils/plot.py
@@ -3,6 +3,74 @@
"""
import math
import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import art3d
+from matplotlib.patches import FancyArrowPatch
+from mpl_toolkits.mplot3d.proj3d import proj_transform
+from mpl_toolkits.mplot3d import Axes3D
+
+from utils.angle import rot_mat_2d
+
+
+def plot_covariance_ellipse(x, y, cov, chi2=3.0, color="-r", ax=None):
+ """
+ This function plots an ellipse that represents a covariance matrix. The ellipse is centered at (x, y) and its shape, size and rotation are determined by the covariance matrix.
+
+ Parameters:
+ x : (float) The x-coordinate of the center of the ellipse.
+ y : (float) The y-coordinate of the center of the ellipse.
+ cov : (numpy.ndarray) A 2x2 covariance matrix that determines the shape, size, and rotation of the ellipse.
+ chi2 : (float, optional) A scalar value that scales the ellipse size. This value is typically set based on chi-squared distribution quantiles to achieve certain confidence levels (e.g., 3.0 corresponds to ~95% confidence for a 2D Gaussian). Defaults to 3.0.
+ color : (str, optional) The color and line style of the ellipse plot, following matplotlib conventions. Defaults to "-r" (a red solid line).
+ ax : (matplotlib.axes.Axes, optional) The Axes object to draw the ellipse on. If None (default), a new figure and axes are created.
+
+ Returns:
+ None. This function plots the covariance ellipse on the specified axes.
+ """
+ eig_val, eig_vec = np.linalg.eig(cov)
+
+ if eig_val[0] >= eig_val[1]:
+ big_ind = 0
+ small_ind = 1
+ else:
+ big_ind = 1
+ small_ind = 0
+ a = math.sqrt(chi2 * eig_val[big_ind])
+ b = math.sqrt(chi2 * eig_val[small_ind])
+ angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
+ plot_ellipse(x, y, a, b, angle, color=color, ax=ax)
+
+
+def plot_ellipse(x, y, a, b, angle, color="-r", ax=None, **kwargs):
+ """
+ This function plots an ellipse based on the given parameters.
+
+ Parameters
+ ----------
+ x : (float) The x-coordinate of the center of the ellipse.
+ y : (float) The y-coordinate of the center of the ellipse.
+ a : (float) The length of the semi-major axis of the ellipse.
+ b : (float) The length of the semi-minor axis of the ellipse.
+ angle : (float) The rotation angle of the ellipse, in radians.
+ color : (str, optional) The color and line style of the ellipse plot, following matplotlib conventions. Defaults to "-r" (a red solid line).
+ ax : (matplotlib.axes.Axes, optional) The Axes object to draw the ellipse on. If None (default), a new figure and axes are created.
+ **kwargs: Additional keyword arguments to pass to plt.plot or ax.plot.
+
+ Returns
+ ---------
+ None. This function plots the ellipse based on the specified parameters.
+ """
+
+ t = np.arange(0, 2 * math.pi + 0.1, 0.1)
+ px = [a * math.cos(it) for it in t]
+ py = [b * math.sin(it) for it in t]
+ fx = rot_mat_2d(angle) @ (np.array([px, py]))
+ px = np.array(fx[0, :] + x).flatten()
+ py = np.array(fx[1, :] + y).flatten()
+ if ax is None:
+ plt.plot(px, py, color, **kwargs)
+ else:
+ ax.plot(px, py, color, **kwargs)
def plot_arrow(x, y, yaw, arrow_length=1.0,
@@ -47,3 +115,120 @@ def plot_arrow(x, y, yaw, arrow_length=1.0,
**kwargs)
if origin_point_plot_style is not None:
plt.plot(x, y, origin_point_plot_style)
+
+
+def plot_curvature(x_list, y_list, heading_list, curvature,
+ k=0.01, c="-c", label="Curvature"):
+ """
+ Plot curvature on 2D path. This plot is a line from the original path,
+ the lateral distance from the original path shows curvature magnitude.
+ Left turning shows right side plot, right turning shows left side plot.
+ For straight path, the curvature plot will be on the path, because
+ curvature is 0 on the straight path.
+
+ Parameters
+ ----------
+ x_list : array_like
+ x position list of the path
+ y_list : array_like
+ y position list of the path
+ heading_list : array_like
+ heading list of the path
+ curvature : array_like
+ curvature list of the path
+ k : float
+ curvature scale factor to calculate distance from the original path
+ c : string
+ color of the plot
+ label : string
+ label of the plot
+ """
+ cx = [x + d * k * np.cos(yaw - np.pi / 2.0) for x, y, yaw, d in
+ zip(x_list, y_list, heading_list, curvature)]
+ cy = [y + d * k * np.sin(yaw - np.pi / 2.0) for x, y, yaw, d in
+ zip(x_list, y_list, heading_list, curvature)]
+
+ plt.plot(cx, cy, c, label=label)
+ for ix, iy, icx, icy in zip(x_list, y_list, cx, cy):
+ plt.plot([ix, icx], [iy, icy], c)
+
+
+class Arrow3D(FancyArrowPatch):
+
+ def __init__(self, x, y, z, dx, dy, dz, *args, **kwargs):
+ super().__init__((0, 0), (0, 0), *args, **kwargs)
+ self._xyz = (x, y, z)
+ self._dxdydz = (dx, dy, dz)
+
+ def draw(self, renderer):
+ x1, y1, z1 = self._xyz
+ dx, dy, dz = self._dxdydz
+ x2, y2, z2 = (x1 + dx, y1 + dy, z1 + dz)
+
+ xs, ys, zs = proj_transform((x1, x2), (y1, y2), (z1, z2), self.axes.M)
+ self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
+ super().draw(renderer)
+
+ def do_3d_projection(self, renderer=None):
+ x1, y1, z1 = self._xyz
+ dx, dy, dz = self._dxdydz
+ x2, y2, z2 = (x1 + dx, y1 + dy, z1 + dz)
+
+ xs, ys, zs = proj_transform((x1, x2), (y1, y2), (z1, z2), self.axes.M)
+ self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
+
+ return np.min(zs)
+
+
+def _arrow3D(ax, x, y, z, dx, dy, dz, *args, **kwargs):
+ '''Add an 3d arrow to an `Axes3D` instance.'''
+ arrow = Arrow3D(x, y, z, dx, dy, dz, *args, **kwargs)
+ ax.add_artist(arrow)
+
+
+def plot_3d_vector_arrow(ax, p1, p2):
+ setattr(Axes3D, 'arrow3D', _arrow3D)
+ ax.arrow3D(p1[0], p1[1], p1[2],
+ p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2],
+ mutation_scale=20,
+ arrowstyle="-|>",
+ )
+
+
+def plot_triangle(p1, p2, p3, ax):
+ ax.add_collection3d(art3d.Poly3DCollection([[p1, p2, p3]], color='b'))
+
+
+def set_equal_3d_axis(ax, x_lims, y_lims, z_lims):
+ """Helper function to set equal axis
+
+ Args:
+ ax (Axes3DSubplot): matplotlib 3D axis, created by
+ `ax = fig.add_subplot(projection='3d')`
+ x_lims (np.array): array containing min and max value of x
+ y_lims (np.array): array containing min and max value of y
+ z_lims (np.array): array containing min and max value of z
+ """
+ x_lims = np.asarray(x_lims)
+ y_lims = np.asarray(y_lims)
+ z_lims = np.asarray(z_lims)
+ # compute max required range
+ max_range = np.array([x_lims.max() - x_lims.min(),
+ y_lims.max() - y_lims.min(),
+ z_lims.max() - z_lims.min()]).max() / 2.0
+ # compute mid-point along each axis
+ mid_x = (x_lims.max() + x_lims.min()) * 0.5
+ mid_y = (y_lims.max() + y_lims.min()) * 0.5
+ mid_z = (z_lims.max() + z_lims.min()) * 0.5
+
+ # set limits to axis
+ ax.set_xlim(mid_x - max_range, mid_x + max_range)
+ ax.set_ylim(mid_y - max_range, mid_y + max_range)
+ ax.set_zlim(mid_z - max_range, mid_z + max_range)
+
+
+if __name__ == '__main__':
+ plot_ellipse(0, 0, 1, 2, np.deg2rad(15))
+ plt.axis('equal')
+ plt.show()
+
-Documentation: [Notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb)
+Reference
+
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
## Particle filter localization
@@ -173,7 +186,7 @@ It is assumed that the robot can measure a distance from landmarks (RFID).
These measurements are used for PF localization.
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -194,7 +207,7 @@ The filter integrates speed input and range observations from RFID for localizat
Initial position is not needed.
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -243,7 +256,7 @@ It can calculate a rotation matrix, and a translation vector between points and
-Ref:
+Reference
- [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
@@ -262,7 +275,7 @@ Black points are landmarks, blue crosses are estimated landmark positions by Fas
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -308,7 +321,7 @@ 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.
-Ref:
+Reference
- [D* Algorithm Wikipedia](https://en.wikipedia.org/wiki/D*)
@@ -322,7 +335,7 @@ The animation shows a robot finding its path and rerouting to avoid obstacles as
Refs:
-- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pd)
+- [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
@@ -333,7 +346,7 @@ 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.
-Ref:
+Reference
- [Robotic Motion Planning:Potential Functions](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf)
@@ -349,11 +362,11 @@ This script is a path planning code with state lattice planning.
This code uses the model predictive trajectory generator to solve boundary problem.
-Ref:
+Reference
-- [Optimal rough terrain trajectory generation for wheeled mobile robots](http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
+- [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](http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
+- [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
@@ -377,7 +390,7 @@ Cyan crosses means searched points with Dijkstra method,
The red line is the final path of PRM.
-Ref:
+Reference
- [Probabilistic roadmap \- Wikipedia](https://en.wikipedia.org/wiki/Probabilistic_roadmap)
@@ -393,15 +406,15 @@ 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.
-Ref:
+Reference
- [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416)
-- [Sampling-based Algorithms for Optimal Motion Planning](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf)
+- [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.
@@ -413,9 +426,9 @@ A double integrator motion model is used for LQR local planner.
-Ref:
+Reference
-- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](http://lis.csail.mit.edu/pubs/perez-icra12.pdf)
+- [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)
@@ -428,9 +441,9 @@ Motion planning with quintic polynomials.
It can calculate a 2D path, velocity, and acceleration profile based on quintic polynomials.
-Ref:
+Reference
-- [Local Path Planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
+- [Local Path Planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
## Reeds Shepp planning
@@ -438,7 +451,7 @@ A sample code with Reeds Shepp path planning.
-Ref:
+Reference
- [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html)
@@ -464,7 +477,7 @@ The cyan line is the target course and black crosses are obstacles.
The red line is the predicted path.
-Ref:
+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)
@@ -477,9 +490,9 @@ Ref:
This is a simulation of moving to a pose control
-
+
-Ref:
+Reference
- [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
@@ -490,7 +503,7 @@ Path tracking simulation with Stanley steering control and PID speed control.
-Ref:
+Reference
- [Stanley: The robot that won the DARPA grand challenge](http://robots.stanford.edu/papers/thrun.stanley05.pdf)
@@ -504,7 +517,7 @@ Path tracking simulation with rear wheel feedback steering control and PID speed
-Ref:
+Reference
- [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
@@ -515,9 +528,9 @@ Path tracking simulation with LQR speed and steering control.
-Ref:
+Reference
-- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](http://ieeexplore.ieee.org/document/5940562/)
+- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](https://ieeexplore.ieee.org/document/5940562/)
## Model predictive speed and steering control
@@ -526,9 +539,9 @@ Path tracking simulation with iterative linear model predictive speed and steeri
-Ref:
+Reference
-- [notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb)
+- [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)
@@ -538,9 +551,9 @@ A motion planning and path tracking simulation with NMPC of C-GMRES
-Ref:
+Reference
-- [notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/cgmres_nmpc/cgmres_nmpc.ipynb)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc.html)
# Arm Navigation
@@ -578,9 +591,9 @@ This is a 3d trajectory generation simulation for a rocket powered landing.
-Ref:
+Reference
-- [notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/AerialNavigation/rocket_powered_landing/rocket_powered_landing.ipynb)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
# Bipedal
@@ -608,7 +621,7 @@ This is a list of user's comment and references:[users\_comments](https://github
Any contribution is welcome!!
-Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/how_to_contribute.html)
+Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html)
# Citing
@@ -616,7 +629,7 @@ If you use this project's code for your academic work, we encourage you to cite
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.
-# Supporting this project
+# Supporting this project
If you or your company would like to support this project, please consider:
@@ -624,16 +637,21 @@ If you or your company would like to support this project, please consider:
- [Become a backer or sponsor on Patreon](https://www.patreon.com/myenigma)
-- [One-time donation via PayPal](https://www.paypal.me/myenigmapay/)
+- [One-time donation via PayPal](https://www.paypal.com/paypalme/myenigmapay/)
If you would like to support us in some other way, please contact with creating an issue.
-## Sponsors
+## Sponsors
-### [JetBrains](https://www.jetbrains.com/)
+### [JetBrains](https://www.jetbrains.com/)
They are providing a free license of their IDEs for this OSS development.
+### [1Password](https://github.com/1Password/for-open-source)
+
+They are providing a free license of their 1Password team license for this OSS project.
+
+
# Authors
- [Contributors to AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/graphs/contributors)
diff --git a/SECURITY.md b/SECURITY.md
index 5047a6fc3d..53dcafa450 100644
--- a/SECURITY.md
+++ b/SECURITY.md
@@ -10,4 +10,4 @@ In this project, we are only support latest code.
## Reporting a Vulnerability
-If you find any sequrity related problem, let us know by creating an issue about it.
+If you find any security related problem, let us know by creating an issue about it.
diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index bb648ce9d9..5c4417d7c3 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/SLAM/EKFSLAM/ekf_slam.py
@@ -8,6 +8,7 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2
@@ -28,10 +29,9 @@
def ekf_slam(xEst, PEst, u, z):
# Predict
- S = STATE_SIZE
- G, Fx = jacob_motion(xEst[0:S], u)
- xEst[0:S] = motion_model(xEst[0:S], u)
- PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
+ G, Fx = jacob_motion(xEst, u)
+ xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
+ PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
initP = np.eye(2)
# Update
@@ -119,7 +119,7 @@ def jacob_motion(x, u):
[0.0, 0.0, DT * u[0, 0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]], dtype=float)
- G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
+ G = np.eye(len(x)) + Fx.T @ jF @ Fx
return G, Fx,
@@ -192,7 +192,7 @@ def jacob_h(q, delta, x, i):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py
index c4beaba257..98f8a66417 100644
--- a/SLAM/FastSLAM1/fast_slam1.py
+++ b/SLAM/FastSLAM1/fast_slam1.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -71,19 +72,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
- # print(xEst)
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -194,7 +194,7 @@ def compute_weight(particle, z, Q_cov):
print("singular")
return 1.0
- num = math.exp(-0.5 * dx.T @ invS @ dx)
+ num = np.exp(-0.5 * (dx.T @ invS @ dx))[0, 0]
den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
w = num / den
@@ -234,28 +234,27 @@ def resampling(particles):
pw = np.array(pw)
n_eff = 1.0 / (pw @ pw.T) # Effective particle number
- # print(n_eff)
if n_eff < NTH: # resampling
w_cum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -274,34 +273,34 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, rfid):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
for i in range(len(rfid[:, 0])):
- dx = rfid[i, 0] - xTrue[0, 0]
- dy = rfid[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_with_noize = angle + np.random.randn() * Q_sim[
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_with_noize = angle + np.random.randn() * Q_SIM[
1, 1] ** 0.5 # add noise
zi = np.array([dn, pi_2_pi(angle_with_noize), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -321,7 +320,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -330,7 +329,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -339,17 +338,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -357,18 +356,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam1(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -376,16 +375,16 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event', lambda event:
[exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py
index 7cd708df81..d4cf0d84dd 100644
--- a/SLAM/FastSLAM2/fast_slam2.py
+++ b/SLAM/FastSLAM2/fast_slam2.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -34,16 +35,16 @@
class Particle:
- def __init__(self, N_LM):
+ def __init__(self, n_landmark):
self.w = 1.0 / N_PARTICLE
self.x = 0.0
self.y = 0.0
self.yaw = 0.0
self.P = np.eye(3)
# landmark x-y positions
- self.lm = np.zeros((N_LM, LM_SIZE))
+ self.lm = np.zeros((n_landmark, LM_SIZE))
# landmark position covariance
- self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))
+ self.lmP = np.zeros((n_landmark * LM_SIZE, LM_SIZE))
def fast_slam2(particles, u, z):
@@ -72,18 +73,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -193,7 +194,7 @@ def compute_weight(particle, z, Q_cov):
except np.linalg.linalg.LinAlgError:
return 1.0
- num = math.exp(-0.5 * dz.T @ invS @ dz)
+ num = np.exp(-0.5 * dz.T @ invS @ dz)[0, 0]
den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
w = num / den
@@ -265,21 +266,21 @@ def resampling(particles):
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -298,35 +299,35 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, RFID):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
- for i in range(len(RFID[:, 0])):
+ for i in range(len(rfid[:, 0])):
- dx = RFID[i, 0] - xTrue[0, 0]
- dy = RFID[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_noise = np.random.randn() * Q_sim[1, 1] ** 0.5
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_noise = np.random.randn() * Q_SIM[1, 1] ** 0.5
angle_with_noise = angle + angle_noise # add noise
zi = np.array([dn, pi_2_pi(angle_with_noise), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -346,7 +347,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -355,7 +356,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -364,17 +365,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -382,18 +383,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam2(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -401,21 +402,21 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for iz in range(len(z[:, 0])):
landmark_id = int(z[2, iz])
- plt.plot([xEst[0], RFID[landmark_id, 0]], [
- xEst[1], RFID[landmark_id, 1]], "-k")
+ plt.plot([x_est[0][0], rfid[landmark_id, 0]], [
+ x_est[1][0], rfid[landmark_id, 1]], "-k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/GraphBasedSLAM/data/README.rst b/SLAM/GraphBasedSLAM/data/README.rst
index c875cce73f..15bc5b6c03 100644
--- a/SLAM/GraphBasedSLAM/data/README.rst
+++ b/SLAM/GraphBasedSLAM/data/README.rst
@@ -3,4 +3,4 @@ Acknowledgments and References
Thanks to Luca Larlone for allowing inclusion of the `Intel dataset