diff --git a/.circleci/config.yml b/.circleci/config.yml
index 9f803ece4a..f6eff674de 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -6,7 +6,7 @@ orbs:
 jobs:
   build_doc:
     docker:
-      - image: cimg/python:3.12
+      - image: cimg/python:3.13
     steps:
       - checkout
       - run:
diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md
index b6ac52efa2..c0d9f7eab2 100644
--- a/.github/pull_request_template.md
+++ b/.github/pull_request_template.md
@@ -1,7 +1,7 @@
 <!-- 
 Thanks for contributing a pull request! 
 Please check this document before submitting:
-- [How to contribute](https://atsushisakai.github.io/PythonRobotics/how_to_contribute.html#adding-a-new-algorithm-example)
+- [How to contribute](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html#adding-a-new-algorithm-example)
 
 Note that this is my hobby project; I appreciate your
 patience during the review process.
diff --git a/.github/workflows/Linux_CI.yml b/.github/workflows/Linux_CI.yml
index 7b3dc14751..22a8baa4e7 100644
--- a/.github/workflows/Linux_CI.yml
+++ b/.github/workflows/Linux_CI.yml
@@ -12,12 +12,12 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        python-version: [ '3.12' ]
+        python-version: [ '3.13' ]
 
     name: Python ${{ matrix.python-version }} CI
 
     steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v5
     - run: git fetch --prune --unshallow
 
     - name: Setup python
diff --git a/.github/workflows/MacOS_CI.yml b/.github/workflows/MacOS_CI.yml
index 5ea15ac72e..aa162cde2e 100644
--- a/.github/workflows/MacOS_CI.yml
+++ b/.github/workflows/MacOS_CI.yml
@@ -16,10 +16,10 @@ jobs:
     runs-on: macos-latest
     strategy:
       matrix:
-        python-version: [ '3.12' ]
+        python-version: [ '3.13' ]
     name: Python ${{ matrix.python-version }} CI
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - run: git fetch --prune --unshallow
 
       - name: Update bash
diff --git a/.github/workflows/Windows_CI.yml b/.github/workflows/Windows_CI.yml
index b9c8dea649..4d83a3a696 100644
--- a/.github/workflows/Windows_CI.yml
+++ b/.github/workflows/Windows_CI.yml
@@ -16,10 +16,10 @@ jobs:
     runs-on: windows-latest
     strategy:
       matrix:
-        python-version: [ '3.12' ]
+        python-version: [ '3.13' ]
     name: Python ${{ matrix.python-version }} CI
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - run: git fetch --prune --unshallow
 
       - name: Setup python
diff --git a/.github/workflows/circleci-artifacts-redirector.yml b/.github/workflows/circleci-artifacts-redirector.yml
index 78eb479aa1..f6dd3f2e7c 100644
--- a/.github/workflows/circleci-artifacts-redirector.yml
+++ b/.github/workflows/circleci-artifacts-redirector.yml
@@ -6,7 +6,7 @@ jobs:
     name: Run CircleCI artifacts redirector!!
     steps:
       - name: run-circleci-artifacts-redirector
-        uses: larsoner/circleci-artifacts-redirector-action@v1.0.0
+        uses: larsoner/circleci-artifacts-redirector-action@v1.2.0
         with:
           repo-token: ${{ secrets.GITHUB_TOKEN }}
           api-token: ${{ secrets.CIRCLECI_TOKEN }}
diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml
index 878a4a4435..0ed1d7e90d 100644
--- a/.github/workflows/codeql.yml
+++ b/.github/workflows/codeql.yml
@@ -16,7 +16,7 @@ jobs:
 
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      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.
diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
index 239f3629c1..e8ba8fa220 100644
--- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
+++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
@@ -5,7 +5,7 @@
 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.
 
@@ -31,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
 
@@ -462,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] +
@@ -536,7 +537,7 @@ def solve(self, **kwargs):
             with warnings.catch_warnings():  # For User warning from solver
                 warnings.simplefilter('ignore')
                 self.prob.solve(verbose=verbose_solver,
-                            solver=solver)
+                                solver=solver)
         except cvxpy.SolverError:
             error = True
 
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 c2227f18e3..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)
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 100%
rename from Control/inverted_pendulum/inverted_pendulum_lqr_control.py
rename to InvertedPendulum/inverted_pendulum_lqr_control.py
diff --git a/Control/inverted_pendulum/inverted_pendulum_mpc_control.py b/InvertedPendulum/inverted_pendulum_mpc_control.py
similarity index 98%
rename from Control/inverted_pendulum/inverted_pendulum_mpc_control.py
rename to InvertedPendulum/inverted_pendulum_mpc_control.py
index 9a5fa2ab41..c45dde8acc 100644
--- a/Control/inverted_pendulum/inverted_pendulum_mpc_control.py
+++ b/InvertedPendulum/inverted_pendulum_mpc_control.py
@@ -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/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
index 2bab3b49c1..e8a988a270 100644
--- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
+++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
@@ -4,7 +4,7 @@
 
 author: Ryohei Sasaki(rsasaki0109)
 
-Ref:
+Reference:
 Ensemble Kalman filtering
 (https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
 
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 2eba550127..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)
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/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/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py
index 177f078871..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/
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
+
+            <Sequence>
+                <Action1 />
+                <Action2 />
+            </Sequence>
+    """
+
+    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
+
+            <Selector>
+                <Action1 />
+                <Action2 />
+            </Selector>
+    """
+
+    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
+
+            <WhileDoElse>
+                <Condition />
+                <Do />
+                <Else />
+            </WhileDoElse>
+    """
+
+    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
+
+            <Sleep sec="1.5" />
+    """
+
+    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
+
+            <Echo message="Hello, World!" />
+    """
+
+    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
+
+            <Inverter>
+                <Action />
+            </Inverter>
+    """
+
+    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
+
+            <Timeout sec="1.5">
+                <Action />
+            </Timeout>
+    """
+
+    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
+
+            <Delay sec="1.5">
+                <Action />
+            </Delay>
+    """
+
+    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: <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 = """
+    <Sequence name="Sequence">
+        <Echo name="Echo0" message="Hello, World0!" />
+        <Delay name="Delay" sec="1.5">
+            <Echo name="Echo1" message="Hello, World1!" />
+        </Delay>
+        <Echo name="Echo2" message="Hello, World2!" />
+    </Sequence>
+   """
+
+
+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 @@
+<Selector name="Robot Main Controller">
+    <!-- Charge battery when power is low -->
+    <Sequence name="Battery Management">
+        <Inverter name="Low Battery Detection">
+            <CheckBattery name="Check Battery" threshold="30" />
+        </Inverter>
+        <Echo name="Low Battery Warning" message="Battery level low! Charging needed" />
+        <ChargeBattery name="Charge Battery" charge_rate="20" />
+    </Sequence>
+    <!-- Main task sequence -->
+    <Sequence name="Patrol Task">
+        <Echo name="Start Task" message="Starting patrol task" />
+        <!-- Move to position A -->
+        <Sequence name="Move to Position A">
+            <MoveToPosition name="Move to A" position="A" move_duration="2" />
+            <!-- Handle obstacles -->
+            <Selector name="Obstacle Handling A">
+                <Sequence name="Obstacle Present">
+                    <DetectObstacle name="Detect Obstacle" obstacle_probability="0.3" />
+                    <AvoidObstacle name="Avoid Obstacle" avoid_duration="1.5" />
+                </Sequence>
+                <Echo name="No Obstacle" message="Path clear" />
+            </Selector>
+            <PerformTask name="Position A Task" task_name="Check Device Status" task_duration="2" />
+        </Sequence>
+        <!-- Move to position B -->
+        <Sequence name="Move to Position B">
+            <Delay name="Short Wait" sec="1">
+                <Echo name="Prepare Movement" message="Preparing to move to next position" />
+            </Delay>
+            <MoveToPosition name="Move to B" position="B" move_duration="3" />
+            <!-- Handle obstacles with timeout -->
+            <Timeout name="Limited Time Obstacle Handling" sec="2">
+                <Sequence name="Obstacle Present">
+                    <DetectObstacle name="Detect Obstacle" obstacle_probability="0.4" />
+                    <AvoidObstacle name="Avoid Obstacle" avoid_duration="1.8" />
+                </Sequence>
+            </Timeout>
+            <PerformTask name="Position B Task" task_name="Data Collection" task_duration="2.5" />
+        </Sequence>
+        <!-- Move to position C -->
+        <WhileDoElse name="Conditional Move to C">
+            <CheckBattery name="Check Sufficient Battery" threshold="50" />
+            <Sequence name="Perform Position C Task">
+                <MoveToPosition name="Move to C" position="C" move_duration="2.5" />
+                <ForceSuccess name="Ensure Completion">
+                    <PerformTask name="Position C Task" task_name="Environment Monitoring"
+                        task_duration="2" />
+                </ForceSuccess>
+            </Sequence>
+            <Echo name="Skip Position C" message="Insufficient power, skipping position C task" />
+        </WhileDoElse>
+        <Echo name="Complete Patrol" message="Patrol task completed, returning to charging station" />
+        <MoveToPosition name="Return to Charging Station" position="Charging Station"
+            move_duration="4" />
+    </Sequence>
+</Selector> 
\ 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_<state>' and 'on_exit_<state>' 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_<state>' 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_<state>' 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/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py
similarity index 100%
rename from PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
rename to PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py
diff --git a/PathPlanning/DStarLite/d_star_lite.py b/PathPlanning/DStarLite/d_star_lite.py
index f099530173..1a44d84fa5 100644
--- a/PathPlanning/DStarLite/d_star_lite.py
+++ b/PathPlanning/DStarLite/d_star_lite.py
@@ -63,9 +63,7 @@ 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 = np.array(
-            [[obstacle.x, obstacle.y] for obstacle in self.obstacles]
-        )
+        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
@@ -73,7 +71,7 @@ def __init__(self, ox: list, oy: list):
         self.kold = 0.0
         self.rhs = self.create_grid(float("inf"))
         self.g = self.create_grid(float("inf"))
