From 7b7bd784093e4d46108b667c54261d054894e875 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Wed, 5 Feb 2025 13:41:28 +0900
Subject: [PATCH 01/84] update introduction (#1141)

---
 .../python_for_robotics_main.rst              | 46 +++++++++++++++++++
 1 file changed, 46 insertions(+)

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..1ad5316f53 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,50 @@
 Python for 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>`_ in 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 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 scientific computing libraries.
+
+To address the more needs of scientific computing, many libraries have been developed.
+
+- `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.
+
+And more domain-specific libraries have been developed:
+- `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.
+
+Python for Robotics
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
 TBD
+

From 322fead45a62417c5d0d7dbb07ef17067122da7b Mon Sep 17 00:00:00 2001
From: Aglargil <34728006+Aglargil@users.noreply.github.com>
Date: Wed, 5 Feb 2025 20:56:13 +0800
Subject: [PATCH 02/84] feat: add DistanceMap (#1142)

* feat: add DistanceMap

* feat: add DistanceMap test

* feat: add DistanceMap doc

* feat: DistanceMap doc update

* feat: DistanceMap update
---
 Mapping/DistanceMap/distance_map.py           | 151 ++++++++++++++++++
 .../3_mapping/distance_map/distance_map.png   | Bin 0 -> 32698 bytes
 .../distance_map/distance_map_main.rst        |  27 ++++
 docs/modules/3_mapping/mapping_main.rst       |   1 +
 tests/test_distance_map.py                    | 118 ++++++++++++++
 5 files changed, 297 insertions(+)
 create mode 100644 Mapping/DistanceMap/distance_map.py
 create mode 100644 docs/modules/3_mapping/distance_map/distance_map.png
 create mode 100644 docs/modules/3_mapping/distance_map/distance_map_main.rst
 create mode 100644 tests/test_distance_map.py

diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py
new file mode 100644
index 0000000000..54c98c6a75
--- /dev/null
+++ b/Mapping/DistanceMap/distance_map.py
@@ -0,0 +1,151 @@
+"""
+Distance Map
+
+author: Wang Zheng (@Aglargil)
+
+Ref:
+
+- [Distance Map]
+(https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf)
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+INF = 1e20
+ENABLE_PLOT = True
+
+
+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/docs/modules/3_mapping/distance_map/distance_map.png b/docs/modules/3_mapping/distance_map/distance_map.png
new file mode 100644
index 0000000000000000000000000000000000000000..2d89252a70358ad1b60025d7bb3d9a217a2cefcc
GIT binary patch
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..45273cd3bb
--- /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
+
+API
+~~~
+
+.. autofunction:: PathPlanning.DistanceMap.distance_map.compute_sdf
+
+.. autofunction:: PathPlanning.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/mapping_main.rst b/docs/modules/3_mapping/mapping_main.rst
index 28e18984d3..825b08d3ec 100644
--- a/docs/modules/3_mapping/mapping_main.rst
+++ b/docs/modules/3_mapping/mapping_main.rst
@@ -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/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__)

From 2234abf63d07e5496e37c1b57a1b927c303272a8 Mon Sep 17 00:00:00 2001
From: Aglargil <34728006+Aglargil@users.noreply.github.com>
Date: Thu, 6 Feb 2025 12:05:20 +0800
Subject: [PATCH 03/84] fix: DistanceMap doc autofunction (#1143)

---
 docs/modules/3_mapping/distance_map/distance_map_main.rst | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/modules/3_mapping/distance_map/distance_map_main.rst b/docs/modules/3_mapping/distance_map/distance_map_main.rst
index 45273cd3bb..0ef9e3022f 100644
--- a/docs/modules/3_mapping/distance_map/distance_map_main.rst
+++ b/docs/modules/3_mapping/distance_map/distance_map_main.rst
@@ -17,9 +17,9 @@ The algorithm is demonstrated on a simple 2D grid with obstacles:
 API
 ~~~
 
-.. autofunction:: PathPlanning.DistanceMap.distance_map.compute_sdf
+.. autofunction:: Mapping.DistanceMap.distance_map.compute_sdf
 
-.. autofunction:: PathPlanning.DistanceMap.distance_map.compute_udf
+.. autofunction:: Mapping.DistanceMap.distance_map.compute_udf
 
 References
 ~~~~~~~~~~

From 0676dfd67e099198c34c485e176b077ad6fa7374 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 6 Feb 2025 15:16:17 +0900
Subject: [PATCH 04/84] update introduction (#1144)

---
 .../python_for_robotics_main.rst              | 57 +++++++++++++++++--
 1 file changed, 51 insertions(+), 6 deletions(-)

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 1ad5316f53..2f89f0c7b5 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,11 +1,13 @@
 Python for Robotics
 ----------------------
 
+This section explains the Python itself and features for 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>`_ in the late 1980s.
+`Guido van Rossum <https://en.wikipedia.org/wiki/Guido_van_Rossum>`_ from the late 1980s.
 
 It features as follows:
 
@@ -17,7 +19,7 @@ It features as follows:
 #. Batteries included
 #. Interoperability for C and Fortran
 
-Due to these features, Python is the most popular programming language
+Due to these features, Python is one of the most popular programming language
 for educational purposes for programming beginners.
 
 Python for Scientific Computing
@@ -29,9 +31,9 @@ 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 scientific computing libraries.
+#. 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 libraries have been developed.
+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.
@@ -39,12 +41,55 @@ To address the more needs of scientific computing, many libraries have been deve
 - `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.
 
-And more domain-specific libraries have been developed:
+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 Python package for the creation, manipulation, and study of the structure, dynamics, and functions of 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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-TBD
+Scientific computation routine are very important for robotics.
+For example, matrix operation, optimization, and visualization are fundamental for robotics.
+
+Python has become an increasingly popular language in robotics due to its versatility, readability, and extensive libraries. Here's a breakdown of why Python is a great choice for robotics development:
+
+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: Python boasts a rich collection of libraries specifically designed for robotics:
+ROS (Robot Operating System): ROS, a widely used framework for robotics development, has strong Python support (rospy). This allows developers to easily create nodes, manage communication between different parts of a robot system, and utilize various ROS tools.
+OpenCV: This powerful library provides tools for computer vision tasks like image processing, object detection, and motion tracking, essential for robots that perceive and interact with their environment.
+NumPy and SciPy: These libraries offer efficient numerical computation and scientific tools, enabling developers to implement complex mathematical models and control algorithms.
+Scikit-learn: This library provides machine learning algorithms, which are increasingly important in robotics for tasks like perception, planning, and control.
+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.
+Use Cases of Python in Robotics:
+
+Robot Control: Python can be used to write control algorithms for robot manipulators, mobile robots, and other robotic systems.
+Perception: Python, combined with libraries like OpenCV, enables robots to process sensor data (camera images, lidar data) to understand their surroundings.
+Path Planning: Python algorithms can be used to plan collision-free paths for robots to navigate in complex environments.
+Machine Learning: Python libraries like Scikit-learn empower robots to learn from data and improve their performance in tasks like object recognition and manipulation.
+Simulation: Python can be used to create simulated environments for testing and developing robot algorithms before deploying them on real hardware.
+Examples of Python in Robotics:
+
+Autonomous Navigation: Python is used in self-driving cars and other autonomous vehicles for tasks like perception, localization, and path planning.
+Industrial Robotics: Python is employed in manufacturing for robot control, quality inspection, and automation.
+Service Robotics: Python powers robots that perform tasks like cleaning, delivery, and customer service in various environments.
+Research and Education: Python is a popular choice in robotics research and education due to its ease of use and versatility.
+Getting Started with Python in Robotics:
+
+Learn Python Basics: Familiarize yourself with Python syntax, data structures, and programming concepts.
+Explore Robotics Libraries: Dive into libraries like ROS, OpenCV, and others relevant to your robotics interests.
+Practice with Projects: Start with small projects to gain hands-on experience, gradually increasing complexity.
+Join the Community: Engage with the robotics community through online forums, workshops, and conferences to learn from others and share your knowledge.
+In conclusion, Python's simplicity, extensive libraries, and strong community support make it an ideal language for robotics development. Whether you're a beginner or an experienced programmer, Python offers the tools and resources you need to build innovative and capable robots.
+
+Python is used for this `PythonRobotics` project because of the above features
+to achieve the purpose of this project described in the :ref:`What is PythonRobotics?`.
 

From 9936f344635e146b9a2ee402ab1672b4e7216d6e Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 7 Feb 2025 13:32:11 +0900
Subject: [PATCH 05/84] update introduction (#1145)

---
 .../definition_of_robotics_main.rst           |  7 +++
 .../python_for_robotics_main.rst              | 63 +++++++++----------
 2 files changed, 37 insertions(+), 33 deletions(-)

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..fd151e3f20 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
@@ -5,3 +5,10 @@ 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.
+
+Examples of Python in Robotics:
+
+Autonomous Navigation: Python is used in self-driving cars and other autonomous vehicles for tasks like perception, localization, and path planning.
+Industrial Robotics: Python is employed in manufacturing for robot control, quality inspection, and automation.
+Service Robotics: Python powers robots that perform tasks like cleaning, delivery, and customer service in various environments.
+Research and Education: Python is a popular choice in robotics research and education due to its ease of use and versatility.
\ No newline at end of file
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 2f89f0c7b5..90edd5dc0c 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
@@ -40,55 +40,52 @@ To address the more needs of scientific computing, many fundamental scientific c
 - `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 Python package for the creation, manipulation, and study of the structure, dynamics, and functions of complex networks.
+- `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
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^
 
-Scientific computation routine are very important for robotics.
-For example, matrix operation, optimization, and visualization are fundamental for robotics.
+Python has become an increasingly popular language in robotics.
 
-Python has become an increasingly popular language in robotics due to its versatility, readability, and extensive libraries. Here's a breakdown of why Python is a great choice for robotics development:
+These are advantages of Python for Robotics:
 
-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.
 
-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: Python boasts a rich collection of libraries specifically designed for robotics:
+
+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): ROS, a widely used framework for robotics development, has strong Python support (rospy). This allows developers to easily create nodes, manage communication between different parts of a robot system, and utilize various ROS tools.
-OpenCV: This powerful library provides tools for computer vision tasks like image processing, object detection, and motion tracking, essential for robots that perceive and interact with their environment.
-NumPy and SciPy: These libraries offer efficient numerical computation and scientific tools, enabling developers to implement complex mathematical models and control algorithms.
-Scikit-learn: This library provides machine learning algorithms, which are increasingly important in robotics for tasks like perception, planning, and control.
-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.
-Use Cases of Python in Robotics:
-
-Robot Control: Python can be used to write control algorithms for robot manipulators, mobile robots, and other robotic systems.
-Perception: Python, combined with libraries like OpenCV, enables robots to process sensor data (camera images, lidar data) to understand their surroundings.
-Path Planning: Python algorithms can be used to plan collision-free paths for robots to navigate in complex environments.
-Machine Learning: Python libraries like Scikit-learn empower robots to learn from data and improve their performance in tasks like object recognition and manipulation.
-Simulation: Python can be used to create simulated environments for testing and developing robot algorithms before deploying them on real hardware.
-Examples of Python in Robotics:
-
-Autonomous Navigation: Python is used in self-driving cars and other autonomous vehicles for tasks like perception, localization, and path planning.
-Industrial Robotics: Python is employed in manufacturing for robot control, quality inspection, and automation.
-Service Robotics: Python powers robots that perform tasks like cleaning, delivery, and customer service in various environments.
-Research and Education: Python is a popular choice in robotics research and education due to its ease of use and versatility.
-Getting Started with Python in Robotics:
-
-Learn Python Basics: Familiarize yourself with Python syntax, data structures, and programming concepts.
-Explore Robotics Libraries: Dive into libraries like ROS, OpenCV, and others relevant to your robotics interests.
-Practice with Projects: Start with small projects to gain hands-on experience, gradually increasing complexity.
-Join the Community: Engage with the robotics community through online forums, workshops, and conferences to learn from others and share your knowledge.
-In conclusion, Python's simplicity, extensive libraries, and strong community support make it an ideal language for robotics development. Whether you're a beginner or an experienced programmer, Python offers the tools and resources you need to build innovative and capable robots.
+
+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.
+
 
 Python is used for this `PythonRobotics` project because of the above features
 to achieve the purpose of this project described in the :ref:`What is PythonRobotics?`.

From 15e106839285033e1fa68cbed6f14e219f3af88f Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 7 Feb 2025 13:42:56 +0900
Subject: [PATCH 06/84] Update CONTRIBUTING.md

---
 CONTRIBUTING.md | 24 +++---------------------
 1 file changed, 3 insertions(+), 21 deletions(-)

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) 

From a8f3388bbe80e41655acedc44c2cbb86d011fb48 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 9 Feb 2025 21:00:02 +0900
Subject: [PATCH 07/84] update introduction (#1146)

---
 .../3_how_to_contribute_main.rst              | 37 ++++++++++++++-----
 .../python_for_robotics_main.rst              | 29 ++++++++++++---
 2 files changed, 51 insertions(+), 15 deletions(-)

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..874564cbb8 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.12.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.
@@ -112,8 +132,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 +156,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 +170,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,8 +185,7 @@ 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:
 
 #. `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.
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 90edd5dc0c..65b1705150 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,6 +1,8 @@
 Python for Robotics
 ----------------------
 
+Python is used for this `PythonRobotics` project because of the above features
+to achieve the purpose of this project described in the :ref:`What is PythonRobotics?`.
 This section explains the Python itself and features for Robotics.
 
 Python for general-purpose programming
@@ -76,7 +78,27 @@ For example:
 
 ROS supports Python
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~
-ROS (Robot Operating System): ROS, a widely used framework for robotics development, has strong Python support (rospy). This allows developers to easily create nodes, manage communication between different parts of a robot system, and utilize various ROS tools.
+`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
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -85,8 +107,3 @@ Python code can run on various operating systems (Windows, macOS, Linux), provid
 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.
-
-
-Python is used for this `PythonRobotics` project because of the above features
-to achieve the purpose of this project described in the :ref:`What is PythonRobotics?`.
-

From e304f07a997bf40977a004ac2a66f476aa2aa861 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Mon, 10 Feb 2025 11:21:19 +0900
Subject: [PATCH 08/84] update introduction (#1147)

---
 .../python_for_robotics_main.rst              | 32 +++++++++++++++++--
 1 file changed, 29 insertions(+), 3 deletions(-)

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 65b1705150..b677d2c59b 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,9 +1,10 @@
 Python for Robotics
 ----------------------
 
-Python is used for this `PythonRobotics` project because of the above features
-to achieve the purpose of this project described in the :ref:`What is PythonRobotics?`.
-This section explains the Python itself and features for Robotics.
+A programing language, Python is used for this `PythonRobotics` project
+to achieve the purposes of this project described in the :ref:`What is PythonRobotics?`.
+
+This section explains the Python itself and features for science computing Robotics.
 
 Python for general-purpose programming
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -107,3 +108,28 @@ Python code can run on various operating systems (Windows, macOS, Linux), provid
 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.

From 610f35ff58e6535efa619f9ed73a113c6ca2c7f7 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 11 Feb 2025 10:42:10 +0900
Subject: [PATCH 09/84] build(deps): bump mypy from 1.14.1 to 1.15.0 in
 /requirements (#1148)

Bumps [mypy](https://github.com/python/mypy) from 1.14.1 to 1.15.0.
- [Changelog](https://github.com/python/mypy/blob/master/CHANGELOG.md)
- [Commits](https://github.com/python/mypy/compare/v1.14.1...v1.15.0)

---
updated-dependencies:
- dependency-name: mypy
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 9d4e7deb4d..178046ac0d 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -4,5 +4,5 @@ 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
+mypy == 1.15.0 # For unit test
 ruff == 0.9.4 # For unit test

From ba307673013376204ceb5a6def16da0e3a86a15d Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 11 Feb 2025 11:35:28 +0900
Subject: [PATCH 10/84] build(deps): bump ruff from 0.9.4 to 0.9.6 in
 /requirements (#1149)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.9.4 to 0.9.6.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.9.4...0.9.6)

---
updated-dependencies:
- dependency-name: ruff
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 178046ac0d..f5f674d7d2 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,4 +5,4 @@ cvxpy == 1.5.3
 pytest == 8.3.4 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.9.4 # For unit test
+ruff == 0.9.6 # For unit test

From b298609b2832c8029baa98b2d3906abce0263ec4 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Tue, 11 Feb 2025 21:15:34 +0900
Subject: [PATCH 11/84] update introduction doc (#1151)

---
 .../definition_of_robotics_main.rst           | 81 ++++++++++++++++---
 .../python_for_robotics_main.rst              |  2 +-
 .../technology_for_robotics_main.rst          |  9 +++
 3 files changed, 82 insertions(+), 10 deletions(-)

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 fd151e3f20..f6fba646b4 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,14 +1,77 @@
 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.
+What is Robotics?
+^^^^^^^^^^^^^^^^^^
 
-Examples of Python in Robotics:
+Robot is a machine that can perform tasks automatically or semi-autonomously.
+Robotics is the study of robots.
+The field of robotics has wide areas of technologies such as mechanical engineering,
+electrical engineering, computer science, and artificial intelligence (AI),
+to create machines that can perform tasks autonomously or semi-autonomously.
+
+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 **Al-Jazari** created intricate mechanical devices, including water clocks and humanoid robots.
+
+The Birth of Modern Robotics (1500s–1800s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **Leonardo da Vinci’s Robot (1495)** – Designed a humanoid knight with mechanical movement.
+- **Jacques de Vaucanson’s Automata (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.
+
+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.
+
+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.
+
+Advanced Robotics and AI Integration (1990s–Present)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **Autonomous Vehicles & Drones** – Self-driving cars and UAVs (unmanned aerial vehicles) became more advanced.
+- **Medical Robotics** – Robots like **da Vinci Surgical System** revolutionized healthcare.
+- **Personal Robots** – Devices like **Roomba** (vacuum robot) and **Sophia** (AI humanoid) became popular.
+- **Collaborative Robots (Cobots)** – Robots started working alongside humans in industries.
+
+Key Components of Robotics
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Robotics consists of several essential components:
+
+#. Sensors – Gather information from the environment (e.g., cameras, LiDAR, 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.
+
+Applications of Robots
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Robots come in various forms depending on their purpose:
+
+#. 🤖 Industrial Robots – Used in manufacturing (e.g., robotic arms in manufacturing factories).
+#. 🏠 Service Robots – Assist in daily life (e.g., vacuum robots, delivery robots).
+#. 🚗 Autonomous Vehicles – Self-driving cars and drones.
+#. 👨‍⚕️ Medical Robots – Assist in surgeries and healthcare.
+#. 🚀 Space & Exploration Robots – Used for planetary exploration (e.g., NASA’s Mars rovers).
+#. 🐶 Humanoid & Social Robots – Designed to interact with humans (e.g., ASIMO, Sophia).
 
-Autonomous Navigation: Python is used in self-driving cars and other autonomous vehicles for tasks like perception, localization, and path planning.
-Industrial Robotics: Python is employed in manufacturing for robot control, quality inspection, and automation.
-Service Robotics: Python powers robots that perform tasks like cleaning, delivery, and customer service in various environments.
-Research and Education: Python is a popular choice in robotics research and education due to its ease of use and versatility.
\ No newline at end of file
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 b677d2c59b..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
@@ -4,7 +4,7 @@ Python for Robotics
 A programing language, Python is used for this `PythonRobotics` project
 to achieve the purposes of this project described in the :ref:`What is PythonRobotics?`.
 
-This section explains the Python itself and features for science computing Robotics.
+This section explains the Python itself and features for science computing and robotics.
 
 Python for general-purpose programming
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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
index 4dd1d1842f..93dc9e3466 100644
--- 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
@@ -1,6 +1,15 @@
 Technology for Robotics
 -------------------------
 
+
+Autonomous Navigation
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+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.
+
 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:

From be608f067cbd96f34955b81d3e5be9e22a46f588 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Wed, 12 Feb 2025 21:51:29 +0900
Subject: [PATCH 12/84] update introduction doc (#1152)

---
 .../definition_of_robotics_main.rst           | 38 ++++++++++++++-----
 .../technology_for_robotics_main.rst          |  3 ++
 2 files changed, 31 insertions(+), 10 deletions(-)

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 f6fba646b4..63525057fe 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,14 +1,24 @@
 Definition of Robotics
 ----------------------
 
+This section explains the definition, history, key components, and applications of robotics.
+
 What is Robotics?
 ^^^^^^^^^^^^^^^^^^
 
 Robot is a machine that can perform tasks automatically or semi-autonomously.
 Robotics is the study of robots.
-The field of robotics has wide areas of technologies such as mechanical engineering,
-electrical engineering, computer science, and artificial intelligence (AI),
-to create machines that can perform tasks autonomously or semi-autonomously.
+
+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.
+
+.. _`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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -18,22 +28,30 @@ 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:
+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.
 
-- **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 **Al-Jazari** created intricate mechanical devices, including water clocks and humanoid robots.
+.. _`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 Automata (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`_  (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)*.
+- **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.
 
 The Birth of Modern Robotics (1950s–1980s)
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
index 93dc9e3466..e460059e20 100644
--- 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
@@ -1,6 +1,9 @@
 Technology for Robotics
 -------------------------
 
+The field of robotics needs wide areas of technologies such as mechanical engineering,
+electrical engineering, computer science, and artificial intelligence (AI).
+
 
 Autonomous Navigation
 ^^^^^^^^^^^^^^^^^^^^^^^^

From 1ecc154fbaf9bc2342671fe93a305e8f0e3f510f Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 13 Feb 2025 17:19:03 +0900
Subject: [PATCH 13/84] update contribution link in README.md to fix invalid
 link (#1154)

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index cb1816c2b5..818d7b0d4e 100644
--- a/README.md
+++ b/README.md
@@ -617,7 +617,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
 

From 156483000524488d64687a853ac030e8d57c18a6 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 13 Feb 2025 17:56:17 +0900
Subject: [PATCH 14/84] update robotics definition document to enhance
 references and clarity (#1155)

---
 .../definition_of_robotics_main.rst           | 29 ++++++++++++++-----
 1 file changed, 22 insertions(+), 7 deletions(-)

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 63525057fe..265814e068 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
@@ -52,21 +52,36 @@ 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.
+- **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.
+- **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)
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-- **Autonomous Vehicles & Drones** – Self-driving cars and UAVs (unmanned aerial vehicles) became more advanced.
-- **Medical Robotics** – Robots like **da Vinci Surgical System** revolutionized healthcare.
-- **Personal Robots** – Devices like **Roomba** (vacuum robot) and **Sophia** (AI humanoid) became popular.
-- **Collaborative Robots (Cobots)** – Robots started working alongside humans in industries.
+- **Autonomous Vehicles** – Self-driving cars for robo taxi like `Waymo`_ and autonomous haulage system in mining like `AHS`_ became more advanced and bisiness-ready.
+- **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.
+
+.. _`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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^

From 77ad3344b5e9df26ca370725d3921dd354c755b1 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 14 Feb 2025 21:20:35 +0900
Subject: [PATCH 15/84] update robotics definition document to improve clarity
 and add references (#1157)

---
 .../definition_of_robotics_main.rst           | 28 +++++++------------
 1 file changed, 10 insertions(+), 18 deletions(-)

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 265814e068..f54d4d41fa 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
@@ -70,12 +70,17 @@ The Birth of Modern Robotics (1950s–1980s)
 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
@@ -88,23 +93,10 @@ Key Components of Robotics
 
 Robotics consists of several essential components:
 
-#. Sensors – Gather information from the environment (e.g., cameras, LiDAR, 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).
+#. 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.
-
-Applications of Robots
-^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-Robots come in various forms depending on their purpose:
-
-#. 🤖 Industrial Robots – Used in manufacturing (e.g., robotic arms in manufacturing factories).
-#. 🏠 Service Robots – Assist in daily life (e.g., vacuum robots, delivery robots).
-#. 🚗 Autonomous Vehicles – Self-driving cars and drones.
-#. 👨‍⚕️ Medical Robots – Assist in surgeries and healthcare.
-#. 🚀 Space & Exploration Robots – Used for planetary exploration (e.g., NASA’s Mars rovers).
-#. 🐶 Humanoid & Social Robots – Designed to interact with humans (e.g., ASIMO, Sophia).
-

From 35c08824d00bd9fb452d1ee951018b177e65fd00 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sat, 15 Feb 2025 16:08:23 +0900
Subject: [PATCH 16/84] add external sensors documentation to appendix (#1159)

---
 docs/modules/12_appendix/appendix_main.rst    |  1 +
 .../12_appendix/external_sensors_main.rst     | 56 +++++++++++++++++++
 2 files changed, 57 insertions(+)
 create mode 100644 docs/modules/12_appendix/external_sensors_main.rst

diff --git a/docs/modules/12_appendix/appendix_main.rst b/docs/modules/12_appendix/appendix_main.rst
index cb1ac04066..89a7fa9303 100644
--- a/docs/modules/12_appendix/appendix_main.rst
+++ b/docs/modules/12_appendix/appendix_main.rst
@@ -10,4 +10,5 @@ Appendix
    steering_motion_model
    Kalmanfilter_basics
    Kalmanfilter_basics_2
+   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..a1dba3b214
--- /dev/null
+++ b/docs/modules/12_appendix/external_sensors_main.rst
@@ -0,0 +1,56 @@
+External Sensors for Robots
+============================
+
+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
+-------------
+
+TBD
+
+
+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
+----------
+
+TBD

From e82a12319b5336beaff01ab9fd2ce7818bb08dfb Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 16 Feb 2025 21:41:22 +0900
Subject: [PATCH 17/84] add internal sensors documentation to appendix and
 create new internal sensors overview (#1161)

---
 docs/modules/12_appendix/appendix_main.rst    |  1 +
 .../12_appendix/external_sensors_main.rst     |  4 +++
 .../12_appendix/internal_sensors_main.rst     | 32 +++++++++++++++++++
 3 files changed, 37 insertions(+)
 create mode 100644 docs/modules/12_appendix/internal_sensors_main.rst

diff --git a/docs/modules/12_appendix/appendix_main.rst b/docs/modules/12_appendix/appendix_main.rst
index 89a7fa9303..a55389e1e6 100644
--- a/docs/modules/12_appendix/appendix_main.rst
+++ b/docs/modules/12_appendix/appendix_main.rst
@@ -10,5 +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
index a1dba3b214..d36b852d42 100644
--- a/docs/modules/12_appendix/external_sensors_main.rst
+++ b/docs/modules/12_appendix/external_sensors_main.rst
@@ -1,6 +1,10 @@
 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
 ------------
 
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..13b8de4203
--- /dev/null
+++ b/docs/modules/12_appendix/internal_sensors_main.rst
@@ -0,0 +1,32 @@
+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
+------------
+
+Global Navigation Satellite System (GNSS)
+-------------------------------------------
+
+Gyroscope
+----------
+
+Accelerometer
+--------------
+
+Magnetometer
+--------------
+
+Inertial Measurement Unit (IMU)
+--------------------------------
+
+Pressure Sensor
+-----------------
+
+Temperature Sensor
+--------------------
+
+

From c92aaf36d8665a3b9ecd1a661f151cfd6ede4c66 Mon Sep 17 00:00:00 2001
From: Aglargil <34728006+Aglargil@users.noreply.github.com>
Date: Mon, 17 Feb 2025 18:47:04 +0800
Subject: [PATCH 18/84] feat: add ElasticBands (#1156)

* feat: add ElasticBands

* feat: Elastic Bands update

* feat: ElasticBands update

* feat: ElasticBands add test

* feat: ElasticBands reduce occupation

* fix: ElasticBands test

* feat: ElasticBands remove tangential component

* feat: Elastic Bands update

* feat: Elastic Bands doc

* feat: Elastic Bands update

* feat: ElasticBands update
---
 Mapping/DistanceMap/distance_map.py           |  51 +++
 PathPlanning/ElasticBands/elastic_bands.py    | 300 ++++++++++++++++++
 PathPlanning/ElasticBands/obstacles.npy       | Bin 0 -> 384 bytes
 PathPlanning/ElasticBands/path.npy            | Bin 0 -> 224 bytes
 .../elastic_bands/elastic_bands_main.rst      |  73 +++++
 .../5_path_planning/path_planning_main.rst    |   1 +
 tests/test_elastic_bands.py                   |  23 ++
 7 files changed, 448 insertions(+)
 create mode 100644 PathPlanning/ElasticBands/elastic_bands.py
 create mode 100644 PathPlanning/ElasticBands/obstacles.npy
 create mode 100644 PathPlanning/ElasticBands/path.npy
 create mode 100644 docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
 create mode 100644 tests/test_elastic_bands.py

diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py
index 54c98c6a75..0dcc7380c5 100644
--- a/Mapping/DistanceMap/distance_map.py
+++ b/Mapping/DistanceMap/distance_map.py
@@ -11,11 +11,62 @@
 
 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.
diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py
new file mode 100644
index 0000000000..785f822d14
--- /dev/null
+++ b/PathPlanning/ElasticBands/elastic_bands.py
@@ -0,0 +1,300 @@
+"""
+Elastic Bands
+
+author: Wang Zheng (@Aglargil)
+
+Ref:
+
+- [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 0000000000000000000000000000000000000000..af4376afcf0e987bbb62c4a80c7afac3d221961d
GIT binary patch
diff --git a/PathPlanning/ElasticBands/path.npy b/PathPlanning/ElasticBands/path.npy
new file mode 100644
index 0000000000000000000000000000000000000000..be7c253d6507299a2ef2f2ea901472838c89e76f
GIT binary patch
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..139996f291
--- /dev/null
+++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
@@ -0,0 +1,73 @@
+Elastic Bands
+-------------
+
+This is a path planning with Elastic Bands.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ElasticBands/animation.gif
+
+
+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
+
+Ref:
+
+-  `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/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst
index 4960330b3e..65fbfdbc3d 100644
--- a/docs/modules/5_path_planning/path_planning_main.rst
+++ b/docs/modules/5_path_planning/path_planning_main.rst
@@ -31,3 +31,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/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__)

From cbe61f8ca62785a072652741d8b33832caee1942 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 18 Feb 2025 10:45:49 +0900
Subject: [PATCH 19/84] build(deps): bump scipy from 1.15.1 to 1.15.2 in
 /requirements (#1163)

Bumps [scipy](https://github.com/scipy/scipy) from 1.15.1 to 1.15.2.
- [Release notes](https://github.com/scipy/scipy/releases)
- [Commits](https://github.com/scipy/scipy/compare/v1.15.1...v1.15.2)

---
updated-dependencies:
- dependency-name: scipy
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index f5f674d7d2..ea4550edb8 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,5 +1,5 @@
 numpy == 2.2.2
-scipy == 1.15.1
+scipy == 1.15.2
 matplotlib == 3.10.0
 cvxpy == 1.5.3
 pytest == 8.3.4 # For unit test

From 395fca59cc1f9c44e9ea7f7c08ce41958e399481 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Tue, 18 Feb 2025 20:06:34 +0900
Subject: [PATCH 20/84] fix: update robotics documentation for clarity and
 correct terminology (#1165)

---
 docs/modules/12_appendix/external_sensors_main.rst           | 2 ++
 docs/modules/12_appendix/internal_sensors_main.rst           | 2 ++
 .../1_definition_of_robotics/definition_of_robotics_main.rst | 5 +++++
 .../technologies_for_robotics_main.rst}                      | 4 +++-
 docs/modules/1_introduction/introduction_main.rst            | 2 +-
 5 files changed, 13 insertions(+), 2 deletions(-)
 rename docs/modules/1_introduction/{3_technology_for_robotics/technology_for_robotics_main.rst => 3_technologies_for_robotics/technologies_for_robotics_main.rst} (88%)

diff --git a/docs/modules/12_appendix/external_sensors_main.rst b/docs/modules/12_appendix/external_sensors_main.rst
index d36b852d42..3597418150 100644
--- a/docs/modules/12_appendix/external_sensors_main.rst
+++ b/docs/modules/12_appendix/external_sensors_main.rst
@@ -1,3 +1,5 @@
+.. _`External Sensors for Robots`:
+
 External Sensors for Robots
 ============================
 
diff --git a/docs/modules/12_appendix/internal_sensors_main.rst b/docs/modules/12_appendix/internal_sensors_main.rst
index 13b8de4203..18f209098e 100644
--- a/docs/modules/12_appendix/internal_sensors_main.rst
+++ b/docs/modules/12_appendix/internal_sensors_main.rst
@@ -1,3 +1,5 @@
+.. _`Internal Sensors for Robots`:
+
 Internal Sensors for Robots
 ============================
 
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 f54d4d41fa..1e7a833b19 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
@@ -17,6 +17,10 @@ In the play, robots were artificial workers created to serve humans, but they ev
 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
 
@@ -100,3 +104,4 @@ Robotics consists of several essential components:
 #. 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 Robotics`_ or :ref:`External Sensors for Robotics`_.
diff --git a/docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst
similarity index 88%
rename from docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst
rename to docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst
index e460059e20..64c6945c32 100644
--- a/docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst
+++ b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst
@@ -1,8 +1,10 @@
-Technology for Robotics
+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.
+
 
 
 Autonomous Navigation
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
 

From d53711998aeb1d0b3aab7a62862ef5d358f2aff9 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Tue, 18 Feb 2025 21:49:28 +0900
Subject: [PATCH 21/84] fix: update robotics documentation for clarity and
 correct terminology (#1166)

---
 README.md                                                  | 4 ++++
 .../0_getting_started/1_what_is_python_robotics_main.rst   | 6 ++++++
 .../definition_of_robotics_main.rst                        | 2 +-
 .../5_path_planning/elastic_bands/elastic_bands_main.rst   | 7 ++++---
 4 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/README.md b/README.md
index 818d7b0d4e..9e605435ce 100644
--- a/README.md
+++ b/README.md
@@ -89,6 +89,10 @@ See this documentation
 
 - [Getting Started — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/getting_started.html#what-is-pythonrobotics)
 
+or this Youtube video:
+
+- [PythonRobotics project audio overview](https://www.youtube.com/watch?v=uMeRnNoJAfU)
+
 or this paper for more details:
 
 - [\[1808\.10703\] PythonRobotics: a Python code collection of robotics algorithms](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib))
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/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 1e7a833b19..5d78bf858b 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
@@ -103,5 +103,5 @@ Robotics consists of several essential components:
 #. 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.
+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 Robotics`_ or :ref:`External Sensors for Robotics`_.
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
index 139996f291..8a3e517105 100644
--- a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
+++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
@@ -16,7 +16,7 @@ Core Concept
   * Maintain global path connectivity.
 
 Bubble Representation
-~~~~~~~~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~~~~~~~~
 - **Definition**: A local free-space region around configuration :math:`b`:
 
   .. math::
@@ -56,7 +56,7 @@ External Repulsion Force
      \frac{\partial \rho}{\partial x} \approx \frac{\rho(b_i + h) - \rho(b_i - h)}{2h}.
 
 Dynamic Path Maintenance
-~~~~~~~~~~~~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~~~~~~~~~~
 1. **Node Update**:
    
    .. math::
@@ -68,6 +68,7 @@ Dynamic Path Maintenance
 - Insert new nodes if adjacent nodes are too far apart
 - Remove redundant nodes if adjacent nodes are too close
 
-Ref:
+References
+~~~~~~~~~~~~~~~~~~~~~~~
 
 -  `Elastic Bands: Connecting Path Planning and Control <http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf>`__

From 8064488a1d63a1579d8c9d786a376c8b583c02a8 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 18 Feb 2025 21:49:40 +0900
Subject: [PATCH 22/84] build(deps): bump numpy from 2.2.2 to 2.2.3 in
 /requirements (#1164)

Bumps [numpy](https://github.com/numpy/numpy) from 2.2.2 to 2.2.3.
- [Release notes](https://github.com/numpy/numpy/releases)
- [Changelog](https://github.com/numpy/numpy/blob/main/doc/RELEASE_WALKTHROUGH.rst)
- [Commits](https://github.com/numpy/numpy/compare/v2.2.2...v2.2.3)

---
updated-dependencies:
- dependency-name: numpy
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index ea4550edb8..8176364c29 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,4 +1,4 @@
-numpy == 2.2.2
+numpy == 2.2.3
 scipy == 1.15.2
 matplotlib == 3.10.0
 cvxpy == 1.5.3

From 2b7080991e5289c9524a505244fcd2a4995da06e Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Wed, 19 Feb 2025 13:36:07 +0900
Subject: [PATCH 23/84] Add GitHub copilot pro sponser (#1167)

* fix: correct terminology in documentation and update Sphinx options

* fix: correct terminology in documentation and update Sphinx options

* fix: correct terminology in documentation and update Sphinx options

* fix: correct terminology in documentation and update Sphinx options

* fix: correct terminology in documentation and update Sphinx options
---
 docs/Makefile                                                  | 3 ++-
 docs/modules/0_getting_started/3_how_to_contribute_main.rst    | 2 ++
 .../1_definition_of_robotics/definition_of_robotics_main.rst   | 2 +-
 docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst       | 2 +-
 4 files changed, 6 insertions(+), 3 deletions(-)

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/modules/0_getting_started/3_how_to_contribute_main.rst b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
index 874564cbb8..1e61760649 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
@@ -187,6 +187,7 @@ If you would like to support us in some other way, please contact with creating
 
 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.
 
@@ -202,6 +203,7 @@ 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/
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 5d78bf858b..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
@@ -104,4 +104,4 @@ Robotics consists of several essential components:
 #. 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 Robotics`_ or :ref:`External Sensors for 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/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
 

From c7fb228d24856d6c7a0cb28c715176bfb8b17281 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 20 Feb 2025 12:11:04 +0900
Subject: [PATCH 24/84] fix: update section references to use consistent
 formatting (#1169)

---
 .../technologies_for_robotics_main.rst        | 48 +++++++++++++++----
 .../2_localization/localization_main.rst      |  2 +-
 docs/modules/3_mapping/mapping_main.rst       |  2 +-
 docs/modules/4_slam/slam_main.rst             |  2 +-
 .../5_path_planning/path_planning_main.rst    |  2 +-
 .../6_path_tracking/path_tracking_main.rst    |  2 +-
 .../7_arm_navigation/arm_navigation_main.rst  |  2 +-
 .../aerial_navigation_main.rst                |  2 +-
 docs/modules/9_bipedal/bipedal_main.rst       |  2 +-
 9 files changed, 48 insertions(+), 16 deletions(-)

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
index 64c6945c32..c77997a138 100644
--- 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
@@ -5,22 +5,54 @@ The field of robotics needs wide areas of technologies such as mechanical engine
 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
 ^^^^^^^^^^^^^^^^^^^^^^^^
-
-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.
-
-An autonomous navigation system is a system that can move to a goal over long
+Autonomous navigation is a capability 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:
+
+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`
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/3_mapping/mapping_main.rst b/docs/modules/3_mapping/mapping_main.rst
index 825b08d3ec..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
 =======
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/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst
index 65fbfdbc3d..a0f9c30a3d 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
 =============
diff --git a/docs/modules/6_path_tracking/path_tracking_main.rst b/docs/modules/6_path_tracking/path_tracking_main.rst
index d7a895b562..130a2340c1 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
 =============
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/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/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
 =================

From f343573a7bb9b99fd8fcfc72cef7336ccf859c28 Mon Sep 17 00:00:00 2001
From: Aglargil <34728006+Aglargil@users.noreply.github.com>
Date: Thu, 20 Feb 2025 18:09:30 +0800
Subject: [PATCH 25/84] Update move_to_pose for cases where alpha > pi/2 or
 alpha < -pi/2 (#1168)

* Update move_to_pose for cases where alpha > pi/2 or alpha < -pi/2

* Update move_to_pose

* Add move_to_pose test

* Update move_to_pose
---
 Control/move_to_pose/move_to_pose.py | 90 +++++++++++++++++++---------
 tests/test_move_to_pose.py           | 74 ++++++++++++++++++++++-
 2 files changed, 134 insertions(+), 30 deletions(-)

diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py
index 279ba0625b..34736a2e21 100644
--- a/Control/move_to_pose/move_to_pose.py
+++ b/Control/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
 
+        # Ref: 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/tests/test_move_to_pose.py b/tests/test_move_to_pose.py
index 8bc11a8d24..94c3ec1102 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
 
 
-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__)

From 64779298ffa77c918c63f9206b694f0d8d439c71 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 21 Feb 2025 21:40:21 +0900
Subject: [PATCH 26/84] =?UTF-8?q?refactor:=20rename=20files=20and=20update?=
 =?UTF-8?q?=20references=20for=20inverted=20pendulum=20an=E2=80=A6=20(#117?=
 =?UTF-8?q?1)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* refactor: rename files and update references for inverted pendulum and path tracking modules

* refactor: rename inverted pendulum control files and update type check references

* refactor: update import statements to use consistent casing for InvertedPendulum module
---
 Control/move_to_pose/__init__.py                    |   0
 .../inverted_pendulum_lqr_control.py                |   0
 .../inverted_pendulum_mpc_control.py                |   0
 {Control => PathTracking/move_to_pose}/__init__.py  |   0
 .../move_to_pose/move_to_pose.py                    |   0
 .../move_to_pose/move_to_pose_robot.py              |   0
 docs/index_main.rst                                 |   2 +-
 docs/modules/10_control/control_main.rst            |  12 ------------
 .../inverted-pendulum.png                           | Bin
 .../inverted_pendulum_main.rst}                     |   6 ++++--
 docs/modules/11_utils/utils_main.rst                |   2 +-
 docs/modules/12_appendix/appendix_main.rst          |   2 +-
 .../technologies_for_robotics_main.rst              |  10 ++++++++++
 .../move_to_a_pose_control_main.rst                 |   0
 docs/modules/6_path_tracking/path_tracking_main.rst |   1 +
 tests/test_inverted_pendulum_lqr_control.py         |   2 +-
 tests/test_inverted_pendulum_mpc_control.py         |   2 +-
 tests/test_move_to_pose.py                          |   2 +-
 tests/test_move_to_pose_robot.py                    |   2 +-
 tests/test_mypy_type_check.py                       |   2 +-
 20 files changed, 23 insertions(+), 22 deletions(-)
 delete mode 100644 Control/move_to_pose/__init__.py
 rename {Control/inverted_pendulum => InvertedPendulum}/inverted_pendulum_lqr_control.py (100%)
 rename {Control/inverted_pendulum => InvertedPendulum}/inverted_pendulum_mpc_control.py (100%)
 rename {Control => PathTracking/move_to_pose}/__init__.py (100%)
 rename {Control => PathTracking}/move_to_pose/move_to_pose.py (100%)
 rename {Control => PathTracking}/move_to_pose/move_to_pose_robot.py (100%)
 delete mode 100644 docs/modules/10_control/control_main.rst
 rename docs/modules/{10_control/inverted_pendulum_control => 10_inverted_pendulum}/inverted-pendulum.png (100%)
 rename docs/modules/{10_control/inverted_pendulum_control/inverted_pendulum_control_main.rst => 10_inverted_pendulum/inverted_pendulum_main.rst} (97%)
 rename docs/modules/{10_control => 6_path_tracking}/move_to_a_pose_control/move_to_a_pose_control_main.rst (100%)

diff --git a/Control/move_to_pose/__init__.py b/Control/move_to_pose/__init__.py
deleted file mode 100644
index e69de29bb2..0000000000
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 100%
rename from Control/inverted_pendulum/inverted_pendulum_mpc_control.py
rename to InvertedPendulum/inverted_pendulum_mpc_control.py
diff --git a/Control/__init__.py b/PathTracking/move_to_pose/__init__.py
similarity index 100%
rename from Control/__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 100%
rename from Control/move_to_pose/move_to_pose.py
rename to PathTracking/move_to_pose/move_to_pose.py
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/docs/index_main.rst b/docs/index_main.rst
index c1e8d22d32..75805d1184 100644
--- a/docs/index_main.rst
+++ b/docs/index_main.rst
@@ -43,7 +43,7 @@ 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/11_utils/utils
    modules/12_appendix/appendix
 
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 97%
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..048cbea9ac 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.
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/appendix_main.rst b/docs/modules/12_appendix/appendix_main.rst
index a55389e1e6..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
 ==============
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
index c77997a138..0ed51e961b 100644
--- 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
@@ -56,3 +56,13 @@ Robot type specific technologies
 
 #. :ref:`Aerial Navigation`
 #. :ref:`Bipedal`
+#. :ref:`Inverted Pendulum`
+
+
+Additional Information
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+#. :ref:`utils`
+#. :ref:`Appendix`
+
+
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_control/move_to_a_pose_control_main.rst
similarity index 100%
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_control/move_to_a_pose_control_main.rst
diff --git a/docs/modules/6_path_tracking/path_tracking_main.rst b/docs/modules/6_path_tracking/path_tracking_main.rst
index 130a2340c1..d98e324583 100644
--- a/docs/modules/6_path_tracking/path_tracking_main.rst
+++ b/docs/modules/6_path_tracking/path_tracking_main.rst
@@ -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_control/move_to_a_pose_control
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_move_to_pose.py b/tests/test_move_to_pose.py
index 94c3ec1102..e06d801555 100644
--- a/tests/test_move_to_pose.py
+++ b/tests/test_move_to_pose.py
@@ -1,7 +1,7 @@
 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_random():
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"
 ]
 
 

From 6e13e8292aad80661093603ed262c6f0bdcb4137 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 25 Feb 2025 08:07:42 +0900
Subject: [PATCH 27/84] build(deps): bump ruff from 0.9.6 to 0.9.7 in
 /requirements (#1173)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.9.6 to 0.9.7.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.9.6...0.9.7)

---
updated-dependencies:
- dependency-name: ruff
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 8176364c29..b439ea4266 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,4 +5,4 @@ cvxpy == 1.5.3
 pytest == 8.3.4 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.9.6 # For unit test
+ruff == 0.9.7 # For unit test

From 0c8ff11645e6804db18bec6bd918e03ed03454f7 Mon Sep 17 00:00:00 2001
From: Jonathan Schwartz <jonbs98@gmail.com>
Date: Tue, 25 Feb 2025 06:53:36 -0500
Subject: [PATCH 28/84] Space-Time AStar (#1170)

* wip - sketch out obstacles

* move to correct path

* better animation

* clean up

* use np to sample points

* implemented time-based A*

* cleaning up Grid + adding new obstacle arrangement

* added unit test

* formatting p1

* format STA* file

* remove newlines by docstrings

* linter

* working on typehints

* fix linter errors

* lint some more

* appease AppVeyor

* dataclasses are :fire:

* back to @total_ordering

* trailing whitespace

* add docs page on SpaceTimeA*

* docs lint

* remove trailing newlines in doc

* address comments

* Update docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst

---------

Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>
---
 .../GridWithDynamicObstacles.py               | 273 ++++++++++++++++++
 .../TimeBasedPathPlanning/SpaceTimeAStar.py   | 220 ++++++++++++++
 .../TimeBasedPathPlanning/__init__.py         |   0
 .../5_path_planning/path_planning_main.rst    |   1 +
 .../time_based_grid_search_main.rst           |  22 ++
 tests/test_space_time_astar.py                |  33 +++
 6 files changed, 549 insertions(+)
 create mode 100644 PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
 create mode 100644 PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
 create mode 100644 PathPlanning/TimeBasedPathPlanning/__init__.py
 create mode 100644 docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst
 create mode 100644 tests/test_space_time_astar.py

diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
new file mode 100644
index 0000000000..7b0190d023
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
@@ -0,0 +1,273 @@
+"""
+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
+
+@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)}"
+        )
+
+
+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
+
+
+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)
+
+        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 (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
+
+    """
+    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 new position is in grid
+        if not self.inside_grid_bounds(position):
+            return False
+
+        # Check if new 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)
+
+
+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/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
new file mode 100644
index 0000000000..3b3613d695
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
@@ -0,0 +1,220 @@
+"""
+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 respescting any dynamic obstacles in the environment.
+
+    Reference: https://www.davidsilver.uk/wp-content/uploads/2020/03/coop-path-AIWisdom.pdf
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+    Grid,
+    ObstacleArrangement,
+    Position,
+)
+import heapq
+from collections.abc import Generator
+import random
+from dataclasses import dataclass
+from functools import total_ordering
+
+
+# Seed randomness for reproducibility
+RANDOM_SEED = 50
+random.seed(RANDOM_SEED)
+np.random.seed(RANDOM_SEED)
+
+@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)
+
+    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
+
+
+class NodePath:
+    path: list[Node]
+    positions_at_time: dict[int, Position] = {}
+
+    def __init__(self, path: list[Node]):
+        self.path = path
+        for node in path:
+            self.positions_at_time[node.time] = 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
+
+
+class SpaceTimeAStar:
+    grid: Grid
+    start: Position
+    goal: Position
+
+    def __init__(self, grid: Grid, start: Position, goal: Position):
+        self.grid = grid
+        self.start = start
+        self.goal = goal
+
+    def plan(self, verbose: bool = False) -> NodePath:
+        open_set: list[Node] = []
+        heapq.heappush(
+            open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1)
+        )
+
+        expanded_set: list[Node] = []
+        while open_set:
+            expanded_node: Node = heapq.heappop(open_set)
+            if verbose:
+                print("Expanded node:", expanded_node)
+
+            if expanded_node.time + 1 >= self.grid.time_limit:
+                if verbose:
+                    print(f"\tSkipping node that is past time limit: {expanded_node}")
+                continue
+
+            if expanded_node.position == self.goal:
+                print(f"Found path to goal after {len(expanded_set)} expansions")
+                path = []
+                path_walker: Node = expanded_node
+                while True:
+                    path.append(path_walker)
+                    if path_walker.parent_index == -1:
+                        break
+                    path_walker = expanded_set[path_walker.parent_index]
+
+                # reverse path so it goes start -> goal
+                path.reverse()
+                return NodePath(path)
+
+            expanded_idx = len(expanded_set)
+            expanded_set.append(expanded_node)
+
+            for child in self.generate_successors(expanded_node, expanded_idx, verbose):
+                heapq.heappush(open_set, child)
+
+        raise Exception("No path found")
+
+    """
+    Generate possible successors of the provided `parent_node`
+    """
+    def generate_successors(
+        self, parent_node: Node, parent_node_idx: int, verbose: bool
+    ) -> 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
+            if self.grid.valid_position(new_pos, parent_node.time + 1):
+                new_node = Node(
+                    new_pos,
+                    parent_node.time + 1,
+                    self.calculate_heuristic(new_pos),
+                    parent_node_idx,
+                )
+                if verbose:
+                    print("\tNew successor node: ", new_node)
+                yield new_node
+
+    def calculate_heuristic(self, position) -> int:
+        diff = self.goal - position
+        return abs(diff.x) + abs(diff.y)
+
+
+show_animation = True
+verbose = False
+
+def main():
+    start = Position(1, 11)
+    goal = Position(19, 19)
+    grid_side_length = 21
+    grid = Grid(
+        np.array([grid_side_length, grid_side_length]),
+        num_obstacles=40,
+        obstacle_avoid_points=[start, goal],
+        obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+    )
+
+    planner = SpaceTimeAStar(grid, start, goal)
+    path = planner.plan(verbose)
+
+    if verbose:
+        print(f"Path: {path}")
+
+    if not show_animation:
+        return
+
+    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_side_length, 1))
+    ax.set_yticks(np.arange(0, grid_side_length, 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]
+    )
+
+    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)
+        path_points.set_data([path_position.x], [path_position.y])
+        plt.pause(0.2)
+    plt.show()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/PathPlanning/TimeBasedPathPlanning/__init__.py b/PathPlanning/TimeBasedPathPlanning/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst
index a0f9c30a3d..0c84a19c22 100644
--- a/docs/modules/5_path_planning/path_planning_main.rst
+++ b/docs/modules/5_path_planning/path_planning_main.rst
@@ -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
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..0c26badec7
--- /dev/null
+++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst
@@ -0,0 +1,22 @@
+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.
+
+References:
+~~~~~~~~~~~
+
+-  `Cooperative Pathfinding <https://www.davidsilver.uk/wp-content/uploads/2020/03/coop-path-AIWisdom.pdf>`__
diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py
new file mode 100644
index 0000000000..5290738eb4
--- /dev/null
+++ b/tests/test_space_time_astar.py
@@ -0,0 +1,33 @@
+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
+    planner = m.SpaceTimeAStar(grid, start, goal)
+
+    path = planner.plan(False)
+
+    # path should have 28 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__)

From 67a3ca7138384b4a013974dbf22383252f83686e Mon Sep 17 00:00:00 2001
From: Aglargil <34728006+Aglargil@users.noreply.github.com>
Date: Fri, 28 Feb 2025 19:30:24 +0800
Subject: [PATCH 29/84] add state machine (#1172)

* add state machine

state_machine test

state_machine_update

* add state machine test/doc

* state machine update

* state machine generate_plantuml() can show diagram by using https://www.plantuml.com/plantuml/
---
 .../StateMachine/robot_behavior_case.py       | 111 +++++++
 MissionPlanning/StateMachine/state_machine.py | 294 ++++++++++++++++++
 docs/index_main.rst                           |   1 +
 .../mission_planning_main.rst                 |  12 +
 .../state_machine/robot_behavior_case.png     | Bin 0 -> 28565 bytes
 .../state_machine/state_machine_main.rst      |  74 +++++
 tests/test_state_machine.py                   |  51 +++
 7 files changed, 543 insertions(+)
 create mode 100644 MissionPlanning/StateMachine/robot_behavior_case.py
 create mode 100644 MissionPlanning/StateMachine/state_machine.py
 create mode 100644 docs/modules/13_mission_planning/mission_planning_main.rst
 create mode 100644 docs/modules/13_mission_planning/state_machine/robot_behavior_case.png
 create mode 100644 docs/modules/13_mission_planning/state_machine/state_machine_main.rst
 create mode 100644 tests/test_state_machine.py

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..de72f0f451
--- /dev/null
+++ b/MissionPlanning/StateMachine/state_machine.py
@@ -0,0 +1,294 @@
+"""
+State Machine
+
+author: Wang Zheng (@Aglargil)
+
+Ref:
+
+- [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.
+
+    Ref: 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/docs/index_main.rst b/docs/index_main.rst
index 75805d1184..65634f32e8 100644
--- a/docs/index_main.rst
+++ b/docs/index_main.rst
@@ -44,6 +44,7 @@ this graph shows GitHub star history of this project:
    modules/8_aerial_navigation/aerial_navigation
    modules/9_bipedal/bipedal
    modules/10_inverted_pendulum/inverted_pendulum
+   modules/13_mission_planning/mission_planning
    modules/11_utils/utils
    modules/12_appendix/appendix
 
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..385e62f68e
--- /dev/null
+++ b/docs/modules/13_mission_planning/mission_planning_main.rst
@@ -0,0 +1,12 @@
+.. _`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
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 0000000000000000000000000000000000000000..fbc1369cbcd71f278a89d38adbcda6da4b737e36
GIT binary patch
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..abaece1b11
--- /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
+
+API
+~~~
+
+.. 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/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__)

From 346037a6e2362ed17870928ccf26992b60b0e54c Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 4 Mar 2025 07:26:47 +0900
Subject: [PATCH 30/84] build(deps): bump pytest from 8.3.4 to 8.3.5 in
 /requirements (#1178)

Bumps [pytest](https://github.com/pytest-dev/pytest) from 8.3.4 to 8.3.5.
- [Release notes](https://github.com/pytest-dev/pytest/releases)
- [Changelog](https://github.com/pytest-dev/pytest/blob/main/CHANGELOG.rst)
- [Commits](https://github.com/pytest-dev/pytest/compare/8.3.4...8.3.5)

---
updated-dependencies:
- dependency-name: pytest
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index b439ea4266..b46a0e41f1 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -2,7 +2,7 @@ numpy == 2.2.3
 scipy == 1.15.2
 matplotlib == 3.10.0
 cvxpy == 1.5.3
-pytest == 8.3.4 # For unit test
+pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
 ruff == 0.9.7 # For unit test

From cd09abd5e00ca29d15f42c539121cf62751ecd52 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 4 Mar 2025 12:21:33 +0900
Subject: [PATCH 31/84] build(deps): bump ruff from 0.9.7 to 0.9.9 in
 /requirements (#1179)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.9.7 to 0.9.9.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.9.7...0.9.9)

---
updated-dependencies:
- dependency-name: ruff
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index b46a0e41f1..3e2a1e7c7c 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,4 +5,4 @@ cvxpy == 1.5.3
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.9.7 # For unit test
+ruff == 0.9.9 # For unit test

From 5f3be9bccd6366493ea8d159ceeae26d55ed5250 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 4 Mar 2025 12:52:57 +0900
Subject: [PATCH 32/84] build(deps): bump matplotlib from 3.10.0 to 3.10.1 in
 /requirements (#1181)

Bumps [matplotlib](https://github.com/matplotlib/matplotlib) from 3.10.0 to 3.10.1.
- [Release notes](https://github.com/matplotlib/matplotlib/releases)
- [Commits](https://github.com/matplotlib/matplotlib/compare/v3.10.0...v3.10.1)

---
updated-dependencies:
- dependency-name: matplotlib
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 3e2a1e7c7c..552bf8482b 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,6 +1,6 @@
 numpy == 2.2.3
 scipy == 1.15.2
-matplotlib == 3.10.0
+matplotlib == 3.10.1
 cvxpy == 1.5.3
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test

From 30a61add126e2c21c035d50a7a448ed8eb06afb0 Mon Sep 17 00:00:00 2001
From: Surya Singh <133056660+spnsingh@users.noreply.github.com>
Date: Fri, 7 Mar 2025 09:01:37 -0500
Subject: [PATCH 33/84] bug: fix typo on line 6 of SpaceTimeAStar.py (#1182)

* bug: fix typo on line 6 of SpaceTimeAStar.py

* bug: removed extra line return on line 11 of SpaceTimeAStar.py
---
 PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
index 3b3613d695..a7aed41869 100644
--- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
+++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
@@ -3,7 +3,7 @@
     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 respescting any dynamic obstacles in the environment.
+    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
 """

From fc160179c06cb4b57f821d83a9bdcf192379bd72 Mon Sep 17 00:00:00 2001
From: Aglargil <34728006+Aglargil@users.noreply.github.com>
Date: Sat, 8 Mar 2025 18:26:23 +0800
Subject: [PATCH 34/84] feat: add behavior tree (#1177)

* feat: add behavior tree

* feat: add behavior tree test

* feat: add behavior tree doc

* feat: add behavior tree update
---
 MissionPlanning/BehaviorTree/behavior_tree.py | 690 ++++++++++++++++++
 .../BehaviorTree/robot_behavior_case.py       | 247 +++++++
 .../BehaviorTree/robot_behavior_tree.xml      |  57 ++
 .../behavior_tree/behavior_tree_main.rst      | 104 +++
 .../behavior_tree/robot_behavior_case.svg     |  22 +
 .../mission_planning_main.rst                 |   1 +
 tests/test_behavior_tree.py                   | 207 ++++++
 7 files changed, 1328 insertions(+)
 create mode 100644 MissionPlanning/BehaviorTree/behavior_tree.py
 create mode 100644 MissionPlanning/BehaviorTree/robot_behavior_case.py
 create mode 100644 MissionPlanning/BehaviorTree/robot_behavior_tree.xml
 create mode 100644 docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
 create mode 100644 docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
 create mode 100644 tests/test_behavior_tree.py

diff --git a/MissionPlanning/BehaviorTree/behavior_tree.py b/MissionPlanning/BehaviorTree/behavior_tree.py
new file mode 100644
index 0000000000..59f4c713f1
--- /dev/null
+++ b/MissionPlanning/BehaviorTree/behavior_tree.py
@@ -0,0 +1,690 @@
+"""
+Behavior Tree
+
+author: Wang Zheng (@Aglargil)
+
+Ref:
+
+- [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/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..ae3e16da81
--- /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)
+
+Core Concepts
+~~~~~~~~~~~~~
+
+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
index 385e62f68e..c35eacd8d5 100644
--- a/docs/modules/13_mission_planning/mission_planning_main.rst
+++ b/docs/modules/13_mission_planning/mission_planning_main.rst
@@ -10,3 +10,4 @@ Mission planning includes tools such as finite state machines and behavior trees
    :caption: Contents
 
    state_machine/state_machine
+   behavior_tree/behavior_tree
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__)

From e7f893ef1aa503df04c83ec6355d18dd05d36394 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 9 Mar 2025 20:21:24 +0900
Subject: [PATCH 35/84] feat: add linkcode_resolve function for enhanced
 documentation linking (#1185)

* feat: add linkcode_resolve function for enhanced documentation linking

* feat: enhance documentation linking with linkcode and relative path resolution
---
 docs/conf.py | 47 +++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 45 insertions(+), 2 deletions(-)

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

From 73ebcd85dc967dbb160e29ee4c529b7a64f11e2b Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Wed, 12 Mar 2025 21:26:09 +0900
Subject: [PATCH 36/84] build(deps): bump ruff from 0.9.9 to 0.9.10 in
 /requirements (#1186)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.9.9 to 0.9.10.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.9.9...0.9.10)

---
updated-dependencies:
- dependency-name: ruff
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 552bf8482b..cf6dea3a46 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,4 +5,4 @@ cvxpy == 1.5.3
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.9.9 # For unit test
+ruff == 0.9.10 # For unit test

From 1308e76424723eb68a4a864117e60c0bbd054df1 Mon Sep 17 00:00:00 2001
From: Jonathan Schwartz <jonbs98@gmail.com>
Date: Thu, 13 Mar 2025 10:24:53 -0400
Subject: [PATCH 37/84] Add expanded node set to SpaceTime AStar (#1183)

* speed up spacetime astar

* forgot to include hash impl on Position

* add condition to test on node expansions

* remove heuristic from Node __hash__ impl

* update rst with note about optimization
---
 .../GridWithDynamicObstacles.py               |  3 ++
 .../TimeBasedPathPlanning/SpaceTimeAStar.py   | 51 +++++++++++++------
 .../time_based_grid_search_main.rst           | 17 +++++++
 tests/test_space_time_astar.py                |  1 +
 4 files changed, 57 insertions(+), 15 deletions(-)

diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
index 7b0190d023..7416ff2f08 100644
--- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
+++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
@@ -30,6 +30,9 @@ def __sub__(self, other):
             f"Subtraction not supported for Position and {type(other)}"
         )
 
+    def __hash__(self):
+        return hash((self.x, self.y))
+
 
 class ObstacleArrangement(Enum):
     # Random obstacle positions and movements
diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
index a7aed41869..c4e2802d37 100644
--- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
+++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
@@ -20,7 +20,7 @@
 import random
 from dataclasses import dataclass
 from functools import total_ordering
-
+import time
 
 # Seed randomness for reproducibility
 RANDOM_SEED = 50
@@ -48,11 +48,17 @@ def __lt__(self, other: object):
             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: list[Node]
@@ -86,6 +92,8 @@ class SpaceTimeAStar:
     grid: Grid
     start: Position
     goal: Position
+    # Used to evaluate solutions
+    expanded_node_count: int = -1
 
     def __init__(self, grid: Grid, start: Position, goal: Position):
         self.grid = grid
@@ -98,7 +106,8 @@ def plan(self, verbose: bool = False) -> NodePath:
             open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1)
         )
 
-        expanded_set: list[Node] = []
+        expanded_list: list[Node] = []
+        expanded_set: set[Node] = set()
         while open_set:
             expanded_node: Node = heapq.heappop(open_set)
             if verbose:
@@ -110,23 +119,25 @@ def plan(self, verbose: bool = False) -> NodePath:
                 continue
 
             if expanded_node.position == self.goal:
-                print(f"Found path to goal after {len(expanded_set)} expansions")
+                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_set[path_walker.parent_index]
+                    path_walker = expanded_list[path_walker.parent_index]
 
                 # reverse path so it goes start -> goal
                 path.reverse()
+                self.expanded_node_count = len(expanded_set)
                 return NodePath(path)
 
-            expanded_idx = len(expanded_set)
-            expanded_set.append(expanded_node)
+            expanded_idx = len(expanded_list)
+            expanded_list.append(expanded_node)
+            expanded_set.add(expanded_node)
 
-            for child in self.generate_successors(expanded_node, expanded_idx, verbose):
+            for child in self.generate_successors(expanded_node, expanded_idx, verbose, expanded_set):
                 heapq.heappush(open_set, child)
 
         raise Exception("No path found")
@@ -135,7 +146,7 @@ def plan(self, verbose: bool = False) -> NodePath:
     Generate possible successors of the provided `parent_node`
     """
     def generate_successors(
-        self, parent_node: Node, parent_node_idx: int, verbose: bool
+        self, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node]
     ) -> Generator[Node, None, None]:
         diffs = [
             Position(0, 0),
@@ -146,13 +157,17 @@ def generate_successors(
         ]
         for diff in diffs:
             new_pos = parent_node.position + diff
+            new_node = Node(
+                new_pos,
+                parent_node.time + 1,
+                self.calculate_heuristic(new_pos),
+                parent_node_idx,
+            )
+
+            if new_node in expanded_set:
+                continue
+
             if self.grid.valid_position(new_pos, parent_node.time + 1):
-                new_node = Node(
-                    new_pos,
-                    parent_node.time + 1,
-                    self.calculate_heuristic(new_pos),
-                    parent_node_idx,
-                )
                 if verbose:
                     print("\tNew successor node: ", new_node)
                 yield new_node
@@ -166,9 +181,12 @@ def calculate_heuristic(self, position) -> int:
 verbose = False
 
 def main():
-    start = Position(1, 11)
+    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,
@@ -179,6 +197,9 @@ def main():
     planner = SpaceTimeAStar(grid, start, goal)
     path = planner.plan(verbose)
 
+    runtime = time.time() - start_time
+    print(f"Planning took: {runtime:.5f} seconds")
+
     if verbose:
         print(f"Path: {path}")
 
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
index 0c26badec7..48dc1289c2 100644
--- 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
@@ -16,6 +16,23 @@ Using a time-based cost and heuristic ensures the path found is optimal in terms
 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).
+
 References:
 ~~~~~~~~~~~
 
diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py
index 5290738eb4..390c7732dc 100644
--- a/tests/test_space_time_astar.py
+++ b/tests/test_space_time_astar.py
@@ -28,6 +28,7 @@ def test_1():
     # path should end at the goal
     assert path.path[-1].position == goal
 
+    assert planner.expanded_node_count < 1000
 
 if __name__ == "__main__":
     conftest.run_this_test(__file__)

From 41187d67b11f06265431b87dcf0b9da679345249 Mon Sep 17 00:00:00 2001
From: Zichao Yang <yangtzech@qq.com>
Date: Sun, 16 Mar 2025 21:40:11 +0800
Subject: [PATCH 38/84] fix "ModuleNotFoundError: No module named 'utils'"
 (#1188)

---
 .../lqr_speed_steer_control/lqr_speed_steer_control.py     | 4 ++--
 PathTracking/lqr_steer_control/lqr_steer_control.py        | 4 ++--
 PathTracking/rear_wheel_feedback/rear_wheel_feedback.py    | 7 +++++--
 PathTracking/stanley_controller/stanley_controller.py      | 4 ++--
 4 files changed, 11 insertions(+), 8 deletions(-)

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/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
index 1702bd47dd..fd04fb6d17 100644
--- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
+++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.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_controller/stanley_controller.py
index 08aa049395..bc98175f17 100644
--- a/PathTracking/stanley_controller/stanley_controller.py
+++ b/PathTracking/stanley_controller/stanley_controller.py
@@ -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

From aa61a6ea570e013d346c40874f8a33890ec66d28 Mon Sep 17 00:00:00 2001
From: Jonathan Schwartz <jonbs98@gmail.com>
Date: Mon, 17 Mar 2025 09:01:07 -0400
Subject: [PATCH 39/84] Safe Interval Path Planner (#1184)

* it works and is WAY faster than a*

* some bug fixes from testing different scenarios

* add some docs & address todos

* add sipp test

* spiff up comments

revert changes in speed-up

* explain what the removal is doing

* linting

* fix docs build

* docs formatting

* revert change to file (maybe linter did it?)

* point at gifs in gifs repo

* use raw githubusercontent gif links

* change formatting on planner results

* format output differently

* proper formatting final

* missing underline

* revert unintended change

* grammar + add descriptions for gifs

* missing ::

* add title to gifs section

* dont use sections for sub-sections

* constent a* spelling

* Update PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Update tests/test_safe_interval_path_planner.py

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Update docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst

Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>

* Update PathPlanning/TimeBasedPathPlanning/SafeInterval.py

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* addressing comments

* revert np.full change

---------

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>
---
 .../GridWithDynamicObstacles.py               |  57 +++-
 .../TimeBasedPathPlanning/SafeInterval.py     | 303 ++++++++++++++++++
 .../time_based_grid_search_main.rst           |  43 ++-
 tests/test_safe_interval_path_planner.py      |  33 ++
 4 files changed, 434 insertions(+), 2 deletions(-)
 create mode 100644 PathPlanning/TimeBasedPathPlanning/SafeInterval.py
 create mode 100644 tests/test_safe_interval_path_planner.py

diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
index 7416ff2f08..2a47023f8c 100644
--- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
+++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
@@ -33,6 +33,10 @@ def __sub__(self, other):
     def __hash__(self):
         return hash((self.x, self.y))
 
+@dataclass
+class Interval:
+    start_time: int
+    end_time: int
 
 class ObstacleArrangement(Enum):
     # Random obstacle positions and movements
@@ -40,6 +44,14 @@ class ObstacleArrangement(Enum):
     # Obstacles start in a line in y at center of grid and move side-to-side in x
     ARRANGEMENT1 = 1
 
+"""
+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
@@ -89,7 +101,7 @@ def __init__(
     """
     def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
         obstacle_paths = []
-        for _ in (0, obs_count):
+        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):
@@ -234,6 +246,49 @@ def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]:
             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
 
 show_animation = True
 
diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
new file mode 100644
index 0000000000..7fea10c67f
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
@@ -0,0 +1,303 @@
+"""
+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
+import matplotlib.pyplot as plt
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+    Grid,
+    Interval,
+    ObstacleArrangement,
+    Position,
+    empty_2d_array_of_lists,
+)
+import heapq
+import random
+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 Node:
+    position: Position
+    time: int
+    heuristic: int
+    parent_index: int
+    interval: Interval
+
+    """
+    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)
+
+    """
+    Equality only cares about position and time. Heuristic and interval will always be the same for a given
+    (position, time) pairing, so they are not considered in equality.
+    """
+    def __eq__(self, other: object):
+        if not isinstance(other, Node):
+            return NotImplemented
+        return self.position == other.position and self.time == other.time
+
+@dataclass
+class EntryTimeAndInterval:
+    entry_time: int
+    interval: Interval
+
+class NodePath:
+    path: list[Node]
+    positions_at_time: dict[int, Position] = {}
+
+    def __init__(self, path: list[Node]):
+        self.path = path
+        for (i, node) in enumerate(path):
+            if i > 0:
+                # account for waiting in interval at previous node
+                prev_node = path[i-1]
+                for t in range(prev_node.time, node.time):
+                    self.positions_at_time[t] = prev_node.position
+
+            self.positions_at_time[node.time] = 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
+
+
+class SafeIntervalPathPlanner:
+    grid: Grid
+    start: Position
+    goal: Position
+
+    def __init__(self, grid: Grid, start: Position, goal: Position):
+        self.grid = grid
+        self.start = start
+        self.goal = goal
+
+        # Seed randomness for reproducibility
+        RANDOM_SEED = 50
+        random.seed(RANDOM_SEED)
+        np.random.seed(RANDOM_SEED)
+
+    """
+    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
+    """
+    def plan(self, verbose: bool = False) -> NodePath:
+
+        safe_intervals = self.grid.get_safe_intervals()
+
+        open_set: list[Node] = []
+        first_node_interval = safe_intervals[self.start.x, self.start.y][0]
+        heapq.heappush(
+            open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1, first_node_interval)
+        )
+
+        expanded_list: list[Node] = []
+        visited_intervals = empty_2d_array_of_lists(self.grid.grid_size[0], self.grid.grid_size[1])
+        while open_set:
+            expanded_node: Node = heapq.heappop(open_set)
+            if verbose:
+                print("Expanded node:", expanded_node)
+
+            if expanded_node.time + 1 >= self.grid.time_limit:
+                if verbose:
+                    print(f"\tSkipping node that is past time limit: {expanded_node}")
+                continue
+
+            if expanded_node.position == self.goal:
+                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)
+
+            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 self.generate_successors(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
+    """
+    def generate_successors(
+        self, parent_node: Node, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray
+    ) -> list[Node]:
+        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 self.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 self.grid.valid_position(new_pos, possible_t):
+                        new_nodes.append(Node(
+                            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),
+                            self.calculate_heuristic(new_pos),
+                            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
+    """
+    def calculate_heuristic(self, position) -> int:
+        diff = self.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: Node):
+    # 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,
+    )
+
+    planner = SafeIntervalPathPlanner(grid, start, goal)
+    path = planner.plan(verbose)
+    runtime = time.time() - start_time
+    print(f"Planning took: {runtime:.5f} seconds")
+
+    if verbose:
+        print(f"Path: {path}")
+
+    if not show_animation:
+        return
+
+    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_side_length, 1))
+    ax.set_yticks(np.arange(0, grid_side_length, 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]
+    )
+
+    for i in range(0, path.goal_reached_time() + 1):
+        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)
+        path_points.set_data([path_position.x], [path_position.y])
+        plt.pause(0.2)
+    plt.show()
+
+
+if __name__ == "__main__":
+    main()
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
index 48dc1289c2..04001d4504 100644
--- 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
@@ -33,7 +33,48 @@ After::
 
 When starting at (1, 11) in the structured obstacle arrangement (second of the two gifs above).
 
-References:
+
+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
+
+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>`__
diff --git a/tests/test_safe_interval_path_planner.py b/tests/test_safe_interval_path_planner.py
new file mode 100644
index 0000000000..f1fbf90d2a
--- /dev/null
+++ b/tests/test_safe_interval_path_planner.py
@@ -0,0 +1,33 @@
+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
+    planner = m.SafeIntervalPathPlanner(grid, start, goal)
+
+    path = planner.plan(False)
+
+    # 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__)

From 19fc48d60fea057f6c14365c502db4d18490c3a8 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 18 Mar 2025 20:22:31 +0900
Subject: [PATCH 40/84] build(deps): bump numpy from 2.2.3 to 2.2.4 in
 /requirements (#1189)

Bumps [numpy](https://github.com/numpy/numpy) from 2.2.3 to 2.2.4.
- [Release notes](https://github.com/numpy/numpy/releases)
- [Changelog](https://github.com/numpy/numpy/blob/main/doc/RELEASE_WALKTHROUGH.rst)
- [Commits](https://github.com/numpy/numpy/compare/v2.2.3...v2.2.4)

---
updated-dependencies:
- dependency-name: numpy
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index cf6dea3a46..3ea4197736 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,4 +1,4 @@
-numpy == 2.2.3
+numpy == 2.2.4
 scipy == 1.15.2
 matplotlib == 3.10.1
 cvxpy == 1.5.3

From 764ba21f86fc0d5c2ae5b613449ca81ea6406036 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 18 Mar 2025 20:29:12 +0900
Subject: [PATCH 41/84] build(deps): bump ruff from 0.9.10 to 0.11.0 in
 /requirements (#1191)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.9.10 to 0.11.0.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.9.10...0.11.0)

---
updated-dependencies:
- dependency-name: ruff
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 3ea4197736..5438678f87 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,4 +5,4 @@ cvxpy == 1.5.3
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.9.10 # For unit test
+ruff == 0.11.0 # For unit test

From 1c1596be21f015b3b2c3db6489b93cd94d6eca01 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Mon, 28 Apr 2025 08:51:09 +0900
Subject: [PATCH 42/84] docs: enhance contribution guide with instructions for
 linking code in GitHub (#1202)

---
 docs/_static/img/source_link_1.png             | Bin 0 -> 87987 bytes
 docs/_static/img/source_link_2.png             | Bin 0 -> 237071 bytes
 .../3_how_to_contribute_main.rst               |  13 +++++++++++++
 3 files changed, 13 insertions(+)
 create mode 100644 docs/_static/img/source_link_1.png
 create mode 100644 docs/_static/img/source_link_2.png

diff --git a/docs/_static/img/source_link_1.png b/docs/_static/img/source_link_1.png
new file mode 100644
index 0000000000000000000000000000000000000000..53febda7cbc62ba625f3f179b399552c7da351e5
GIT binary patch
diff --git a/docs/_static/img/source_link_2.png b/docs/_static/img/source_link_2.png
new file mode 100644
index 0000000000000000000000000000000000000000..d5647cac605e25ceddf274147ce22bcc6104bf1b
GIT binary patch
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 1e61760649..30e3001f36 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
@@ -107,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`:
 
@@ -210,5 +222,6 @@ Current Major Sponsors:
 .. _`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
 
 

From f466f25f5328e019bd1e0ccc62b176ff2fc72187 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Mon, 28 Apr 2025 08:53:35 +0900
Subject: [PATCH 43/84] build(deps): bump ruff from 0.11.0 to 0.11.6 in
 /requirements (#1201)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.0 to 0.11.6.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.0...0.11.6)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.6
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 5438678f87..f3e6c3dc51 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,4 +5,4 @@ cvxpy == 1.5.3
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.11.0 # For unit test
+ruff == 0.11.6 # For unit test

From af0442d358b1deca5710c0c6812424b12b39f7e7 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 1 May 2025 13:08:29 +0900
Subject: [PATCH 44/84] build(deps): update cvxpy version from 1.5.3 to 1.6.5
 in requirements (#1203)

* build(deps): update cvxpy version from 1.5.3 to 1.6.5 in requirements

* Add ECOS solver and improve solver handling for stability

Added ECOS to requirements and enhanced compatibility with cvxpy solvers by specifying 'order' for matrix reshaping. Updated solver configurations in rocket landing and pendulum control for consistency and reliability. Improved test behavior by enforcing stricter warning handling in pytest.
---
 .../rocket_powered_landing/rocket_powered_landing.py     | 9 +++++----
 InvertedPendulum/inverted_pendulum_mpc_control.py        | 2 +-
 requirements/requirements.txt                            | 3 ++-
 tests/conftest.py                                        | 2 +-
 4 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
index 239f3629c1..1918dc1cee 100644
--- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
+++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
@@ -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/InvertedPendulum/inverted_pendulum_mpc_control.py b/InvertedPendulum/inverted_pendulum_mpc_control.py
index 9a5fa2ab41..c45dde8acc 100644
--- a/InvertedPendulum/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/requirements/requirements.txt b/requirements/requirements.txt
index f3e6c3dc51..e9c68be7e6 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,7 +1,8 @@
 numpy == 2.2.4
 scipy == 1.15.2
 matplotlib == 3.10.1
-cvxpy == 1.5.3
+cvxpy == 1.6.5
+ecos == 2.0.14
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
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)])

From 1f729cb8cbd5075871bb52c8fb31df2732ad6641 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Thu, 1 May 2025 13:59:15 +0900
Subject: [PATCH 45/84] build(deps): bump ruff from 0.11.6 to 0.11.7 in
 /requirements (#1205)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.6 to 0.11.7.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.6...0.11.7)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.7
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index e9c68be7e6..88cbb21f81 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.11.6 # For unit test
+ruff == 0.11.7 # For unit test

From a2c42c3d6837658edb734e9f16ab64c1bd2662e7 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 1 May 2025 21:53:12 +0900
Subject: [PATCH 46/84] Update Python version to 3.13 across the project
 (#1208)

* Update Python version to 3.13 across the project

Upgraded the required Python version to 3.13 in configurations, CI workflows, documentation, and environment files. This ensures compatibility with the latest Python release and maintains consistency across all project components.

* Update Python version to 3.13 across the project

Upgraded the required Python version to 3.13 in configurations, CI workflows, documentation, and environment files. This ensures compatibility with the latest Python release and maintains consistency across all project components.
---
 .circleci/config.yml                                        | 2 +-
 .github/workflows/Linux_CI.yml                              | 2 +-
 .github/workflows/MacOS_CI.yml                              | 2 +-
 .github/workflows/Windows_CI.yml                            | 2 +-
 README.md                                                   | 2 +-
 appveyor.yml                                                | 2 +-
 docs/modules/0_getting_started/3_how_to_contribute_main.rst | 2 +-
 requirements/environment.yml                                | 2 +-
 ruff.toml                                                   | 4 ++--
 9 files changed, 10 insertions(+), 10 deletions(-)

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/workflows/Linux_CI.yml b/.github/workflows/Linux_CI.yml
index 7b3dc14751..907d36452d 100644
--- a/.github/workflows/Linux_CI.yml
+++ b/.github/workflows/Linux_CI.yml
@@ -12,7 +12,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        python-version: [ '3.12' ]
+        python-version: [ '3.13' ]
 
     name: Python ${{ matrix.python-version }} CI
 
diff --git a/.github/workflows/MacOS_CI.yml b/.github/workflows/MacOS_CI.yml
index 5ea15ac72e..03db65f43d 100644
--- a/.github/workflows/MacOS_CI.yml
+++ b/.github/workflows/MacOS_CI.yml
@@ -16,7 +16,7 @@ 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
diff --git a/.github/workflows/Windows_CI.yml b/.github/workflows/Windows_CI.yml
index b9c8dea649..5cb19699b2 100644
--- a/.github/workflows/Windows_CI.yml
+++ b/.github/workflows/Windows_CI.yml
@@ -16,7 +16,7 @@ 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
diff --git a/README.md b/README.md
index 9e605435ce..6d04c90bc8 100644
--- a/README.md
+++ b/README.md
@@ -102,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/)
  
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/modules/0_getting_started/3_how_to_contribute_main.rst b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
index 30e3001f36..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
@@ -26,7 +26,7 @@ to understand the philosophies of this project.
 Check your Python version.
 ---------------------------
 
-We only accept a PR for Python 3.12.x or higher.
+We only accept a PR for Python 3.13.x or higher.
 
 We will not accept a PR for Python 2.x.
 
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/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]
 

From 22cbee49218a1f8acb07ad488416db3c952c19d6 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 2 May 2025 10:01:19 +0900
Subject: [PATCH 47/84] Standardize "Ref:" to "Reference" across files (#1210)

Updated all instances of "Ref:" to "Reference" for consistency in both code and documentation. This change improves clarity and aligns with standard terminology practices.
---
 .../rocket_powered_landing.py                 |   2 +-
 .../two_joint_arm_to_point_control.py         |   2 +-
 .../ensemble_kalman_filter.py                 |   2 +-
 Mapping/DistanceMap/distance_map.py           |   2 +-
 .../rectangle_fitting/rectangle_fitting.py    |   2 +-
 MissionPlanning/BehaviorTree/behavior_tree.py |   2 +-
 MissionPlanning/StateMachine/state_machine.py |   4 +-
 PathPlanning/ElasticBands/elastic_bands.py    |   2 +-
 .../Eta3SplinePath/eta3_spline_path.py        |   2 +-
 .../cartesian_frenet_converter.py             |   2 +-
 .../frenet_optimal_trajectory.py              |   2 +-
 .../potential_field_planning.py               |   2 +-
 .../quintic_polynomials_planner.py            |   2 +-
 .../state_lattice_planner.py                  |   2 +-
 PathTracking/cgmres_nmpc/cgmres_nmpc.py       |   2 +-
 PathTracking/move_to_pose/move_to_pose.py     |   2 +-
 .../stanley_controller/stanley_controller.py  |   2 +-
 README.md                                     |  42 ++---
 .../Kalmanfilter_basics_2_main.rst            |   2 +-
 .../12_appendix/Kalmanfilter_basics_main.rst  |   2 +-
 .../steering_motion_model_main.rst            |   2 +-
 ...samble_kalman_filter_localization_main.rst |   5 +
 ...tended_kalman_filter_localization_main.rst | 168 +++++++++---------
 .../histogram_filter_localization_main.rst    |   7 +-
 .../particle_filter_localization_main.rst     |   8 +-
 ...cented_kalman_filter_localization_main.rst |   8 +-
 .../modules/4_slam/ekf_slam/ekf_slam_main.rst |   2 +-
 .../4_slam/graph_slam/graph_slam_main.rst     |   2 +-
 .../bezier_path/bezier_path_main.rst          |   2 +-
 .../eta3_spline/eta3_spline_main.rst          |   2 +-
 .../frenet_frame_path_main.rst                |   2 +-
 .../grid_base_search_main.rst                 |   6 +-
 ...l_predictive_trajectory_generator_main.rst |   2 +-
 .../prm_planner/prm_planner_main.rst          |   2 +-
 .../quintic_polynomials_planner_main.rst      |   2 +-
 .../reeds_shepp_path_main.rst                 |   2 +-
 docs/modules/5_path_planning/rrt/rrt_main.rst |   8 +-
 .../state_lattice_planner_main.rst            |   2 +-
 .../vrm_planner/vrm_planner_main.rst          |   2 +-
 .../lqr_speed_and_steering_control_main.rst   |   2 +-
 .../lqr_steering_control_main.rst             |   2 +-
 .../pure_pursuit_tracking_main.rst            |   2 +-
 .../rear_wheel_feedback_control_main.rst      |   2 +-
 .../stanley_control/stanley_control_main.rst  |   2 +-
 users_comments.md                             |   2 +-
 45 files changed, 173 insertions(+), 155 deletions(-)

diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
index 1918dc1cee..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.
 
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/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
index 0dcc7380c5..0f96c9e8c6 100644
--- a/Mapping/DistanceMap/distance_map.py
+++ b/Mapping/DistanceMap/distance_map.py
@@ -3,7 +3,7 @@
 
 author: Wang Zheng (@Aglargil)
 
-Ref:
+Reference:
 
 - [Distance Map]
 (https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf)
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
index 59f4c713f1..9ad886aafb 100644
--- a/MissionPlanning/BehaviorTree/behavior_tree.py
+++ b/MissionPlanning/BehaviorTree/behavior_tree.py
@@ -3,7 +3,7 @@
 
 author: Wang Zheng (@Aglargil)
 
-Ref:
+Reference:
 
 - [Behavior Tree](https://en.wikipedia.org/wiki/Behavior_tree_(artificial_intelligence,_robotics_and_control))
 """
diff --git a/MissionPlanning/StateMachine/state_machine.py b/MissionPlanning/StateMachine/state_machine.py
index de72f0f451..454759236e 100644
--- a/MissionPlanning/StateMachine/state_machine.py
+++ b/MissionPlanning/StateMachine/state_machine.py
@@ -3,7 +3,7 @@
 
 author: Wang Zheng (@Aglargil)
 
-Ref:
+Reference:
 
 - [State Machine]
 (https://en.wikipedia.org/wiki/Finite-state_machine)
@@ -23,7 +23,7 @@ def deflate_and_encode(plantuml_text):
     """
     zlib compress the plantuml text and encode it for the plantuml server.
 
-    Ref: https://plantuml.com/en/text-encoding
+    Reference https://plantuml.com/en/text-encoding
     """
     plantuml_alphabet = (
         string.digits + string.ascii_uppercase + string.ascii_lowercase + "-_"
diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py
index 785f822d14..77d4e6e399 100644
--- a/PathPlanning/ElasticBands/elastic_bands.py
+++ b/PathPlanning/ElasticBands/elastic_bands.py
@@ -3,7 +3,7 @@
 
 author: Wang Zheng (@Aglargil)
 
-Ref:
+Reference:
 
 - [Elastic Bands: Connecting Path Planning and Control]
 (http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf)
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/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/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
index 7f8e725e0a..05f8df78f8 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
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/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index 34736a2e21..faf1264953 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -76,7 +76,7 @@ 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
 
-        # Ref: The velocity v always has a constant sign which depends on the initial value of α.
+        # The velocity v always has a constant sign which depends on the initial value of α.
         rho = np.hypot(x_diff, y_diff)
         v = self.Kp_rho * rho
 
diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py
index bc98175f17..01c2ec0229 100644
--- a/PathTracking/stanley_controller/stanley_controller.py
+++ b/PathTracking/stanley_controller/stanley_controller.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)
 
diff --git a/README.md b/README.md
index 6d04c90bc8..05fd0262df 100644
--- a/README.md
+++ b/README.md
@@ -168,7 +168,7 @@ 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)
 
@@ -186,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/)
 
@@ -207,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/)
 
@@ -256,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)
 
@@ -275,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/)
 
@@ -321,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*)
 
@@ -346,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)
 
@@ -362,7 +362,7 @@ 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)
 
@@ -390,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)
 
@@ -406,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)
 
@@ -426,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)
 
@@ -441,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/)
 
@@ -451,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) 
 
@@ -477,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)
 
@@ -492,7 +492,7 @@ This is a simulation of moving to a pose control
 
 ![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/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)
 
@@ -503,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)
 
@@ -517,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)
 
@@ -528,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/)
 
@@ -539,7 +539,7 @@ 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)
 
@@ -551,7 +551,7 @@ 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)
 
@@ -591,7 +591,7 @@ 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)
 
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/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/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/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/4_slam/ekf_slam/ekf_slam_main.rst b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
index b27971225e..a1486ffe1e 100644
--- a/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
+++ b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
@@ -578,7 +578,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/graph_slam_main.rst b/docs/modules/4_slam/graph_slam/graph_slam_main.rst
index 987eed385c..a2ef513527 100644
--- a/docs/modules/4_slam/graph_slam/graph_slam_main.rst
+++ b/docs/modules/4_slam/graph_slam/graph_slam_main.rst
@@ -17,7 +17,7 @@ The black stars are landmarks for graph edge generation.
 .. 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/5_path_planning/bezier_path/bezier_path_main.rst b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
index d306a85352..9d9b3de709 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,7 +13,7 @@ You can get different Beizer course:
 
 .. image:: Figure_2.png
 
-Ref:
+Reference
 
 -  `Continuous Curvature Path Generation Based on Bezier Curves for
    Autonomous
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..82e0a71044 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,7 @@ Eta^3 Spline path planning
 
 This is a path planning with Eta^3 spline.
 
-Ref:
+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..371d536e3f 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
@@ -35,7 +35,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..bf82c9f391 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
@@ -63,7 +63,7 @@ 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:
+Reference
 
 -  `D* search Wikipedia <https://en.wikipedia.org/wiki/D*>`__
 
@@ -74,7 +74,7 @@ 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:
+Reference
 
 - `Improved Fast Replanning for Robot Navigation in Unknown Terrain <http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf>`_
 
@@ -88,7 +88,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>`__
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..1c5df1c9cc 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
@@ -16,7 +16,7 @@ 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/prm_planner/prm_planner_main.rst b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
index 0628719176..f816c5c1b9 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,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>`__
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..66c3001c05 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
@@ -97,7 +97,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..a4a5d28e01 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
@@ -380,7 +380,7 @@ 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/rrt/rrt_main.rst b/docs/modules/5_path_planning/rrt/rrt_main.rst
index e5f351a7ba..da3a4957a5 100644
--- a/docs/modules/5_path_planning/rrt/rrt_main.rst
+++ b/docs/modules/5_path_planning/rrt/rrt_main.rst
@@ -53,7 +53,7 @@ This is a path planning code with Informed RRT*.
 
 The cyan ellipse is the heuristic sampling domain of Informed RRT*.
 
-Ref:
+Reference
 
 -  `Informed RRT\*: Optimal Sampling-based Path Planning Focused via
    Direct Sampling of an Admissible Ellipsoidal
@@ -68,7 +68,7 @@ Batch Informed RRT\*
 
 This is a path planning code with Batch Informed RRT*.
 
-Ref:
+Reference
 
 -  `Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the
    Heuristically Guided Search of Implicit Random Geometric
@@ -87,7 +87,7 @@ In this code, pure-pursuit algorithm is used for steering control,
 
 PID is used for speed control.
 
-Ref:
+Reference
 
 -  `Motion Planning in Complex Environments using Closed-loop
    Prediction <https://acl.mit.edu/papers/KuwataGNC08.pdf>`__
@@ -109,7 +109,7 @@ A double integrator motion model is used for LQR local planner.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif
 
-Ref:
+Reference
 
 -  `LQR-RRT\*: Optimal Sampling-Based Motion Planning with Automatically
    Derived Extension
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..bf89ac11ae 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
@@ -22,7 +22,7 @@ Lane sampling
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif
 
-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/vrm_planner/vrm_planner_main.rst b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
index 92e729ab29..65e0e53465 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,7 @@ Cyan crosses mean searched points with Dijkstra method,
 
 The red line is the final path of Vornoi Road-Map.
 
-Ref:
+Reference
 
 -  `Robotic Motion Planning <https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf>`__
 
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..568ef9a0df 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
@@ -134,7 +134,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..831423f364 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
@@ -113,7 +113,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/pure_pursuit_tracking/pure_pursuit_tracking_main.rst b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
index 5c7bcef85f..d7354cf8fb 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,7 @@ 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:
+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..d18cd6fbf7 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,7 @@ PID speed control.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif
 
-References:
+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..69089ac33b 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,7 @@ control.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif
 
-References:
+Reference
 ~~~~~~~~~~~
 
 -  `Stanley: The robot that won the DARPA grand
diff --git a/users_comments.md b/users_comments.md
index a2f798eac4..1674f21377 100644
--- a/users_comments.md
+++ b/users_comments.md
@@ -10,7 +10,7 @@ This is an electric wheelchair control demo by [Katsushun89](https://github.com/
 
 ![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)
 

From 5392fcff4d555421b4417827dc67c611ad14cc06 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sat, 3 May 2025 09:25:12 +0900
Subject: [PATCH 48/84] Refactor module links and improve code documentation.
 (#1211)

* Refactor module links and improve code documentation.

Updated documentation to rename "API" sections to "Code Link" for clarity and consistency. Enhanced docstrings for `circle_fitting` and `kmeans_clustering` functions, improving parameter descriptions and adding return value details. Fixed typos in function and file names in the ray casting grid map module.

* Fix import typo in ray casting grid map test module.

Corrected the import statement in the test file by updating the module's name to `ray_casting_grid_map` for consistency with the source file. This ensures proper functionality of the test suite.
---
 Mapping/circle_fitting/circle_fitting.py      | 33 +++++++++++++++----
 .../kmeans_clustering/kmeans_clustering.py    | 27 ++++++++++++++-
 .../ray_casting_grid_map.py}                  |  4 +--
 .../circle_fitting/circle_fitting_main.rst    |  4 +++
 .../distance_map/distance_map_main.rst        |  4 +--
 .../gaussian_grid_map_main.rst                |  6 ++++
 .../k_means_object_clustering_main.rst        |  6 ++++
 .../lidar_to_grid_map_tutorial_main.rst       |  6 ++++
 .../normal_vector_estimation_main.rst         |  8 ++---
 .../point_cloud_sampling_main.rst             |  8 ++---
 .../ray_casting_grid_map_main.rst             |  7 +++-
 .../rectangle_fitting_main.rst                |  4 +--
 ...id_map.py => test_ray_casting_grid_map.py} |  2 +-
 13 files changed, 96 insertions(+), 23 deletions(-)
 rename Mapping/{raycasting_grid_map/raycasting_grid_map.py => ray_casting_grid_map/ray_casting_grid_map.py} (96%)
 rename tests/{test_raycasting_grid_map.py => test_ray_casting_grid_map.py} (71%)

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/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_main.rst b/docs/modules/3_mapping/distance_map/distance_map_main.rst
index 0ef9e3022f..ec60e752c9 100644
--- a/docs/modules/3_mapping/distance_map/distance_map_main.rst
+++ b/docs/modules/3_mapping/distance_map/distance_map_main.rst
@@ -14,8 +14,8 @@ The algorithm is demonstrated on a simple 2D grid with obstacles:
 
 .. image:: distance_map.png
 
-API
-~~~
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 .. autofunction:: Mapping.DistanceMap.distance_map.compute_sdf
 
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/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/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():

From e1cdb24ecfd68ff4eaf51626e5a3f4854da588a4 Mon Sep 17 00:00:00 2001
From: Yuri Harada <55676955+NightzDev@users.noreply.github.com>
Date: Fri, 2 May 2025 21:51:20 -0300
Subject: [PATCH 49/84] docs: rewrite internal sensors section with detailed
 descriptions and references (#1197)

* docs: rewrite and enhance internal sensors documentation

* Update docs/modules/12_appendix/internal_sensors_main.rst

---------

Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>
---
 .../12_appendix/internal_sensors_main.rst     | 41 +++++++++++++++----
 1 file changed, 34 insertions(+), 7 deletions(-)

diff --git a/docs/modules/12_appendix/internal_sensors_main.rst b/docs/modules/12_appendix/internal_sensors_main.rst
index 18f209098e..fa2594a2bf 100644
--- a/docs/modules/12_appendix/internal_sensors_main.rst
+++ b/docs/modules/12_appendix/internal_sensors_main.rst
@@ -3,32 +3,59 @@
 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.
+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

From d2fe5ae8f0e414f394c00570771c07807d167e68 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 4 May 2025 09:50:49 +0900
Subject: [PATCH 50/84] Add code links to SLAM module documentation (#1212)

This commit updates SLAM module documentation files to include direct code links for better navigation. It also adjusts imports in `test_iterative_closest_point.py` to reflect updated module structure and adds a safety check for the directory in `runtests.sh`.
---
 .../icp_matching.py}                                      | 0
 docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst          | 5 +++++
 docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst          | 6 ++++++
 docs/modules/4_slam/ekf_slam/ekf_slam_main.rst            | 6 ++++++
 docs/modules/4_slam/graph_slam/graph_slam_main.rst        | 6 ++++++
 .../iterative_closest_point_matching_main.rst             | 8 +++++++-
 runtests.sh                                               | 1 +
 tests/test_iterative_closest_point.py                     | 2 +-
 8 files changed, 32 insertions(+), 2 deletions(-)
 rename SLAM/{iterative_closest_point/iterative_closest_point.py => ICPMatching/icp_matching.py} (100%)

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/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 a1486ffe1e..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
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
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 a2ef513527..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,6 +13,12 @@ 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
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/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/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():

From a38da41bafaf82bb56d4491d94a0095c1019ac4a Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 4 May 2025 20:32:11 +0900
Subject: [PATCH 51/84] Add code links to documentation and fix naming
 inconsistencies (#1213)

Added references to related Python functions in documentation for better navigation and usability. Corrected inconsistencies in module and test names to align with their respective directories and improve clarity.
---
 .../rear_wheel_feedback_control.py}                 |  0
 .../stanley_control.py}                             |  0
 .../10_inverted_pendulum/inverted_pendulum_main.rst | 12 ++++++++++++
 .../behavior_tree/behavior_tree_main.rst            |  2 +-
 .../state_machine/state_machine_main.rst            |  4 ++--
 .../cgmres_nmpc/cgmres_nmpc_main.rst                |  5 +++++
 .../lqr_speed_and_steering_control_main.rst         |  7 ++++++-
 .../lqr_steering_control_main.rst                   |  6 +++++-
 ...l_predictive_speed_and_steering_control_main.rst | 13 ++++++-------
 .../move_to_a_pose_main.rst}                        |  8 +++++++-
 docs/modules/6_path_tracking/path_tracking_main.rst |  2 +-
 .../pure_pursuit_tracking_main.rst                  |  6 ++++++
 .../rear_wheel_feedback_control_main.rst            |  6 ++++++
 .../stanley_control/stanley_control_main.rst        |  6 ++++++
 .../n_joint_arm_to_point_control_main.rst           |  7 +++++++
 .../obstacle_avoidance_arm_navigation_main.rst      |  7 ++++++-
 .../7_arm_navigation/planar_two_link_ik_main.rst    |  6 ++++++
 .../drone_3d_trajectory_following_main.rst          |  5 +++++
 .../rocket_powered_landing_main.rst                 |  8 ++++++++
 .../bipedal_planner/bipedal_planner_main.rst        |  5 +++++
 tests/test_rear_wheel_feedback.py                   |  2 +-
 tests/test_stanley_controller.py                    |  2 +-
 22 files changed, 102 insertions(+), 17 deletions(-)
 rename PathTracking/{rear_wheel_feedback/rear_wheel_feedback.py => rear_wheel_feedback_control/rear_wheel_feedback_control.py} (100%)
 rename PathTracking/{stanley_controller/stanley_controller.py => stanley_control/stanley_control.py} (100%)
 rename docs/modules/6_path_tracking/{move_to_a_pose_control/move_to_a_pose_control_main.rst => move_to_a_pose/move_to_a_pose_main.rst} (97%)

diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
similarity index 100%
rename from PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
rename to PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_control/stanley_control.py
similarity index 100%
rename from PathTracking/stanley_controller/stanley_controller.py
rename to PathTracking/stanley_control/stanley_control.py
diff --git a/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
index 048cbea9ac..58dc0f2e57 100644
--- a/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
+++ b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
@@ -89,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:
@@ -101,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/13_mission_planning/behavior_tree/behavior_tree_main.rst b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
index ae3e16da81..22849f7c54 100644
--- a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
+++ b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
@@ -5,7 +5,7 @@ Behavior Tree is a modular, hierarchical decision model that is widely used in r
 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)
 
-Core Concepts
+Code Link
 ~~~~~~~~~~~~~
 
 Control Node
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
index abaece1b11..3f516d46a9 100644
--- a/docs/modules/13_mission_planning/state_machine/state_machine_main.rst
+++ b/docs/modules/13_mission_planning/state_machine/state_machine_main.rst
@@ -12,8 +12,8 @@ Core Concepts
 - **Action**: An operation executed during transition (before entering new state)
 - **Guard**: A precondition that must be satisfied to allow transition
 
-API
-~~~
+Code Link
+~~~~~~~~~~~
 
 .. autoclass:: MissionPlanning.StateMachine.state_machine.StateMachine
    :members: add_transition, process, register_state
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 568ef9a0df..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
 ~~~~~~~~
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 831423f364..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
 ~~~~~~~~
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/6_path_tracking/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/6_path_tracking/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/6_path_tracking/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 d98e324583..742cc807e6 100644
--- a/docs/modules/6_path_tracking/path_tracking_main.rst
+++ b/docs/modules/6_path_tracking/path_tracking_main.rst
@@ -16,4 +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_control/move_to_a_pose_control
+   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 d7354cf8fb..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,6 +9,12 @@ 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.
 
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.pure_pursuit.pure_pursuit.pure_pursuit_steer_control
+
+
 Reference
 ~~~~~~~~~~~
 
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 d18cd6fbf7..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,6 +6,12 @@ PID speed control.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif
 
+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
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 69089ac33b..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,6 +6,12 @@ control.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif
 
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.stanley_control.stanley_control.stanley_control
+
+
 Reference
 ~~~~~~~~~~~
 
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/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_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/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_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():

From 73e1c0bebccf4f4fc67ae59ada53b783a8533096 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Mon, 5 May 2025 17:29:45 +0900
Subject: [PATCH 52/84] Add "Code Link" sections and rename classes for
 consistency (#1214)

This commit adds "Code Link" sections to documentation across various path planning modules, linking to relevant class and function APIs. Additionally, several class renaming changes were made, such as `Dijkstra` to `DijkstraPlanner` and `eta3_trajectory` to `Eta3SplineTrajectory`, to enhance naming consistency. Minor fixes include file restructuring and image renaming for the RRT module.
---
 ..._rrtstar.py => batch_informed_rrt_star.py} |   0
 PathPlanning/Dijkstra/dijkstra.py             |   4 +-
 .../eta3_spline_trajectory.py                 |  14 ++---
 .../bezier_path/bezier_path_main.rst          |   9 ++-
 .../bspline_path/bspline_path_main.rst        |   8 +--
 .../bugplanner/bugplanner_main.rst            |   8 +++
 .../catmull_rom_spline_main.rst               |   4 +-
 .../clothoid_path/clothoid_path_main.rst      |   5 ++
 .../coverage_path/coverage_path_main.rst      |  21 +++++++
 .../cubic_spline/cubic_spline_main.rst        |   8 +--
 .../dubins_path/dubins_path_main.rst          |   2 +-
 .../dynamic_window_approach_main.rst          |  12 +++-
 .../elastic_bands/elastic_bands_main.rst      |   7 ++-
 .../eta3_spline/eta3_spline_main.rst          |   7 +++
 .../frenet_frame_path_main.rst                |   6 ++
 .../grid_base_search_main.rst                 |  50 ++++++++++++++++
 .../hybridastar/hybridastar_main.rst          |   5 ++
 .../lqr_path/lqr_path_main.rst                |   5 ++
 ...l_predictive_trajectory_generator_main.rst |   7 +++
 .../prm_planner/prm_planner_main.rst          |   7 +++
 .../quintic_polynomials_planner_main.rst      |   5 ++
 .../reeds_shepp_path_main.rst                 |   7 +++
 .../closed_loop_rrt_star_car/Figure_1.png     | Bin
 .../closed_loop_rrt_star_car/Figure_3.png     | Bin
 .../closed_loop_rrt_star_car/Figure_4.png     | Bin
 .../closed_loop_rrt_star_car/Figure_5.png     | Bin
 docs/modules/5_path_planning/rrt/rrt_main.rst |  54 ++++++++++++++++++
 docs/modules/5_path_planning/rrt/rrt_star.rst |   6 ++
 .../state_lattice_planner_main.rst            |  17 ++++++
 .../time_based_grid_search_main.rst           |   9 +++
 .../visibility_road_map_planner_main.rst      |   7 ++-
 .../vrm_planner/vrm_planner_main.rst          |   6 ++
 tests/test_batch_informed_rrt_star.py         |   2 +-
 33 files changed, 277 insertions(+), 25 deletions(-)
 rename PathPlanning/BatchInformedRRTStar/{batch_informed_rrtstar.py => batch_informed_rrt_star.py} (100%)
 rename docs/modules/5_path_planning/{ => rrt}/closed_loop_rrt_star_car/Figure_1.png (100%)
 rename docs/modules/5_path_planning/{ => rrt}/closed_loop_rrt_star_car/Figure_3.png (100%)
 rename docs/modules/5_path_planning/{ => rrt}/closed_loop_rrt_star_car/Figure_4.png (100%)
 rename docs/modules/5_path_planning/{ => rrt}/closed_loop_rrt_star_car/Figure_5.png (100%)

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/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/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/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
index 9d9b3de709..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
 
+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
index 8a3e517105..d0109d4ec3 100644
--- a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
+++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
@@ -5,6 +5,11 @@ 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
 ~~~~~~~~~~~~
@@ -69,6 +74,6 @@ Dynamic Path Maintenance
 - 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 82e0a71044..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.
 
+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 371d536e3f..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
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
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 bf82c9f391..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.
 
+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
 
+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.
 
+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 1c5df1c9cc..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
 ~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -17,6 +23,7 @@ Lookup table generation sample
 .. image:: lookup_table.png
 
 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/prm_planner/prm_planner_main.rst b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
index f816c5c1b9..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.
 
+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 66c3001c05..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
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 a4a5d28e01..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.
@@ -381,6 +387,7 @@ Hence, we have:
 
 
 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 da3a4957a5..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*.
 
+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*.
 
+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.
 
+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 @@ Reference
 -  `[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
 
+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 bf89ac11ae..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
 
+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
index 04001d4504..9517e95b31 100644
--- 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
@@ -33,6 +33,10 @@ After::
 
 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
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -73,6 +77,11 @@ Arrangement 1 starting at (1, 18)::
 
 .. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/SafeIntervalPathPlanner/path_animation2.gif
 
+Code Link
+^^^^^^^^^^^^^
+.. autoclass:: PathPlanning.TimeBasedPathPlanning.SafeInterval.SafeIntervalPathPlanner
+
+
 References
 ~~~~~~~~~~~
 
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 65e0e53465..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.
 
+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/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():

From 0c82dde2be24e5f3955018caa061750d7404978d Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 6 May 2025 07:22:51 +0900
Subject: [PATCH 53/84] build(deps): bump ruff from 0.11.7 to 0.11.8 in
 /requirements (#1215)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.7 to 0.11.8.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.7...0.11.8)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.8
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 88cbb21f81..e78b9b8d51 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.11.7 # For unit test
+ruff == 0.11.8 # For unit test

From 292c9dc613d1b0ace3d25ca8a0295e19549516f2 Mon Sep 17 00:00:00 2001
From: atm <72185056+sutatoruta@users.noreply.github.com>
Date: Sat, 10 May 2025 09:34:19 +0900
Subject: [PATCH 54/84] fix broken links (#1216)

---
 .github/pull_request_template.md |  2 +-
 README.md                        | 12 ++++++------
 users_comments.md                |  2 +-
 3 files changed, 8 insertions(+), 8 deletions(-)

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/README.md b/README.md
index 05fd0262df..85be274adb 100644
--- a/README.md
+++ b/README.md
@@ -87,7 +87,7 @@ 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:
 
@@ -170,7 +170,7 @@ All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation
 
 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
 
@@ -490,7 +490,7 @@ Reference
 
 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)
 
 Reference
 
@@ -541,7 +541,7 @@ Path tracking simulation with iterative linear model predictive speed and steeri
 
 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)
 
@@ -553,7 +553,7 @@ A motion planning and path tracking simulation with NMPC of C-GMRES
 
 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
@@ -593,7 +593,7 @@ This is a 3d trajectory generation simulation for a rocket powered landing.
 
 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
 
diff --git a/users_comments.md b/users_comments.md
index 1674f21377..30dc2d2d85 100644
--- a/users_comments.md
+++ b/users_comments.md
@@ -6,7 +6,7 @@ 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)
 

From cfaab296a4212948333f6316e98575ef904a0932 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 13 May 2025 21:55:01 +0900
Subject: [PATCH 55/84] build(deps): bump ruff from 0.11.8 to 0.11.9 in
 /requirements (#1218)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.8 to 0.11.9.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.8...0.11.9)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.9
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index e78b9b8d51..32a9b31938 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.11.8 # For unit test
+ruff == 0.11.9 # For unit test

From c9051758e74896d35c633d506c1df59d49220463 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 20 May 2025 22:32:30 +0900
Subject: [PATCH 56/84] build(deps): bump ruff from 0.11.9 to 0.11.10 in
 /requirements (#1220)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.9 to 0.11.10.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.9...0.11.10)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.10
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 32a9b31938..4463aacb36 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.11.9 # For unit test
+ruff == 0.11.10 # For unit test

From 453c25e461a0cda5c8a572c388307b41c92759a2 Mon Sep 17 00:00:00 2001
From: Yutharsan <yutharsan.22@cse.mrt.ac.lk>
Date: Sun, 25 May 2025 10:27:02 +0530
Subject: [PATCH 57/84] Add root directory to system path for lattice planner
 (#1221)

---
 PathPlanning/StateLatticePlanner/state_lattice_planner.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
index 05f8df78f8..554cd0f3b7 100644
--- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py
+++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
@@ -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

From bdf50dffcdc72d923b5e3d2e25be5e237d0849f6 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 27 May 2025 21:36:36 +0900
Subject: [PATCH 58/84] build(deps): bump
 larsoner/circleci-artifacts-redirector-action (#1224)

Bumps [larsoner/circleci-artifacts-redirector-action](https://github.com/larsoner/circleci-artifacts-redirector-action) from 1.0.0 to 1.1.0.
- [Release notes](https://github.com/larsoner/circleci-artifacts-redirector-action/releases)
- [Commits](https://github.com/larsoner/circleci-artifacts-redirector-action/compare/v1.0.0...v1.1.0)

---
updated-dependencies:
- dependency-name: larsoner/circleci-artifacts-redirector-action
  dependency-version: 1.1.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 .github/workflows/circleci-artifacts-redirector.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/circleci-artifacts-redirector.yml b/.github/workflows/circleci-artifacts-redirector.yml
index 78eb479aa1..692942cddd 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.1.0
         with:
           repo-token: ${{ secrets.GITHUB_TOKEN }}
           api-token: ${{ secrets.CIRCLECI_TOKEN }}

From 74610fd56bfe7eee02054c48afaa7328bec0d6b7 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 27 May 2025 21:36:59 +0900
Subject: [PATCH 59/84] build(deps): bump ruff from 0.11.10 to 0.11.11 in
 /requirements (#1225)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.10 to 0.11.11.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.10...0.11.11)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.11
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 4463aacb36..cc15d4e335 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.3.5 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.15.0 # For unit test
-ruff == 0.11.10 # For unit test
+ruff == 0.11.11 # For unit test

From 26264a343269cbde9f4d3c6d9016684ed4f32d5c Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 3 Jun 2025 07:32:07 +0900
Subject: [PATCH 60/84] build(deps): bump pytest-xdist from 3.6.1 to 3.7.0 in
 /requirements (#1226)

Bumps [pytest-xdist](https://github.com/pytest-dev/pytest-xdist) from 3.6.1 to 3.7.0.
- [Release notes](https://github.com/pytest-dev/pytest-xdist/releases)
- [Changelog](https://github.com/pytest-dev/pytest-xdist/blob/master/CHANGELOG.rst)
- [Commits](https://github.com/pytest-dev/pytest-xdist/compare/v3.6.1...v3.7.0)

---
updated-dependencies:
- dependency-name: pytest-xdist
  dependency-version: 3.7.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index cc15d4e335..78ca18f42d 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -4,6 +4,6 @@ matplotlib == 3.10.1
 cvxpy == 1.6.5
 ecos == 2.0.14
 pytest == 8.3.5 # For unit test
-pytest-xdist == 3.6.1 # For unit test
+pytest-xdist == 3.7.0 # For unit test
 mypy == 1.15.0 # For unit test
 ruff == 0.11.11 # For unit test

From 4b4d377a541656f6f063c05cbc5e8f0883ad3fd3 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 3 Jun 2025 09:26:11 +0900
Subject: [PATCH 61/84] build(deps): bump pytest from 8.3.5 to 8.4.0 in
 /requirements (#1228)

Bumps [pytest](https://github.com/pytest-dev/pytest) from 8.3.5 to 8.4.0.
- [Release notes](https://github.com/pytest-dev/pytest/releases)
- [Changelog](https://github.com/pytest-dev/pytest/blob/main/CHANGELOG.rst)
- [Commits](https://github.com/pytest-dev/pytest/compare/8.3.5...8.4.0)

---
updated-dependencies:
- dependency-name: pytest
  dependency-version: 8.4.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 78ca18f42d..5a21fd2b27 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -3,7 +3,7 @@ scipy == 1.15.2
 matplotlib == 3.10.1
 cvxpy == 1.6.5
 ecos == 2.0.14
-pytest == 8.3.5 # For unit test
+pytest == 8.4.0 # For unit test
 pytest-xdist == 3.7.0 # For unit test
 mypy == 1.15.0 # For unit test
 ruff == 0.11.11 # For unit test

From eee0031eb862e22f5d108d32a023327d87ededec Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 3 Jun 2025 23:11:13 +0900
Subject: [PATCH 62/84] build(deps): bump mypy from 1.15.0 to 1.16.0 in
 /requirements (#1227)

Bumps [mypy](https://github.com/python/mypy) from 1.15.0 to 1.16.0.
- [Changelog](https://github.com/python/mypy/blob/master/CHANGELOG.md)
- [Commits](https://github.com/python/mypy/compare/v1.15.0...v1.16.0)

---
updated-dependencies:
- dependency-name: mypy
  dependency-version: 1.16.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 5a21fd2b27..f154142331 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,5 +5,5 @@ cvxpy == 1.6.5
 ecos == 2.0.14
 pytest == 8.4.0 # For unit test
 pytest-xdist == 3.7.0 # For unit test
-mypy == 1.15.0 # For unit test
+mypy == 1.16.0 # For unit test
 ruff == 0.11.11 # For unit test

From ef8c154c73a562f9a90857559e4f380df6968893 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Wed, 4 Jun 2025 12:59:20 +0900
Subject: [PATCH 63/84] build(deps): bump ruff from 0.11.11 to 0.11.12 in
 /requirements (#1229)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.11 to 0.11.12.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.11...0.11.12)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.12
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index f154142331..0c0719cca5 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.4.0 # For unit test
 pytest-xdist == 3.7.0 # For unit test
 mypy == 1.16.0 # For unit test
-ruff == 0.11.11 # For unit test
+ruff == 0.11.12 # For unit test

From a8a45033c6daffcb6e5ccc8b02727d407314ee6a Mon Sep 17 00:00:00 2001
From: Zichao Yang <yangtzech@qq.com>
Date: Sat, 7 Jun 2025 19:05:32 +0800
Subject: [PATCH 64/84] feat: Add reverse mode in pure pursuit (#1194)

* feat: Add vehicle model rendering function and pause simulation function

* feat: Add support for reverse mode and related function adjustments

* feat: Add reverse mode test case

* feat: Limit the maximum steering angle to improve path tracking control

* feat: Add a newline for improved readability in plot_arrow function

* fix: Replace max/min with np.clip for steering angle limitation in pure pursuit control
---
 PathTracking/pure_pursuit/pure_pursuit.py | 168 +++++++++++++++++++---
 tests/test_pure_pursuit.py                |   4 +
 2 files changed, 155 insertions(+), 17 deletions(-)

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/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__)

From 4ffd8e76e3429daaeb85a2db0764e7af71604af3 Mon Sep 17 00:00:00 2001
From: atm <72185056+sutatoruta@users.noreply.github.com>
Date: Sat, 7 Jun 2025 20:07:06 +0900
Subject: [PATCH 65/84] Optimize is_obstacle using set (#1223)

---
 PathPlanning/DStarLite/d_star_lite.py | 36 ++++++---------------------
 1 file changed, 7 insertions(+), 29 deletions(-)

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)

From ed02c96abd15f39fbad2bc092c6b46292c0610b4 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Wed, 11 Jun 2025 12:18:34 +0900
Subject: [PATCH 66/84] build(deps): bump ruff from 0.11.12 to 0.11.13 in
 /requirements (#1233)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.12 to 0.11.13.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.12...0.11.13)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.11.13
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 0c0719cca5..5f8bf52415 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.4.0 # For unit test
 pytest-xdist == 3.7.0 # For unit test
 mypy == 1.16.0 # For unit test
-ruff == 0.11.12 # For unit test
+ruff == 0.11.13 # For unit test

From 194be251e1e62faf94b47f6500444f6f5c698dd8 Mon Sep 17 00:00:00 2001
From: Rg Zhong <35895359+phonixer@users.noreply.github.com>
Date: Sun, 15 Jun 2025 21:22:10 +0800
Subject: [PATCH 67/84] Update README.md (#1235)

The link is invalid and needs to be updated.
---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 85be274adb..d1b801f219 100644
--- a/README.md
+++ b/README.md
@@ -366,7 +366,7 @@ 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

From 0e93ecb66957c5ff6642252c6ca3290b0955d63e Mon Sep 17 00:00:00 2001
From: Shibo Liu <78792689+ShiboWork@users.noreply.github.com>
Date: Sat, 21 Jun 2025 07:04:57 +0800
Subject: [PATCH 68/84] Fix/path smoothing robot radius (#1231)

* Fix: Include robot_radius in path_smoothing collision check

* Test: Add unit test to verify smoothed path respects robot_radius
---
 PathPlanning/RRT/rrt_with_pathsmoothing.py  | 121 +++++++++++++++-----
 tests/test_rrt_with_pathsmoothing_radius.py |  48 ++++++++
 2 files changed, 141 insertions(+), 28 deletions(-)
 create mode 100644 tests/test_rrt_with_pathsmoothing_radius.py

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/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__)

From fb667d3d7e635c1b6c6ca13a1b40e8ae623e15a0 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sat, 21 Jun 2025 19:01:21 +0900
Subject: [PATCH 69/84] build(deps): bump cvxpy from 1.6.5 to 1.6.6 in
 /requirements (#1238)

Bumps [cvxpy](https://github.com/cvxpy/cvxpy) from 1.6.5 to 1.6.6.
- [Release notes](https://github.com/cvxpy/cvxpy/releases)
- [Commits](https://github.com/cvxpy/cvxpy/compare/v1.6.5...v1.6.6)

---
updated-dependencies:
- dependency-name: cvxpy
  dependency-version: 1.6.6
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 5f8bf52415..98838b4f68 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,7 +1,7 @@
 numpy == 2.2.4
 scipy == 1.15.2
 matplotlib == 3.10.1
-cvxpy == 1.6.5
+cvxpy == 1.6.6
 ecos == 2.0.14
 pytest == 8.4.0 # For unit test
 pytest-xdist == 3.7.0 # For unit test

From e34b90081f05bd92041415593939e819633e7f6d Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sat, 21 Jun 2025 19:01:52 +0900
Subject: [PATCH 70/84] build(deps): bump mypy from 1.16.0 to 1.16.1 in
 /requirements (#1237)

Bumps [mypy](https://github.com/python/mypy) from 1.16.0 to 1.16.1.
- [Changelog](https://github.com/python/mypy/blob/master/CHANGELOG.md)
- [Commits](https://github.com/python/mypy/compare/v1.16.0...v1.16.1)

---
updated-dependencies:
- dependency-name: mypy
  dependency-version: 1.16.1
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 98838b4f68..b61c51850e 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,5 +5,5 @@ cvxpy == 1.6.6
 ecos == 2.0.14
 pytest == 8.4.0 # For unit test
 pytest-xdist == 3.7.0 # For unit test
-mypy == 1.16.0 # For unit test
+mypy == 1.16.1 # For unit test
 ruff == 0.11.13 # For unit test

From b537376c7619b8ab487e6d1169b4f304e600b358 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 24 Jun 2025 19:46:10 +0900
Subject: [PATCH 71/84] build(deps): bump ruff from 0.11.13 to 0.12.0 in
 /requirements (#1239)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.11.13 to 0.12.0.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.11.13...0.12.0)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.12.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index b61c51850e..ce89e2f2db 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.4.0 # For unit test
 pytest-xdist == 3.7.0 # For unit test
 mypy == 1.16.1 # For unit test
-ruff == 0.11.13 # For unit test
+ruff == 0.12.0 # For unit test

From c50abb38c89451f07ced8687a488105b85064b31 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Fri, 27 Jun 2025 22:10:29 +0900
Subject: [PATCH 72/84] build(deps): bump pytest from 8.4.0 to 8.4.1 in
 /requirements (#1240)

Bumps [pytest](https://github.com/pytest-dev/pytest) from 8.4.0 to 8.4.1.
- [Release notes](https://github.com/pytest-dev/pytest/releases)
- [Changelog](https://github.com/pytest-dev/pytest/blob/main/CHANGELOG.rst)
- [Commits](https://github.com/pytest-dev/pytest/compare/8.4.0...8.4.1)

---
updated-dependencies:
- dependency-name: pytest
  dependency-version: 8.4.1
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index ce89e2f2db..30458e03d1 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -3,7 +3,7 @@ scipy == 1.15.2
 matplotlib == 3.10.1
 cvxpy == 1.6.6
 ecos == 2.0.14
-pytest == 8.4.0 # For unit test
+pytest == 8.4.1 # For unit test
 pytest-xdist == 3.7.0 # For unit test
 mypy == 1.16.1 # For unit test
 ruff == 0.12.0 # For unit test

From e8c6a0f2d81d96e503a7924beadc174b7e73f470 Mon Sep 17 00:00:00 2001
From: Viral Panchal <53462443+Viralpanchal7899@users.noreply.github.com>
Date: Fri, 27 Jun 2025 09:12:20 -0400
Subject: [PATCH 73/84] Enhance external sensors documentation.  (#1236)

* add: external sensors documentation

* add a line
---
 docs/modules/12_appendix/external_sensors_main.rst | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/docs/modules/12_appendix/external_sensors_main.rst b/docs/modules/12_appendix/external_sensors_main.rst
index 3597418150..b7caaf2c3a 100644
--- a/docs/modules/12_appendix/external_sensors_main.rst
+++ b/docs/modules/12_appendix/external_sensors_main.rst
@@ -10,9 +10,7 @@ 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.
+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
 -------------
@@ -22,7 +20,7 @@ Laser sensors measure distances by utilizing light, commonly referred to as Ligh
 Radar Sensors
 -------------
 
-TBD
+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
@@ -59,4 +57,9 @@ Ultrasonic sensors are commonly used in indoor robots and some automotive autono
 References
 ----------
 
-TBD
+- 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

From 93228bda8c5a8aaa129a90ca1294e3bf2741e154 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 1 Jul 2025 23:19:42 +0900
Subject: [PATCH 74/84] build(deps): bump ruff from 0.12.0 to 0.12.1 in
 /requirements (#1244)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.12.0 to 0.12.1.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.12.0...0.12.1)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.12.1
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 30458e03d1..410bf3f39d 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.4.1 # For unit test
 pytest-xdist == 3.7.0 # For unit test
 mypy == 1.16.1 # For unit test
-ruff == 0.12.0 # For unit test
+ruff == 0.12.1 # For unit test

From 3a1ffdeb11dab931ff70bbcc3cabb21d879c0eaf Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 8 Jul 2025 22:22:52 +0900
Subject: [PATCH 75/84] build(deps): bump pytest-xdist from 3.7.0 to 3.8.0 in
 /requirements (#1245)

Bumps [pytest-xdist](https://github.com/pytest-dev/pytest-xdist) from 3.7.0 to 3.8.0.
- [Release notes](https://github.com/pytest-dev/pytest-xdist/releases)
- [Changelog](https://github.com/pytest-dev/pytest-xdist/blob/master/CHANGELOG.rst)
- [Commits](https://github.com/pytest-dev/pytest-xdist/compare/v3.7.0...v3.8.0)

---
updated-dependencies:
- dependency-name: pytest-xdist
  dependency-version: 3.8.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 410bf3f39d..23c7393dce 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -4,6 +4,6 @@ matplotlib == 3.10.1
 cvxpy == 1.6.6
 ecos == 2.0.14
 pytest == 8.4.1 # For unit test
-pytest-xdist == 3.7.0 # For unit test
+pytest-xdist == 3.8.0 # For unit test
 mypy == 1.16.1 # For unit test
 ruff == 0.12.1 # For unit test

From 19bff8c04b91abb58c024fceb6e10ffab4e7d723 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 8 Jul 2025 22:51:17 +0900
Subject: [PATCH 76/84] build(deps): bump ruff from 0.12.1 to 0.12.2 in
 /requirements (#1246)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.12.1 to 0.12.2.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.12.1...0.12.2)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.12.2
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 23c7393dce..96734f3c2a 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.4.1 # For unit test
 pytest-xdist == 3.8.0 # For unit test
 mypy == 1.16.1 # For unit test
-ruff == 0.12.1 # For unit test
+ruff == 0.12.2 # For unit test

From e9c86ab707ace8fa1407222b99c8946dcb20ec80 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 15 Jul 2025 22:21:03 +0900
Subject: [PATCH 77/84] build(deps): bump ruff from 0.12.2 to 0.12.3 in
 /requirements (#1248)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.12.2 to 0.12.3.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.12.2...0.12.3)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.12.3
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 96734f3c2a..21bf7b9dec 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.4.1 # For unit test
 pytest-xdist == 3.8.0 # For unit test
 mypy == 1.16.1 # For unit test
-ruff == 0.12.2 # For unit test
+ruff == 0.12.3 # For unit test

From d918947360f7e2393c2f1cef110dab77bbbe50db Mon Sep 17 00:00:00 2001
From: Jonathan Schwartz <jonbs98@gmail.com>
Date: Wed, 16 Jul 2025 08:56:00 -0400
Subject: [PATCH 78/84] Collaborative astar (#1247)

* consolidate Node definition

* add base class for single agent planner

* add base class for single agent planner

* its working

* use single agent plotting util

* cleanup, bug fix, add some results to docs

* remove seeding from sta* - it happens in Node

* remove stale todo

* rename CA* and speed up plotting

* paper

* proper paper (ofc its csail)

* some cleanup

* update docs

* add unit test

* add logic for saving animation as gif

* address github bot

* Revert "add logic for saving animation as gif"

This reverts commit 639167795c80a76a1dbd40a6fbf1f1ba70c14d54.

* fix tests

* docs lint

* add gifs

* copilot review

* appease mypy
---
 .../TimeBasedPathPlanning/BaseClasses.py      |  49 ++++++
 .../GridWithDynamicObstacles.py               |  95 +++++++---
 PathPlanning/TimeBasedPathPlanning/Node.py    |  99 +++++++++++
 .../TimeBasedPathPlanning/Plotting.py         | 135 +++++++++++++++
 .../PriorityBasedPlanner.py                   |  95 ++++++++++
 .../TimeBasedPathPlanning/SafeInterval.py     | 163 ++++--------------
 .../TimeBasedPathPlanning/SpaceTimeAStar.py   | 150 +++-------------
 .../time_based_grid_search_main.rst           |  19 ++
 tests/test_priority_based_planner.py          |  39 +++++
 tests/test_safe_interval_path_planner.py      |   4 +-
 tests/test_space_time_astar.py                |   6 +-
 11 files changed, 566 insertions(+), 288 deletions(-)
 create mode 100644 PathPlanning/TimeBasedPathPlanning/BaseClasses.py
 create mode 100644 PathPlanning/TimeBasedPathPlanning/Node.py
 create mode 100644 PathPlanning/TimeBasedPathPlanning/Plotting.py
 create mode 100644 PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py
 create mode 100644 tests/test_priority_based_planner.py

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
index 2a47023f8c..ccc2989001 100644
--- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
+++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
@@ -7,31 +7,7 @@
 import matplotlib.pyplot as plt
 from enum import Enum
 from dataclasses import dataclass
-
-@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))
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position
 
 @dataclass
 class Interval:
@@ -43,6 +19,8 @@ class ObstacleArrangement(Enum):
     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.
@@ -87,6 +65,8 @@ def __init__(
             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
@@ -184,6 +164,26 @@ def obstacle_arrangement_1(self, obs_count: int) -> list[list[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
@@ -196,11 +196,11 @@ def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]:
         bool: True if position/time combination is valid, False otherwise
     """
     def valid_position(self, position: Position, t: int) -> bool:
-        # Check if new position is in grid
+        # Check if position is in grid
         if not self.inside_grid_bounds(position):
             return False
 
-        # Check if new position is not occupied at time t
+        # Check if position is not occupied at time t
         return self.reservation_matrix[position.x, position.y, t] == 0
 
     """
@@ -289,9 +289,48 @@ def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]:
         # 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
 
-show_animation = True
+            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(
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
index 7fea10c67f..446847ac6d 100644
--- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
+++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
@@ -9,7 +9,6 @@
 """
 
 import numpy as np
-import matplotlib.pyplot as plt
 from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
     Grid,
     Interval,
@@ -17,8 +16,11 @@
     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
-import random
 from dataclasses import dataclass
 from functools import total_ordering
 import time
@@ -29,116 +31,50 @@
 #     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 Node:
-    position: Position
-    time: int
-    heuristic: int
-    parent_index: int
+class SIPPNode(Node):
     interval: Interval
 
-    """
-    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)
-
-    """
-    Equality only cares about position and time. Heuristic and interval will always be the same for a given
-    (position, time) pairing, so they are not considered in equality.
-    """
-    def __eq__(self, other: object):
-        if not isinstance(other, Node):
-            return NotImplemented
-        return self.position == other.position and self.time == other.time
-
 @dataclass
 class EntryTimeAndInterval:
     entry_time: int
     interval: Interval
 
-class NodePath:
-    path: list[Node]
-    positions_at_time: dict[int, Position] = {}
-
-    def __init__(self, path: list[Node]):
-        self.path = path
-        for (i, node) in enumerate(path):
-            if i > 0:
-                # account for waiting in interval at previous node
-                prev_node = path[i-1]
-                for t in range(prev_node.time, node.time):
-                    self.positions_at_time[t] = prev_node.position
-
-            self.positions_at_time[node.time] = 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
-
-
-class SafeIntervalPathPlanner:
-    grid: Grid
-    start: Position
-    goal: Position
-
-    def __init__(self, grid: Grid, start: Position, goal: Position):
-        self.grid = grid
-        self.start = start
-        self.goal = goal
-
-        # Seed randomness for reproducibility
-        RANDOM_SEED = 50
-        random.seed(RANDOM_SEED)
-        np.random.seed(RANDOM_SEED)
+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
     """
-    def plan(self, verbose: bool = False) -> NodePath:
+    @staticmethod
+    def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
 
-        safe_intervals = self.grid.get_safe_intervals()
+        safe_intervals = grid.get_safe_intervals()
 
-        open_set: list[Node] = []
-        first_node_interval = safe_intervals[self.start.x, self.start.y][0]
+        open_set: list[SIPPNode] = []
+        first_node_interval = safe_intervals[start.x, start.y][0]
         heapq.heappush(
-            open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1, first_node_interval)
+            open_set, SIPPNode(start, 0, SafeIntervalPathPlanner.calculate_heuristic(start, goal), -1, first_node_interval)
         )
 
-        expanded_list: list[Node] = []
-        visited_intervals = empty_2d_array_of_lists(self.grid.grid_size[0], self.grid.grid_size[1])
+        expanded_list: list[SIPPNode] = []
+        visited_intervals = empty_2d_array_of_lists(grid.grid_size[0], grid.grid_size[1])
         while open_set:
-            expanded_node: Node = heapq.heappop(open_set)
+            expanded_node: SIPPNode = heapq.heappop(open_set)
             if verbose:
                 print("Expanded node:", expanded_node)
 
-            if expanded_node.time + 1 >= self.grid.time_limit:
+            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 == self.goal:
-                print(f"Found path to goal after {len(expanded_list)} expansions")
+            if expanded_node.position == goal:
+                if verbose:
+                    print(f"Found path to goal after {len(expanded_list)} expansions")
+
                 path = []
-                path_walker: Node = expanded_node
+                path_walker: SIPPNode = expanded_node
                 while True:
                     path.append(path_walker)
                     if path_walker.parent_index == -1:
@@ -147,14 +83,14 @@ def plan(self, verbose: bool = False) -> NodePath:
 
                 # reverse path so it goes start -> goal
                 path.reverse()
-                return NodePath(path)
+                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 self.generate_successors(expanded_node, expanded_idx, safe_intervals, visited_intervals):
+            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")
@@ -162,9 +98,10 @@ def plan(self, verbose: bool = False) -> NodePath:
     """
     Generate list of possible successors of the provided `parent_node` that are worth expanding
     """
+    @staticmethod
     def generate_successors(
-        self, parent_node: Node, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray
-    ) -> list[Node]:
+        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),
@@ -175,7 +112,7 @@ def generate_successors(
         ]
         for diff in diffs:
             new_pos = parent_node.position + diff
-            if not self.grid.inside_grid_bounds(new_pos):
+            if not grid.inside_grid_bounds(new_pos):
                 continue
 
             current_interval = parent_node.interval
@@ -203,12 +140,12 @@ def generate_successors(
                 # 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 self.grid.valid_position(new_pos, possible_t):
-                        new_nodes.append(Node(
+                    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),
-                            self.calculate_heuristic(new_pos),
+                            SafeIntervalPathPlanner.calculate_heuristic(new_pos, goal),
                             parent_node_idx,
                             interval,
                         ))
@@ -220,8 +157,9 @@ def generate_successors(
     """
     Calculate the heuristic for a given position - Manhattan distance to the goal
     """
-    def calculate_heuristic(self, position) -> int:
-        diff = self.goal - position
+    @staticmethod
+    def calculate_heuristic(position: Position, goal: Position) -> int:
+        diff = goal - position
         return abs(diff.x) + abs(diff.y)
 
 
@@ -229,7 +167,7 @@ def calculate_heuristic(self, position) -> int:
 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: 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:
@@ -257,8 +195,7 @@ def main():
         # obstacle_arrangement=ObstacleArrangement.RANDOM,
     )
 
-    planner = SafeIntervalPathPlanner(grid, start, goal)
-    path = planner.plan(verbose)
+    path = SafeIntervalPathPlanner.plan(grid, start, goal, verbose)
     runtime = time.time() - start_time
     print(f"Planning took: {runtime:.5f} seconds")
 
@@ -268,35 +205,7 @@ def main():
     if not show_animation:
         return
 
-    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_side_length, 1))
-    ax.set_yticks(np.arange(0, grid_side_length, 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]
-    )
-
-    for i in range(0, path.goal_reached_time() + 1):
-        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)
-        path_points.set_data([path_position.x], [path_position.y])
-        plt.pause(0.2)
-    plt.show()
+    PlotNodePath(grid, start, goal, path)
 
 
 if __name__ == "__main__":
diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
index c4e2802d37..b85569f5dc 100644
--- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
+++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
@@ -9,101 +9,25 @@
 """
 
 import numpy as np
-import matplotlib.pyplot as plt
 from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
     Grid,
     ObstacleArrangement,
     Position,
 )
+from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath
 import heapq
 from collections.abc import Generator
-import random
-from dataclasses import dataclass
-from functools import total_ordering
 import time
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath
 
-# Seed randomness for reproducibility
-RANDOM_SEED = 50
-random.seed(RANDOM_SEED)
-np.random.seed(RANDOM_SEED)
-
-@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
+class SpaceTimeAStar(SingleAgentPlanner):
 
-    """
-    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: list[Node]
-    positions_at_time: dict[int, Position] = {}
-
-    def __init__(self, path: list[Node]):
-        self.path = path
-        for node in path:
-            self.positions_at_time[node.time] = 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
-
-
-class SpaceTimeAStar:
-    grid: Grid
-    start: Position
-    goal: Position
-    # Used to evaluate solutions
-    expanded_node_count: int = -1
-
-    def __init__(self, grid: Grid, start: Position, goal: Position):
-        self.grid = grid
-        self.start = start
-        self.goal = goal
-
-    def plan(self, verbose: bool = False) -> NodePath:
+    @staticmethod
+    def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
         open_set: list[Node] = []
         heapq.heappush(
-            open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1)
+            open_set, Node(start, 0, SpaceTimeAStar.calculate_heuristic(start, goal), -1)
         )
 
         expanded_list: list[Node] = []
@@ -113,13 +37,15 @@ def plan(self, verbose: bool = False) -> NodePath:
             if verbose:
                 print("Expanded node:", expanded_node)
 
-            if expanded_node.time + 1 >= self.grid.time_limit:
+            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 == self.goal:
-                print(f"Found path to goal after {len(expanded_list)} expansions")
+            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:
@@ -130,14 +56,13 @@ def plan(self, verbose: bool = False) -> NodePath:
 
                 # reverse path so it goes start -> goal
                 path.reverse()
-                self.expanded_node_count = len(expanded_set)
-                return NodePath(path)
+                return NodePath(path, len(expanded_set))
 
             expanded_idx = len(expanded_list)
             expanded_list.append(expanded_node)
             expanded_set.add(expanded_node)
 
-            for child in self.generate_successors(expanded_node, expanded_idx, verbose, expanded_set):
+            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")
@@ -145,8 +70,9 @@ def plan(self, verbose: bool = False) -> NodePath:
     """
     Generate possible successors of the provided `parent_node`
     """
+    @staticmethod
     def generate_successors(
-        self, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node]
+        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),
@@ -160,20 +86,22 @@ def generate_successors(
             new_node = Node(
                 new_pos,
                 parent_node.time + 1,
-                self.calculate_heuristic(new_pos),
+                SpaceTimeAStar.calculate_heuristic(new_pos, goal),
                 parent_node_idx,
             )
 
             if new_node in expanded_set:
                 continue
 
-            if self.grid.valid_position(new_pos, parent_node.time + 1):
+            # 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
 
-    def calculate_heuristic(self, position) -> int:
-        diff = self.goal - position
+    @staticmethod
+    def calculate_heuristic(position: Position, goal: Position) -> int:
+        diff = goal - position
         return abs(diff.x) + abs(diff.y)
 
 
@@ -194,8 +122,7 @@ def main():
         obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
     )
 
-    planner = SpaceTimeAStar(grid, start, goal)
-    path = planner.plan(verbose)
+    path = SpaceTimeAStar.plan(grid, start, goal, verbose)
 
     runtime = time.time() - start_time
     print(f"Planning took: {runtime:.5f} seconds")
@@ -206,36 +133,7 @@ def main():
     if not show_animation:
         return
 
-    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_side_length, 1))
-    ax.set_yticks(np.arange(0, grid_side_length, 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]
-    )
-
-    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)
-        path_points.set_data([path_position.x], [path_position.y])
-        plt.pause(0.2)
-    plt.show()
-
+    PlotNodePath(grid, start, goal, path)
 
 if __name__ == "__main__":
     main()
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
index 9517e95b31..a44dd20a97 100644
--- 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
@@ -81,9 +81,28 @@ 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/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_safe_interval_path_planner.py b/tests/test_safe_interval_path_planner.py
index f1fbf90d2a..bbcd4e427c 100644
--- a/tests/test_safe_interval_path_planner.py
+++ b/tests/test_safe_interval_path_planner.py
@@ -18,9 +18,7 @@ def test_1():
     )
 
     m.show_animation = False
-    planner = m.SafeIntervalPathPlanner(grid, start, goal)
-
-    path = planner.plan(False)
+    path = m.SafeIntervalPathPlanner.plan(grid, start, goal)
 
     # path should have 31 entries
     assert len(path.path) == 31
diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py
index 390c7732dc..1194c61d2e 100644
--- a/tests/test_space_time_astar.py
+++ b/tests/test_space_time_astar.py
@@ -18,9 +18,7 @@ def test_1():
     )
 
     m.show_animation = False
-    planner = m.SpaceTimeAStar(grid, start, goal)
-
-    path = planner.plan(False)
+    path = m.SpaceTimeAStar.plan(grid, start, goal)
 
     # path should have 28 entries
     assert len(path.path) == 31
@@ -28,7 +26,7 @@ def test_1():
     # path should end at the goal
     assert path.path[-1].position == goal
 
-    assert planner.expanded_node_count < 1000
+    assert path.expanded_node_count < 1000
 
 if __name__ == "__main__":
     conftest.run_this_test(__file__)

From 0a8766929d83dfa90b262f0dba03bef59c84dca7 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 29 Jul 2025 23:08:47 +0900
Subject: [PATCH 79/84] build(deps): bump ruff from 0.12.3 to 0.12.5 in
 /requirements (#1251)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.12.3 to 0.12.5.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.12.3...0.12.5)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.12.5
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 21bf7b9dec..39519f93c0 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ ecos == 2.0.14
 pytest == 8.4.1 # For unit test
 pytest-xdist == 3.8.0 # For unit test
 mypy == 1.16.1 # For unit test
-ruff == 0.12.3 # For unit test
+ruff == 0.12.5 # For unit test

From 7e8fbfb635fd7fd8beabbd7fe8f6c876af471201 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Wed, 30 Jul 2025 12:57:43 +0900
Subject: [PATCH 80/84] build(deps): bump cvxpy from 1.6.6 to 1.7.1 in
 /requirements (#1253)

Bumps [cvxpy](https://github.com/cvxpy/cvxpy) from 1.6.6 to 1.7.1.
- [Release notes](https://github.com/cvxpy/cvxpy/releases)
- [Commits](https://github.com/cvxpy/cvxpy/compare/v1.6.6...v1.7.1)

---
updated-dependencies:
- dependency-name: cvxpy
  dependency-version: 1.7.1
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 39519f93c0..6ca0bf9606 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,7 +1,7 @@
 numpy == 2.2.4
 scipy == 1.15.2
 matplotlib == 3.10.1
-cvxpy == 1.6.6
+cvxpy == 1.7.1
 ecos == 2.0.14
 pytest == 8.4.1 # For unit test
 pytest-xdist == 3.8.0 # For unit test

From f9db30bff1df50b078dd3696ee8ce05f2bc8ebe5 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Wed, 30 Jul 2025 18:37:08 +0900
Subject: [PATCH 81/84] build(deps): bump mypy from 1.16.1 to 1.17.0 in
 /requirements (#1252)

Bumps [mypy](https://github.com/python/mypy) from 1.16.1 to 1.17.0.
- [Changelog](https://github.com/python/mypy/blob/master/CHANGELOG.md)
- [Commits](https://github.com/python/mypy/compare/v1.16.1...v1.17.0)

---
updated-dependencies:
- dependency-name: mypy
  dependency-version: 1.17.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 6ca0bf9606..66b5202aca 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,5 +5,5 @@ 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.16.1 # For unit test
+mypy == 1.17.0 # For unit test
 ruff == 0.12.5 # For unit test

From 458c52f39024e4970bef420d3cac8cbe0776628e Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Thu, 7 Aug 2025 07:33:33 +0900
Subject: [PATCH 82/84] build(deps): bump
 larsoner/circleci-artifacts-redirector-action (#1254)

Bumps [larsoner/circleci-artifacts-redirector-action](https://github.com/larsoner/circleci-artifacts-redirector-action) from 1.1.0 to 1.2.0.
- [Release notes](https://github.com/larsoner/circleci-artifacts-redirector-action/releases)
- [Commits](https://github.com/larsoner/circleci-artifacts-redirector-action/compare/v1.1.0...v1.2.0)

---
updated-dependencies:
- dependency-name: larsoner/circleci-artifacts-redirector-action
  dependency-version: 1.2.0
  dependency-type: direct:production
  update-type: version-update:semver-minor
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 .github/workflows/circleci-artifacts-redirector.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/circleci-artifacts-redirector.yml b/.github/workflows/circleci-artifacts-redirector.yml
index 692942cddd..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.1.0
+        uses: larsoner/circleci-artifacts-redirector-action@v1.2.0
         with:
           repo-token: ${{ secrets.GITHUB_TOKEN }}
           api-token: ${{ secrets.CIRCLECI_TOKEN }}

From db06b9f9669b216b4a78269c8acfa07abc6d45e7 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Thu, 7 Aug 2025 21:54:40 +0900
Subject: [PATCH 83/84] build(deps): bump mypy from 1.17.0 to 1.17.1 in
 /requirements (#1255)

Bumps [mypy](https://github.com/python/mypy) from 1.17.0 to 1.17.1.
- [Changelog](https://github.com/python/mypy/blob/master/CHANGELOG.md)
- [Commits](https://github.com/python/mypy/compare/v1.17.0...v1.17.1)

---
updated-dependencies:
- dependency-name: mypy
  dependency-version: 1.17.1
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 66b5202aca..f8f16dc34d 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -5,5 +5,5 @@ 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.0 # For unit test
+mypy == 1.17.1 # For unit test
 ruff == 0.12.5 # For unit test

From 6455a4db8f7e3d995af0b08087f0c9543f2b7ca3 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sat, 9 Aug 2025 11:36:45 +0900
Subject: [PATCH 84/84] build(deps): bump ruff from 0.12.5 to 0.12.7 in
 /requirements (#1256)

Bumps [ruff](https://github.com/astral-sh/ruff) from 0.12.5 to 0.12.7.
- [Release notes](https://github.com/astral-sh/ruff/releases)
- [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md)
- [Commits](https://github.com/astral-sh/ruff/compare/0.12.5...0.12.7)

---
updated-dependencies:
- dependency-name: ruff
  dependency-version: 0.12.7
  dependency-type: direct:production
  update-type: version-update:semver-patch
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 requirements/requirements.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index f8f16dc34d..38001d34a9 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -6,4 +6,4 @@ 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.5 # For unit test
+ruff == 0.12.7 # For unit test