-        self.detected_obstacles_xy = np.empty((0, 2))
+        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
@@ -84,18 +82,8 @@ def create_grid(self, val: float):
         return np.full((self.x_max, self.y_max), val)
 
     def is_obstacle(self, node: Node):
-        x = np.array([node.x])
-        y = np.array([node.y])
-        obstacle_x_equal = self.obstacles_xy[:, 0] == x
-        obstacle_y_equal = self.obstacles_xy[:, 1] == y
-        is_in_obstacles = (obstacle_x_equal & obstacle_y_equal).any()
-
-        is_in_detected_obstacles = False
-        if self.detected_obstacles_xy.shape[0] > 0:
-            is_x_equal = self.detected_obstacles_xy[:, 0] == x
-            is_y_equal = self.detected_obstacles_xy[:, 1] == y
-            is_in_detected_obstacles = (is_x_equal & is_y_equal).any()
-
+        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):
@@ -157,7 +145,7 @@ def initialize(self, start: Node, goal: Node):
             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 = np.empty((0, 2))
+            self.detected_obstacles_xy = set()
 
     def update_vertex(self, u: Node):
         if not compare_coordinates(u, self.goal):
@@ -215,12 +203,7 @@ def detect_changes(self):
                    compare_coordinates(spoofed_obstacle, self.goal):
                     continue
                 changed_vertices.append(spoofed_obstacle)
-                self.detected_obstacles_xy = np.concatenate(
-                    (
-                        self.detected_obstacles_xy,
-                        [[spoofed_obstacle.x, spoofed_obstacle.y]]
-                    )
-                )
+                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)
@@ -241,12 +224,7 @@ def detect_changes(self):
                compare_coordinates(new_obs, self.goal):
                 return changed_vertices
             changed_vertices.append(Node(x, y))
-            self.detected_obstacles_xy = np.concatenate(
-                (
-                    self.detected_obstacles_xy,
-                    [[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/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py
index 8a585e4b18..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):
         """
@@ -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/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 a73797dacb..e72d33261e 100644
--- a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
+++ b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
@@ -29,7 +29,7 @@ def __init__(self, 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
 
@@ -300,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)
@@ -334,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)
@@ -400,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
index 4cc8650c89..482712ceaf 100644
--- a/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py
+++ b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py
@@ -4,7 +4,7 @@
 
 author: Wang Zheng (@Aglargil)
 
-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)
diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
index 248894c1c6..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)
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/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
index fdc181afab..86f9f662da 100644
--- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
+++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
@@ -4,7 +4,7 @@
 
 author: Atsushi Sakai (@Atsushi_twi)
 
-Ref:
+Reference:
 
 - [Local Path planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
 
diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py
index 2ed6a366d1..ac68efe23f 100644
--- a/PathPlanning/RRT/rrt_with_pathsmoothing.py
+++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py
@@ -51,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.hypot(a, 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):
@@ -94,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
@@ -119,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/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
index 7f8e725e0a..554cd0f3b7 100644
--- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py
+++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
@@ -8,7 +8,7 @@
 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
@@ -22,7 +22,9 @@
 import numpy as np
 import math
 import pathlib
+
 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
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/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py
index a582c9da81..ee40e73504 100644
--- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py
+++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py
@@ -4,7 +4,7 @@
 
 author Atsushi Sakai (@Atsushi_twi)
 
-Ref:
+Reference:
 Shunichi09/nonlinear_control: Implementing the nonlinear model predictive
 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 d85fa98a84..5831d02d30 100644
--- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
+++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
@@ -11,9 +11,9 @@
 import numpy as np
 import scipy.linalg as la
 import pathlib
-from utils.angle import angle_mod
-sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
 from PathPlanning.CubicSpline import cubic_spline_planner
 
 # === Parameters =====
diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py
index 461d6e3722..3c066917ff 100644
--- a/PathTracking/lqr_steer_control/lqr_steer_control.py
+++ b/PathTracking/lqr_steer_control/lqr_steer_control.py
@@ -11,9 +11,9 @@
 import numpy as np
 import sys
 import pathlib
-from utils.angle import angle_mod
-sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 
+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
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 68%
rename from Control/move_to_pose/move_to_pose.py
rename to PathTracking/move_to_pose/move_to_pose.py
index 279ba0625b..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,8 +13,13 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
-from random import random
+import sys
+import pathlib
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 from utils.angle import angle_mod
+from random import random
+
 
 class PathFinderController:
     """
@@ -70,14 +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 = angle_mod(np.arctan2(y_diff, x_diff) - theta)
-        beta = angle_mod(theta_goal - theta - alpha)
         v = self.Kp_rho * rho
-        w = self.Kp_alpha * alpha - self.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
 
@@ -85,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
@@ -101,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
@@ -120,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)
@@ -144,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)
@@ -162,15 +193,16 @@ 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.0 * random()
         y_start = 20.0 * random()
@@ -178,10 +210,14 @@ def main():
         x_goal = 20 * random()
         y_goal = 20 * random()
         theta_goal = 2 * np.pi * random() - np.pi
-        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")
+        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 100%
rename from Control/move_to_pose/move_to_pose_robot.py
rename to PathTracking/move_to_pose/move_to_pose_robot.py
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 98%
rename from PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
rename to PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
index 1702bd47dd..fd04fb6d17 100644
--- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
+++ b/PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
@@ -8,11 +8,14 @@
 import matplotlib.pyplot as plt
 import math
 import numpy as np
-from utils.angle import angle_mod
-
+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
diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_control/stanley_control.py
similarity index 99%
rename from PathTracking/stanley_controller/stanley_controller.py
rename to PathTracking/stanley_control/stanley_control.py
index 08aa049395..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)
 
@@ -13,9 +13,9 @@
 import matplotlib.pyplot as plt
 import sys
 import pathlib
-from utils.angle import angle_mod
-sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 
+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
diff --git a/README.md b/README.md
index cb1816c2b5..d1b801f219 100644
--- a/README.md
+++ b/README.md
@@ -87,7 +87,11 @@ Features:
 
 See this documentation 
 
-- [Getting Started — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/getting_started.html#what-is-pythonrobotics)
+- [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:
 
@@ -98,7 +102,7 @@ or this paper for more details:
 
 For running each sample code:
 
-- [Python 3.12.x](https://www.python.org/)
+- [Python 3.13.x](https://www.python.org/)
  
 - [NumPy](https://numpy.org/)
  
@@ -164,9 +168,9 @@ All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation
 
 <img src="https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif" width="640" alt="EKF pic">
 
-Ref:
+Reference
 
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
 
 ## Particle filter localization
 
@@ -182,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/)
 
@@ -203,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/)
 
@@ -252,7 +256,7 @@ It can calculate a rotation matrix, and a translation vector between points and
 
 ![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif)
 
-Ref:
+Reference
 
 - [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
 
@@ -271,7 +275,7 @@ Black points are landmarks, blue crosses are estimated landmark positions by Fas
 ![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif)
 
 
-Ref:
+Reference
 
 - [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
 
@@ -317,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*)
 
@@ -342,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)
 
@@ -358,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](https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
 
-- [State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments](https://www.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
@@ -386,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)
 
@@ -402,7 +406,7 @@ 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)
 
@@ -422,7 +426,7 @@ A double integrator motion model is used for LQR local planner.
 
 ![LQR_RRT](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif)
 
-Ref:
+Reference
 
 - [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](https://lis.csail.mit.edu/pubs/perez-icra12.pdf)
 
@@ -437,7 +441,7 @@ 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](https://ieeexplore.ieee.org/document/637936/)
 
@@ -447,7 +451,7 @@ A sample code with Reeds Shepp path planning.
 
 ![RSPlanning](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true)
 
-Ref:
+Reference
 
 - [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html) 
 
@@ -473,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)
 
@@ -486,9 +490,9 @@ Ref:
 
 This is a simulation of moving to a pose control
 
-![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif)
+![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/move_to_pose/animation.gif)
 
-Ref:
+Reference
 
 - [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
 
@@ -499,7 +503,7 @@ Path tracking simulation with Stanley steering control and PID speed control.
 
 ![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif)
 
-Ref:
+Reference
 
 - [Stanley: The robot that won the DARPA grand challenge](http://robots.stanford.edu/papers/thrun.stanley05.pdf)
 
@@ -513,7 +517,7 @@ Path tracking simulation with rear wheel feedback steering control and PID speed
 
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif)
 
-Ref:
+Reference
 
 - [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
 
@@ -524,7 +528,7 @@ Path tracking simulation with LQR speed and steering control.
 
 ![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif)
 
-Ref:
+Reference
 
 - [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](https://ieeexplore.ieee.org/document/5940562/)
 
@@ -535,9 +539,9 @@ Path tracking simulation with iterative linear model predictive speed and steeri
 
 <img src="https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif" width="640" alt="MPC pic">
 
-Ref:
+Reference
 
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)
+- [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)
 
@@ -547,9 +551,9 @@ A motion planning and path tracking simulation with NMPC of C-GMRES
 
 ![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif)
 
-Ref:
+Reference
 
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/cgmres_nmpc/cgmres_nmpc.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc.html)
 
 
 # Arm Navigation
@@ -587,9 +591,9 @@ This is a 3d trajectory generation simulation for a rocket powered landing.
 
 ![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/rocket_powered_landing/animation.gif)
 
-Ref:
+Reference
 
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
 
 # Bipedal
 
@@ -617,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
 
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 05ad8a2820..72d89acf11 100644
--- a/appveyor.yml
+++ b/appveyor.yml
@@ -8,7 +8,7 @@ environment:
     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/_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 919fa9ac76..eeabab11b1 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -13,6 +13,8 @@
 #
 import os
 import sys
+from pathlib import Path
+
 sys.path.insert(0, os.path.abspath('../'))
 
 
@@ -41,7 +43,7 @@
     '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',
@@ -184,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/index_main.rst b/docs/index_main.rst
index c1e8d22d32..65634f32e8 100644
--- a/docs/index_main.rst
+++ b/docs/index_main.rst
@@ -43,7 +43,8 @@ this graph shows GitHub star history of this project:
    modules/7_arm_navigation/arm_navigation
    modules/8_aerial_navigation/aerial_navigation
    modules/9_bipedal/bipedal
-   modules/10_control/control
+   modules/10_inverted_pendulum/inverted_pendulum
+   modules/13_mission_planning/mission_planning
    modules/11_utils/utils
    modules/12_appendix/appendix
 
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
index 8c932b7263..2a7bd574f0 100644
--- 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
@@ -110,6 +110,12 @@ the following additional libraries are required.
 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
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
diff --git a/docs/modules/0_getting_started/3_how_to_contribute_main.rst b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
index 6e5c1be8ee..0325aaacae 100644
--- a/docs/modules/0_getting_started/3_how_to_contribute_main.rst
+++ b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
@@ -9,10 +9,30 @@ There are several ways to contribute to this project as below:
 #. `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`:
 
-Adding a new algorithm example
+1. Adding a new algorithm example
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This is a step by step manual to add a new algorithm example.
@@ -87,6 +107,18 @@ 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
+
+
 
 .. _`submit a pull request`:
 
@@ -112,8 +144,8 @@ Note that this is my hobby project; I appreciate your patience during the review
 
 .. _`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.
 
@@ -136,8 +168,8 @@ 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.
 
@@ -150,8 +182,8 @@ 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!!.
 
@@ -165,9 +197,9 @@ 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.
 
-Current Major 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.
 
@@ -183,11 +215,13 @@ Current Major Sponsors
 .. _`doc README`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/docs/README.md
 .. _`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.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/10_control/control_main.rst b/docs/modules/10_control/control_main.rst
deleted file mode 100644
index cee2aa9e8e..0000000000
--- a/docs/modules/10_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/10_control/inverted_pendulum_control/inverted-pendulum.png b/docs/modules/10_inverted_pendulum/inverted-pendulum.png
similarity index 100%
rename from docs/modules/10_control/inverted_pendulum_control/inverted-pendulum.png
rename to docs/modules/10_inverted_pendulum/inverted-pendulum.png
diff --git a/docs/modules/10_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/10_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/10_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/utils_main.rst b/docs/modules/11_utils/utils_main.rst
index ff79a26205..95c982b077 100644
--- a/docs/modules/11_utils/utils_main.rst
+++ b/docs/modules/11_utils/utils_main.rst
@@ -1,4 +1,4 @@
-.. _utils:
+.. _`utils`:
 
 Utilities
 ==========
diff --git a/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
index 9ae6fc5bcb..b7ff84e6f6 100644
--- a/docs/modules/12_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/12_appendix/Kalmanfilter_basics_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst
index 6548377e07..a1d99d47ef 100644
--- a/docs/modules/12_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/12_appendix/appendix_main.rst b/docs/modules/12_appendix/appendix_main.rst
index cb1ac04066..d0b9eeea3a 100644
--- a/docs/modules/12_appendix/appendix_main.rst
+++ b/docs/modules/12_appendix/appendix_main.rst
@@ -1,4 +1,4 @@
-.. _appendix:
+.. _`Appendix`:
 
 Appendix
 ==============
@@ -10,4 +10,6 @@ Appendix
    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 <https://en.wikipedia.org/wiki/Lidar>`_
+  - `Radar Sensors <https://en.wikipedia.org/wiki/Radar>`_
+  - `Stereo Cameras <https://en.wikipedia.org/wiki/Stereo_camera>`_
+  - `Ultrasonic Sensors <https://en.wikipedia.org/wiki/Ultrasonic_transducer>`_
\ 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) <https://en.wikipedia.org/wiki/Inertial_measurement_unit>`_
+  - `Accelerometer <https://en.wikipedia.org/wiki/Accelerometer>`_
+  - `Gyroscope <https://en.wikipedia.org/wiki/Gyroscope>`_
+  - `Magnetometer <https://en.wikipedia.org/wiki/Magnetometer>`_
+  - `Pressure sensor <https://en.wikipedia.org/wiki/Pressure_sensor>`_
+  - `Temperature sensor <https://en.wikipedia.org/wiki/Thermometer>`_
+- `Adafruit Sensor Guides <https://learn.adafruit.com/>`_
\ No newline at end of file
diff --git a/docs/modules/12_appendix/steering_motion_model_main.rst b/docs/modules/12_appendix/steering_motion_model_main.rst
index 6e444b7909..c697123fa2 100644
--- a/docs/modules/12_appendix/steering_motion_model_main.rst
+++ b/docs/modules/12_appendix/steering_motion_model_main.rst
@@ -91,7 +91,7 @@ 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}`
 
 
-References:
+Reference
 ~~~~~~~~~~~
 
 - `Vehicle Dynamics and Control <https://link.springer.com/book/10.1007/978-1-4614-1433-9>`_
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 <https://xml-tree-visual.vercel.app/>`_.
+
+.. image:: ./robot_behavior_case.svg
+
+Print the behavior tree
+
+.. code-block:: text
+
+    Behavior Tree
+    [Robot Main Controller]
+        [Battery Management]
+            (Low Battery Detection)
+                <Check Battery>
+            <Low Battery Warning>
+            <Charge Battery>
+        [Patrol Task]
+            <Start Task>
+            [Move to Position A]
+                <Move to A>
+                [Obstacle Handling A]
+                    [Obstacle Present]
+                        <Detect Obstacle>
+                        <Avoid Obstacle>
+                    <No Obstacle>
+                <Position A Task>
+            [Move to Position B]
+                (Short Wait)
+                    <Prepare Movement>
+                <Move to B>
+                (Limited Time Obstacle Handling)
+                    [Obstacle Present]
+                        <Detect Obstacle>
+                        <Avoid Obstacle>
+                <Position B Task>
+            [Conditional Move to C]
+                <Check Sufficient Battery>
+                [Perform Position C Task]
+                    <Move to C>
+                    (Ensure Completion)
+                        <Position C Task>
+                <Skip Position C>
+            <Complete Patrol>
+            <Return to Charging Station>
+    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 @@
+<svg width="2870" height="795" id="tree-svg" style="overflow: visible;" xmlns="http://www.w3.org/2000/svg" viewBox="-1560 -65 4380 810"><style xmlns="http://www.w3.org/1999/xhtml">
+      .node rect { 
+        fill: #fff; 
+        stroke-width: 1px; 
+      }
+      .node text { 
+        font: 12px sans-serif; 
+      }
+      .node-attributes { 
+        font: 10px sans-serif; 
+        fill: #666; 
+      }
+      .link { 
+        fill: none; 
+        stroke: #999; 
+        stroke-width: 1px; 
+      }
+      line {
+        stroke: #444;
+        stroke-width: 2px;
+      }
+    </style><g transform="translate(75,112) scale(0.8)"><path class="link" d="M0,0C0,60,-1125,60,-1125,120"/><path class="link" d="M0,0C0,60,1125,60,1125,120"/><path class="link" d="M-1125,120C-1125,180,-1395,180,-1395,240"/><path class="link" d="M-1125,120C-1125,180,-1125,180,-1125,240"/><path class="link" d="M-1125,120C-1125,180,-855,180,-855,240"/><path class="link" d="M1125,120C1125,180,-405,180,-405,240"/><path class="link" d="M1125,120C1125,180,-135,180,-135,240"/><path class="link" d="M1125,120C1125,180,990,180,990,240"/><path class="link" d="M1125,120C1125,180,2115,180,2115,240"/><path class="link" d="M1125,120C1125,180,2385,180,2385,240"/><path class="link" d="M1125,120C1125,180,2655,180,2655,240"/><path class="link" d="M-1395,240C-1395,300,-1395,300,-1395,360"/><path class="link" d="M-135,240C-135,300,-405,300,-405,360"/><path class="link" d="M-135,240C-135,300,-135,300,-135,360"/><path class="link" d="M-135,240C-135,300,135,300,135,360"/><path class="link" d="M990,240C990,300,585,300,585,360"/><path class="link" d="M990,240C990,300,855,300,855,360"/><path class="link" d="M990,240C990,300,1125,300,1125,360"/><path class="link" d="M990,240C990,300,1395,300,1395,360"/><path class="link" d="M2115,240C2115,300,1845,300,1845,360"/><path class="link" d="M2115,240C2115,300,2115,300,2115,360"/><path class="link" d="M2115,240C2115,300,2385,300,2385,360"/><path class="link" d="M-135,360C-135,420,-270,420,-270,480"/><path class="link" d="M-135,360C-135,420,0,420,0,480"/><path class="link" d="M585,360C585,420,585,420,585,480"/><path class="link" d="M1125,360C1125,420,1125,420,1125,480"/><path class="link" d="M2115,360C2115,420,1980,420,1980,480"/><path class="link" d="M2115,360C2115,420,2250,420,2250,480"/><path class="link" d="M-270,480C-270,540,-405,540,-405,600"/><path class="link" d="M-270,480C-270,540,-135,540,-135,600"/><path class="link" d="M1125,480C1125,540,990,540,990,600"/><path class="link" d="M1125,480C1125,540,1260,540,1260,600"/><path class="link" d="M2250,480C2250,540,2250,540,2250,600"/><g class="node" transform="translate(0,0)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Selector</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Robot Main Controller</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1125,120)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Battery Management</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1125,120)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Patrol Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1395,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">Inverter</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Low Battery Detection</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1125,240)"><rect x="-115" y="-15" width="230" height="90" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Low Battery Warning</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Battery level low!</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">Charging needed</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-855,240)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">ChargeBattery</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Charge Battery</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">charge_rate: 20</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-405,240)"><rect x="-115" y="-15" width="230" height="70" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Start Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Starting patrol task</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-135,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to Position A</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(990,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to Position B</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2115,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">WhileDoElse</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Conditional Move to C</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2385,240)"><rect x="-115" y="-15" width="230" height="110" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Complete Patrol</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Patrol task</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">completed, returning to</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="85" x="-105" text-anchor="start">charging station</text><line x1="-105" y1="90" x2="105" y2="90" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2655,240)"><rect x="-115" y="-15" width="230" height="110" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Return to Charging</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">Station</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">position: Charging Station</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="85" x="-105" text-anchor="start">move_duration: 4</text><line x1="-105" y1="90" x2="105" y2="90" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1395,360)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">CheckBattery</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Check Battery</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">threshold: 30</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-405,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to A</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">position: A</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">move_duration: 2</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-135,360)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Selector</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Obstacle Handling A</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(135,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">PerformTask</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Position A Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">task_name: Check Device Status</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">task_duration: 2</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(585,360)"><rect x="-115" y="-15" width="230" height="70" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">Delay</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Short Wait</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">sec: 1</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(855,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to B</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">position: B</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">move_duration: 3</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1125,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">Timeout</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Limited Time Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">Handling</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">sec: 2</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1395,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">PerformTask</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Position B Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">task_name: Data Collection</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">task_duration: 2.5</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1845,360)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">CheckBattery</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Check Sufficient Battery</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">threshold: 50</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2115,360)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Perform Position C Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2385,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Skip Position C</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Insufficient power,</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">skipping position C task</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-270,480)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Obstacle Present</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(0,480)"><rect x="-115" y="-15" width="230" height="70" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: No Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Path clear</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(585,480)"><rect x="-115" y="-15" width="230" height="90" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Prepare Movement</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Preparing to move to</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">next position</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1125,480)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Obstacle Present</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1980,480)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to C</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">position: C</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">move_duration: 2.5</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2250,480)"><rect x="-115" y="-15" width="230" height="50" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">ForceSuccess</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Ensure Completion</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-405,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">DetectObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Detect Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">obstacle_probability: 0.3</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-135,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">AvoidObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Avoid Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">avoid_duration: 1.5</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(990,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">DetectObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Detect Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">obstacle_probability: 0.4</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1260,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">AvoidObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Avoid Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">avoid_duration: 1.8</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2250,600)"><rect x="-115" y="-15" width="230" height="110" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">PerformTask</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Position C Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">task_name: Environment</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">Monitoring</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="85" x="-105" text-anchor="start">task_duration: 2</text><line x1="-105" y1="90" x2="105" y2="90" stroke="#ddd" stroke-width="1"/></g></g></svg>
\ 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
index 2f31834017..ca595301a6 100644
--- 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
@@ -1,7 +1,107 @@
 Definition of Robotics
 ----------------------
 
-In recent years, autonomous navigation technologies have received huge
-attention in many fields.
-Such fields include, autonomous driving[22], drone flight navigation,
-and other transportation systems.
+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
index 23f31da779..c47c122853 100644
--- 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
@@ -1,4 +1,135 @@
 Python for Robotics
 ----------------------
 
-TBD
+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 <https://www.python.org/>`_ is an general-purpose programming language developed by
+`Guido van Rossum <https://en.wikipedia.org/wiki/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 <https://numpy.org/>`_ is the fundamental package for scientific computing with Python.
+- `SciPy <https://www.scipy.org/>`_ 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 <https://matplotlib.org/>`_ is a plotting library for the Python programming language and its numerical mathematics extension NumPy.
+- `Pandas <https://pandas.pydata.org/>`_ is a fast, powerful, flexible, and easy-to-use open-source data analysis and data manipulation library built on top of NumPy.
+- `SymPy <https://www.sympy.org/>`_ is a Python library for symbolic mathematics.
+- `CVXPy <https://www.cvxpy.org/>`_ 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 <https://scikit-learn.org/stable/>`_ is a free software machine learning library for the Python programming language.
+- `Scikit-image <https://scikit-image.org/>`_ is a collection of algorithms for image processing.
+- `Networkx <https://networkx.org/>`_ is a package for the creation, manipulation for complex networks.
+- `SunPy <https://sunpy.org/>`_ is a community-developed free and open-source software package for solar physics.
+- `Astropy <https://www.astropy.org/>`_ 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/3_technology_for_robotics/technology_for_robotics_main.rst b/docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst
deleted file mode 100644
index 4dd1d1842f..0000000000
--- a/docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst
+++ /dev/null
@@ -1,12 +0,0 @@
-Technology for Robotics
--------------------------
-
-An autonomous navigation system is a system that can move to a goal over long
-periods of time without any external control by an operator.
-The system requires a wide range of technologies:
-- It needs to know where it is (localization)
-- Where it is safe (mapping)
-- 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.
diff --git a/docs/modules/1_introduction/introduction_main.rst b/docs/modules/1_introduction/introduction_main.rst
index a7ce55f9bf..1871dfc3b1 100644
--- a/docs/modules/1_introduction/introduction_main.rst
+++ b/docs/modules/1_introduction/introduction_main.rst
@@ -14,5 +14,5 @@ covered in PythonRobotics.
 
    1_definition_of_robotics/definition_of_robotics
    2_python_for_robotics/python_for_robotics
-   3_technology_for_robotics/technology_for_robotics
+   3_technologies_for_robotics/technologies_for_robotics
 
diff --git a/docs/modules/2_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
index 2543d1186a..214e645d10 100644
--- a/docs/modules/2_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/extended_kalman_filter_localization_main.rst b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
index da214b6de5..adb41e5e40 100644
--- a/docs/modules/2_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
@@ -23,28 +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 <https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter.py>`__
+Code Link
+~~~~~~~~~~~~~
 
-Kalman Filter with Speed Scale Factor Correction
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. autofunction:: Localization.extended_kalman_filter.extended_kalman_filter.ekf_estimation
 
+Extended Kalman Filter algorithm
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-.. image:: ekf_with_velocity_correction_1_0.png
-   :width: 600px
+Localization process using Extended Kalman Filter:EKF is
 
-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`
 
-Code: `PythonRobotics/extended_kalman_ekf_with_velocity_correctionfilter.py
-AtsushiSakai/PythonRobotics <https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_ekf_with_velocity_correctionfilter.py>`__
+:math:`P_{Pred} = J_f P_t J_f^T + Q`
 
-Filter design
-~~~~~~~~~~~~~
+=== Update ===
 
-Position Estimation Kalman Filter
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+: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`.
@@ -82,27 +94,9 @@ In the code, “observation” function generates the input and observation
 vector with noise
 `code <https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L34-L50>`__
 
-Kalman Filter with Speed Scale Factor Correction
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-In this simulation, the robot has a state vector includes 5 states at
-time :math:`t`.
-
-.. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t, s_t]
-
-x, y are a 2D x-y position, :math:`\phi` is orientation, v is
-velocity, and s is a scale factor of velocity.
-
-In the code, “xEst” means the state vector.
-`code <https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_ekf_with_velocity_correctionfilter.py#L163>`__
-
-The rest is the same as the Position Estimation Kalman Filter.
 
 Motion Model
-~~~~~~~~~~~~
-
-Position Estimation Kalman Filter
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+~~~~~~~~~~~~~~~~~
 
 The robot model is
 
@@ -137,9 +131,61 @@ Its Jacobian matrix is
 
 :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v \sin(\phi) \Delta t & \cos(\phi) \Delta t\\ 0 & 1 & v \cos(\phi) \Delta t & \sin(\phi) \Delta t\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \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 & 1 & 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 y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{ \partial v}\\ \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}`
+
 
 Kalman Filter with Speed Scale Factor Correction
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+This is a Extended kalman filter (EKF) localization with velocity correction.
+
+This is for correcting the vehicle speed measured with scale factor errors due to factors such as wheel wear.
+
+
+In this simulation, the robot has a state vector includes 5 states at
+time :math:`t`.
+
+.. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t, s_t]
+
+x, y are a 2D x-y position, :math:`\phi` is orientation, v is
+velocity, and s is a scale factor of velocity.
+
+In the code, “xEst” means the state vector.
+`code <https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_ekf_with_velocity_correctionfilter.py#L163>`__
+
+The rest is the same as the Position Estimation Kalman Filter.
+
+.. 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
 
@@ -178,33 +224,7 @@ Its Jacobian matrix is
 Observation Model
 ~~~~~~~~~~~~~~~~~
 
-Position Estimation Kalman Filter
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-The robot can get x-y position infomation 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 & 1 & 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 y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{ \partial v}\\ \end{bmatrix} \end{equation*}`
-
-:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}`
-
-Kalman Filter with Speed Scale Factor Correction
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-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
 
@@ -225,32 +245,8 @@ Its Jacobian matrix is
 :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ \end{bmatrix} \end{equation*}`
 
 
-Extended Kalman Filter
-~~~~~~~~~~~~~~~~~~~~~~
-
-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}`
 
-Ref:
-~~~~
+Reference
+^^^^^^^^^^^
 
 -  `PROBABILISTIC-ROBOTICS.ORG <http://www.probabilistic-robotics.org/>`__
diff --git a/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst
index fafd578333..3a175b1316 100644
--- a/docs/modules/2_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: <http://www.probabilistic-robotics.org>`_
diff --git a/docs/modules/2_localization/localization_main.rst b/docs/modules/2_localization/localization_main.rst
index 22cbd094da..770a234b69 100644
--- a/docs/modules/2_localization/localization_main.rst
+++ b/docs/modules/2_localization/localization_main.rst
@@ -1,4 +1,4 @@
-.. _localization:
+.. _`Localization`:
 
 Localization
 ============
diff --git a/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst b/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst
index 20a9eb58fc..d648d8e080 100644
--- a/docs/modules/2_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,7 +36,7 @@ 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: <http://www.probabilistic-robotics.org>`_
diff --git a/docs/modules/2_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
index bb6b5b664b..a7a5aab61b 100644
--- a/docs/modules/2_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 <https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization>`_
diff --git a/docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst b/docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst
index 1892d1f8f7..e243529a9c 100644
--- a/docs/modules/3_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 <https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf>`_ paper by Pedro F. Felzenszwalb and Daniel P. Huttenlocher. 
\ No newline at end of file
diff --git a/docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst b/docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst
index b0f112a871..50033d2a20 100644
--- a/docs/modules/3_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/3_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
index e098ca5409..0ece604009 100644
--- a/docs/modules/3_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/3_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
index 1f62179efd..29f5878e48 100644
--- a/docs/modules/3_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
index 28e18984d3..34a3744893 100644
--- a/docs/modules/3_mapping/mapping_main.rst
+++ b/docs/modules/3_mapping/mapping_main.rst
@@ -1,4 +1,4 @@
-.. _mapping:
+.. _`Mapping`:
 
 Mapping
 =======
@@ -17,3 +17,4 @@ Mapping is the ability of a robot to understand its surroundings with external s
    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/normal_vector_estimation/normal_vector_estimation_main.rst b/docs/modules/3_mapping/normal_vector_estimation/normal_vector_estimation_main.rst
index a4d1bf0df2..68a19e1534 100644
--- 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
@@ -25,8 +25,8 @@ This is an example of normal vector calculation:
 
 .. figure:: normal_vector_calc.png
 
-API
-=====
+Code Link
+==========
 
 .. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector
 
@@ -67,8 +67,8 @@ This is an example of RANSAC based normal vector estimation:
 
 .. figure:: ransac_normal_vector_estimation.png
 
-API
-=====
+Code Link
+==========
 
 .. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation
 
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
index cbb5652f56..8cb08d4b78 100644
--- 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
@@ -27,8 +27,8 @@ 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.
 
-API
-=====
+Code Link
+==========
 
 .. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.voxel_point_sampling
 
@@ -61,8 +61,8 @@ 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.
 
-API
-=====
+Code Link
+==========
 
 .. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.poisson_disk_sampling
 
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
index cc5a1a1c5b..bee2f64193 100644
--- 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
@@ -3,4 +3,9 @@ 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
+.. 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
index b6ced1dc1d..06d85efe61 100644
--- a/docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst
+++ b/docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst
@@ -57,8 +57,8 @@ This evaluation function uses the squreed distances between the edges of the rec
 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.
 
-API
-~~~~~~
+Code Link
+~~~~~~~~~~~
 
 .. autoclass:: Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting
 	:members:
diff --git a/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
index a2aa521216..b6bafa0982 100644
--- a/docs/modules/4_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
 ~~~~~~~~~~~~
diff --git a/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst b/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
index 9e79b496a3..59ed3b9f75 100644
--- a/docs/modules/4_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/4_slam/ekf_slam/ekf_slam_main.rst b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
index b27971225e..3967a7ae4d 100644
--- a/docs/modules/4_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
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -578,7 +584,7 @@ reckoning and control functions are passed along here as well.
 
 .. image:: ekf_slam_1_0.png
 
-References:
+Reference
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 - `PROBABILISTIC ROBOTICS <http://www.probabilistic-robotics.org/>`_
diff --git a/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst
index 491320512b..15963aff79 100644
--- a/docs/modules/4_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/4_slam/graph_slam/graph_slam_main.rst b/docs/modules/4_slam/graph_slam/graph_slam_main.rst
index 987eed385c..2ef17e4179 100644
--- a/docs/modules/4_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 <http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf>`_
diff --git a/docs/modules/4_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
index a30b1fc99b..772fe62889 100644
--- a/docs/modules/4_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 <https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf>`_
diff --git a/docs/modules/4_slam/slam_main.rst b/docs/modules/4_slam/slam_main.rst
index dec04f253a..98211986c2 100644
--- a/docs/modules/4_slam/slam_main.rst
+++ b/docs/modules/4_slam/slam_main.rst
@@ -1,4 +1,4 @@
-.. _slam:
+.. _`SLAM`:
 
 SLAM
 ====
diff --git a/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
index d306a85352..19fb89a1b1 100644
--- a/docs/modules/5_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 <https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf&doi=b00b657c3e0e828c589132a14825e7119772003d>`
+   Vehicles <https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf&doi=b00b657c3e0e828c589132a14825e7119772003d>`__
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
index e734352e34..00e5ef2fdb 100644
--- a/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst
+++ b/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst
@@ -105,8 +105,8 @@ The default spline degree is 3, so curvature changes smoothly.
 
 .. image:: interp_and_curvature.png
 
-API
-++++
+Code link
+++++++++++
 
 .. autofunction:: PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path
 
@@ -133,8 +133,8 @@ The default spline degree is 3, so curvature changes smoothly.
 
 .. image:: approx_and_curvature.png
 
-API
-++++
+Code Link
+++++++++++
 
 .. autofunction:: PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path
 
diff --git a/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst
index 81c91c0313..e1cd2fe353 100644
--- a/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst
+++ b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst
@@ -5,4 +5,12 @@ 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 <https://web.archive.org/web/20201103052224/https://sites.google.com/site/ece452bugalgorithms/>`_
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
index 72e558c486..fa2a2ff72b 100644
--- 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
@@ -88,8 +88,8 @@ Catmull-Rom Spline API
 
 This section provides an overview of the functions used for Catmull-Rom spline path planning.
 
-API
-++++
+Code Link
+++++++++++
 
 .. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_point
 
diff --git a/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst b/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst
index 1e8cee694a..16d0ec03c1 100644
--- a/docs/modules/5_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/5_path_planning/coverage_path/coverage_path_main.rst b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst
index 0a688b5ed2..eaa876c80b 100644
--- a/docs/modules/5_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 <https://ieeexplore.ieee.org/abstract/document/1013479>`_
 
 
@@ -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
 
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.WavefrontCPP.wavefront_coverage_path_planner.wavefront
+
+Reference
++++++++++++++
+
 - `Planning paths of complete coverage of an unstructured environment by a mobile robot <https://pinkwink.kr/attachment/cfile3.uf@1354654A4E8945BD13FE77.pdf>`_
 
 
diff --git a/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst b/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst
index 33471f7c85..a110217a2e 100644
--- a/docs/modules/5_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/5_path_planning/dubins_path/dubins_path_main.rst b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst
index 12172fb51e..5a3d14464b 100644
--- a/docs/modules/5_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
diff --git a/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst b/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
index d645838597..ac5322df94 100644
--- a/docs/modules/5_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 <https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf>`__
 
-.. 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 <http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf>`__
diff --git a/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst
index ffc3cc6451..9ee343e8a7 100644
--- a/docs/modules/5_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 <https://ieeexplore.ieee.org/document/4339545/>`__
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
index 38efaf2b53..0f84d381ea 100644
--- 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
@@ -7,6 +7,12 @@ 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
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -35,7 +41,7 @@ Low Speed and Merging and Stopping Scenario
 
 This scenario illustrates the trajectory planning at low speeds with merging and stopping behaviors.
 
-Ref:
+Reference
 
 -  `Optimal Trajectory Generation for Dynamic Street Scenarios in a
    Frenet
diff --git a/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst
index 1644ed00cc..c4aa6882aa 100644
--- a/docs/modules/5_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 <https://en.wikipedia.org/wiki/D*>`__
 
@@ -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 <http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf>`_
 
@@ -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 <https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf>`__
diff --git a/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst b/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst
index a9876fa3d4..36f340e0c2 100644
--- a/docs/modules/5_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/5_path_planning/lqr_path/lqr_path_main.rst b/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst
index 788442ff63..1eb1e4d840 100644
--- a/docs/modules/5_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/5_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
index 463363ddcf..76472a6792 100644
--- a/docs/modules/5_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 <https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328>`__
diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst
index 4960330b3e..0c84a19c22 100644
--- a/docs/modules/5_path_planning/path_planning_main.rst
+++ b/docs/modules/5_path_planning/path_planning_main.rst
@@ -1,4 +1,4 @@
-.. _path_planning:
+.. _`Path Planning`:
 
 Path Planning
 =============
@@ -12,6 +12,7 @@ Path planning is the ability of a robot to search feasible and efficient path to
    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
@@ -31,3 +32,4 @@ Path planning is the ability of a robot to search feasible and efficient path to
    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/5_path_planning/prm_planner/prm_planner_main.rst b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
index 0628719176..d58d1e2633 100644
--- a/docs/modules/5_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 <https://en.wikipedia.org/wiki/Probabilistic_roadmap>`__
diff --git a/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
index 0412b3c9b3..c7bc3fb55c 100644
--- a/docs/modules/5_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,7 +102,7 @@ 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
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
index ff377eb91b..4dd54d7c97 100644
--- 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
@@ -5,6 +5,12 @@ 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.
@@ -380,7 +386,8 @@ Hence, we have:
 - :math:`v = (t - φ)`
 
 
-Ref:
+Reference
+=============
 
 -  `15.3.2 Reeds-Shepp
    Curves <https://lavalle.pl/planning/node822.html>`__
diff --git a/docs/modules/5_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/5_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/5_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/5_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/5_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/5_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/5_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/5_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/5_path_planning/rrt/rrt_main.rst b/docs/modules/5_path_planning/rrt/rrt_main.rst
index e5f351a7ba..1bd5497f4c 100644
--- a/docs/modules/5_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,7 +77,14 @@ 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
@@ -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 <https://arxiv.org/abs/1405.5848>`__
 
+
 .. _closed-loop-rrt*:
 
 Closed Loop RRT\*
@@ -87,7 +126,14 @@ 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 <https://acl.mit.edu/papers/KuwataGNC08.pdf>`__
@@ -98,6 +144,7 @@ Ref:
 -  `[1601.06326] Sampling-based Algorithms for Optimal Motion Planning
    Using Closed-loop Prediction <https://arxiv.org/abs/1601.06326>`__
 
+
 .. _lqr-rrt*:
 
 LQR-RRT\*
@@ -109,7 +156,14 @@ 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
diff --git a/docs/modules/5_path_planning/rrt/rrt_star.rst b/docs/modules/5_path_planning/rrt/rrt_star.rst
index 6deb6b9172..960b9976d5 100644
--- a/docs/modules/5_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
 ^^^^^^^^^^
 
diff --git a/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst
index d5e7ed17d1..9f8cc0c50f 100644
--- a/docs/modules/5_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,17 +12,34 @@ 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 <https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328>`__
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 <https://github.com/AtsushiSakai/PythonRobotics/pull/1183>`__ 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 <https://www.cs.cmu.edu/~maxim/files/sipp_icra11.pdf>`__
+
+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 <https://www.davidsilver.uk/wp-content/uploads/2020/03/coop-path-AIWisdom.pdf>`__
+-  `SIPP: Safe Interval Path Planning for Dynamic Environments <https://www.cs.cmu.edu/~maxim/files/sipp_icra11.pdf>`__
+- `Prioritized Planning Algorithms for Trajectory Coordination of Multiple Mobile Robots <https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf>`__
\ No newline at end of file
diff --git a/docs/modules/5_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
index 3c9b7c008c..aac96d6e19 100644
--- a/docs/modules/5_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 <https://en.wikipedia.org/wiki/Visibility_graph>`_
 
diff --git a/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
index 92e729ab29..a9a41aab74 100644
--- a/docs/modules/5_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 <https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf>`__
 
diff --git a/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
index 23f1218a94..914a4555ff 100644
--- a/docs/modules/6_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
 ~~~~~~~~~~~~~~~~~~~~~~~~
 
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
index ded187e972..b0ba9e0d45 100644
--- 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
@@ -7,7 +7,12 @@ 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] <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py>`_
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.lqr_speed_steer_control.lqr_speed_steer_control.lqr_speed_steering_control
+
 
 Overview
 ~~~~~~~~
@@ -134,7 +139,7 @@ Simulation results
 
 
 
-References:
+Reference
 ~~~~~~~~~~~
 
 -  `Towards fully autonomous driving: Systems and algorithms <https://ieeexplore.ieee.org/document/5940562/>`__
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
index baca7a33fc..fc8f2a33aa 100644
--- 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
@@ -8,7 +8,11 @@ control.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif
 
-`[Code Link] <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/lqr_steer_control/lqr_steer_control.py>`_
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.lqr_steer_control.lqr_steer_control.lqr_steering_control
+
 
 Overview
 ~~~~~~~~
@@ -113,7 +117,7 @@ The optimal control input `u` can be calculated as:
 
 where `K` is the feedback gain matrix obtained by solving the Riccati equation.
 
-References:
+Reference
 ~~~~~~~~~~~
 -  `ApolloAuto/apollo: An open autonomous driving platform <https://github.com/ApolloAuto/apollo>`_
 
diff --git a/docs/modules/6_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
index de55b545ba..76a6819a46 100644
--- a/docs/modules/6_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 <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py>`__
-
 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 <http://www.cvxpy.org/>`__
 
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.model_predictive_speed_and_steer_control.model_predictive_speed_and_steer_control.iterative_linear_mpc_control
+
+
 MPC modeling
 ~~~~~~~~~~~~
 
diff --git a/docs/modules/10_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/10_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/10_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
index d7a895b562..742cc807e6 100644
--- a/docs/modules/6_path_tracking/path_tracking_main.rst
+++ b/docs/modules/6_path_tracking/path_tracking_main.rst
@@ -1,4 +1,4 @@
-.. _path_tracking:
+.. _`Path Tracking`:
 
 Path Tracking
 =============
@@ -16,3 +16,4 @@ Path tracking is the ability of a robot to follow the reference path generated b
    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/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
index 5c7bcef85f..ff6749bbbe 100644
--- a/docs/modules/6_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/6_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
index 70875fdc6c..56d0db63ad 100644
--- a/docs/modules/6_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 <https://arxiv.org/abs/1604.07446>`__
diff --git a/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst b/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst
index fe325b0102..3c491804f6 100644
--- a/docs/modules/6_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/7_arm_navigation/arm_navigation_main.rst b/docs/modules/7_arm_navigation/arm_navigation_main.rst
index bbbc872c58..7acd3ee7d3 100644
--- a/docs/modules/7_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/7_arm_navigation/n_joint_arm_to_point_control_main.rst b/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst
index cc6279f681..56900acde1 100644
--- a/docs/modules/7_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/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst b/docs/modules/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst
index 899a64a5cd..4433ab531b 100644
--- a/docs/modules/7_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/7_arm_navigation/planar_two_link_ik_main.rst b/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst
index 2da2b0dc30..5b2712eb48 100644
--- a/docs/modules/7_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/8_aerial_navigation/aerial_navigation_main.rst b/docs/modules/8_aerial_navigation/aerial_navigation_main.rst
index b2ccb071af..7f76689770 100644
--- a/docs/modules/8_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/8_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
index c3bdc33cdc..1805bb3f6d 100644
--- a/docs/modules/8_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/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst b/docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
index 6c90d2b44e..4bf5117500 100644
--- a/docs/modules/8_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/9_bipedal/bipedal_main.rst b/docs/modules/9_bipedal/bipedal_main.rst
index fc5b933055..dc387dc4e8 100644
--- a/docs/modules/9_bipedal/bipedal_main.rst
+++ b/docs/modules/9_bipedal/bipedal_main.rst
@@ -1,4 +1,4 @@
-.. _bipedal:
+.. _`Bipedal`:
 
 Bipedal
 =================
diff --git a/docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst b/docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst
index 2ee5971e7a..6253715cc1 100644
--- a/docs/modules/9_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/requirements/environment.yml b/requirements/environment.yml
index afbb3fb8ce..023a3d75bf 100644
--- a/requirements/environment.yml
+++ b/requirements/environment.yml
@@ -2,7 +2,7 @@ name: python_robotics
 channels:
   - conda-forge
 dependencies:
-  - python=3.12
+  - python=3.13
   - pip
   - scipy
   - numpy
diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 9d4e7deb4d..1a7b43c438 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,8 +1,9 @@
-numpy == 2.2.2
-scipy == 1.15.1
-matplotlib == 3.10.0
-cvxpy == 1.5.3
-pytest == 8.3.4 # For unit test
-pytest-xdist == 3.6.1 # For unit test
-mypy == 1.14.1 # For unit test
-ruff == 0.9.4 # For unit test
+numpy == 2.2.4
+scipy == 1.15.2
+matplotlib == 3.10.1
+cvxpy == 1.7.1
+ecos == 2.0.14
+pytest == 8.4.1 # For unit test
+pytest-xdist == 3.8.0 # For unit test
+mypy == 1.17.1 # For unit test
+ruff == 0.12.8 # For unit test
diff --git a/ruff.toml b/ruff.toml
index 5823ca3db7..d76b715a06 100644
--- a/ruff.toml
+++ b/ruff.toml
@@ -5,8 +5,8 @@ ignore = ["E501", "E741", "E402"]
 exclude = [
 ]
 
-# Assume Python 3.11
-target-version = "py312"
+# Assume Python 3.13
+target-version = "py313"
 
 [per-file-ignores]
 
diff --git a/runtests.sh b/runtests.sh
index 12d1b80453..c4460c8aa7 100755
--- a/runtests.sh
+++ b/runtests.sh
@@ -1,4 +1,5 @@
 #!/usr/bin/env bash
+cd "$(dirname "$0")" || exit 1
 echo "Run test suites! "
 
 # === pytest based test runner ===
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_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 = """
+        <Sequence>
+            <Echo name="Echo 1" message="Hello, World1!" />
+            <Echo name="Echo 2" message="Hello, World2!" />
+            <ForceFailure name="Force Failure">
+                <Echo name="Echo 3" message="Hello, World3!" />
+            </ForceFailure>
+        </Sequence>
+    """
+    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 = """
+        <Sequence>
+            <Echo name="Echo 1" message="Hello, World1!" />
+            <Echo name="Echo 2" message="Hello, World2!" />
+            <ForceSuccess name="Force Success">
+                <Echo name="Echo 3" message="Hello, World3!" />
+            </ForceSuccess>
+        </Sequence>
+    """
+    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 = """
+        <Selector>
+            <ForceFailure name="Force Failure">
+                <Echo name="Echo 1" message="Hello, World1!" />
+            </ForceFailure>
+            <Echo name="Echo 2" message="Hello, World2!" />
+            <Echo name="Echo 3" message="Hello, World3!" />
+        </Selector>
+    """
+    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 = """
+        <Selector>
+            <ForceFailure name="Force Success">
+                <Echo name="Echo 1" message="Hello, World1!" />
+            </ForceFailure>
+            <ForceFailure name="Force Failure">
+                <Echo name="Echo 2" message="Hello, World2!" />
+            </ForceFailure>
+        </Selector>
+    """
+    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 = """
+        <WhileDoElse>
+            <Count name="Count" count_threshold="3" />
+            <Echo name="Echo 1" message="Hello, World1!" />
+            <Echo name="Echo 2" message="Hello, World2!" />
+        </WhileDoElse>
+    """
+
+    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 = """
+        <Sequence>
+        </Sequence>
+    """
+    bt_factory = BehaviorTreeFactory()
+    with pytest.raises(ValueError):
+        bt_factory.build_tree(xml_string)
+
+    # DecoratorNode Must have child
+    xml_string = """
+        <Inverter>
+        </Inverter>
+    """
+    with pytest.raises(ValueError):
+        bt_factory.build_tree(xml_string)
+
+    # DecoratorNode Must have only one child
+    xml_string = """
+        <Inverter>
+            <Echo name="Echo 1" message="Hello, World1!" />
+            <Echo name="Echo 2" message="Hello, World2!" />
+        </Inverter>
+    """
+    with pytest.raises(ValueError):
+        bt_factory.build_tree(xml_string)
+
+    # ActionNode Must have no children
+    xml_string = """
+        <Echo name="Echo 1" message="Hello, World1!">
+            <Echo name="Echo 2" message="Hello, World2!" />
+        </Echo>
+    """
+    with pytest.raises(ValueError):
+        bt_factory.build_tree(xml_string)
+
+    # WhileDoElse Must have exactly 2 or 3 children
+    xml_string = """
+        <WhileDoElse>
+            <Echo name="Echo 1" message="Hello, World1!" />
+        </WhileDoElse>
+    """
+    with pytest.raises(ValueError):
+        bt = bt_factory.build_tree(xml_string)
+        bt.tick()
+
+    xml_string = """
+        <WhileDoElse>
+            <Echo name="Echo 1" message="Hello, World1!" />
+            <Echo name="Echo 2" message="Hello, World2!" />
+            <Echo name="Echo 3" message="Hello, World3!" />
+            <Echo name="Echo 4" message="Hello, World4!" />
+        </WhileDoElse>
+    """
+    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_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_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_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_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 a2f798eac4..30dc2d2d85 100644
--- a/users_comments.md
+++ b/users_comments.md
@@ -6,11 +6,11 @@ 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)).
 
 ![1](https://github.com/AtsushiSakai/PythonRoboticsGifs/blob/master/Users/WHILL_Model_CR_with_move_to_a_pose/WHLL_Model_CR_with_move_to_a_pose.gif)
 
-Ref:
+Reference:
 
 - [toioと同じように動くWHILL Model CR (in Japanese)](https://qiita.com/KatsuShun89/items/68fde7544ecfe36096d2)
 
