diff --git a/.circleci/config.yml b/.circleci/config.yml
index d8f1e89dc4..f6eff674de 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -6,7 +6,7 @@ orbs:
jobs:
build_doc:
docker:
- - image: cimg/python:3.11
+ - image: cimg/python:3.13
steps:
- checkout
- run:
diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml
index 24f0926299..0d943696cc 100644
--- a/.github/FUNDING.yml
+++ b/.github/FUNDING.yml
@@ -1,4 +1,4 @@
# These are supported funding model platforms
github: AtsushiSakai
patreon: myenigma
-custom: https://www.paypal.me/myenigmapay/
+custom: https://www.paypal.com/paypalme/myenigmapay/
diff --git a/.github/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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/MissionPlanning/StateMachine/robot_behavior_case.py b/MissionPlanning/StateMachine/robot_behavior_case.py
new file mode 100644
index 0000000000..03ee60ae9f
--- /dev/null
+++ b/MissionPlanning/StateMachine/robot_behavior_case.py
@@ -0,0 +1,111 @@
+"""
+A case study of robot behavior using state machine
+
+author: Wang Zheng (@Aglargil)
+"""
+
+from state_machine import StateMachine
+
+
+class Robot:
+ def __init__(self):
+ self.battery = 100
+ self.task_progress = 0
+
+ # Initialize state machine
+ self.machine = StateMachine("robot_sm", self)
+
+ # Add state transition rules
+ self.machine.add_transition(
+ src_state="patrolling",
+ event="detect_task",
+ dst_state="executing_task",
+ guard=None,
+ action=None,
+ )
+
+ self.machine.add_transition(
+ src_state="executing_task",
+ event="task_complete",
+ dst_state="patrolling",
+ guard=None,
+ action="reset_task",
+ )
+
+ self.machine.add_transition(
+ src_state="executing_task",
+ event="low_battery",
+ dst_state="returning_to_base",
+ guard="is_battery_low",
+ )
+
+ self.machine.add_transition(
+ src_state="returning_to_base",
+ event="reach_base",
+ dst_state="charging",
+ guard=None,
+ action=None,
+ )
+
+ self.machine.add_transition(
+ src_state="charging",
+ event="charge_complete",
+ dst_state="patrolling",
+ guard=None,
+ action="battery_full",
+ )
+
+ # Set initial state
+ self.machine.set_current_state("patrolling")
+
+ def is_battery_low(self):
+ """Battery level check condition"""
+ return self.battery < 30
+
+ def reset_task(self):
+ """Reset task progress"""
+ self.task_progress = 0
+ print("[Action] Task progress has been reset")
+
+ # Modify state entry callback naming convention (add state_ prefix)
+ def on_enter_executing_task(self):
+ print("\n------ Start Executing Task ------")
+ print(f"Current battery: {self.battery}%")
+ while self.machine.get_current_state().name == "executing_task":
+ self.task_progress += 10
+ self.battery -= 25
+ print(
+ f"Task progress: {self.task_progress}%, Remaining battery: {self.battery}%"
+ )
+
+ if self.task_progress >= 100:
+ self.machine.process("task_complete")
+ break
+ elif self.is_battery_low():
+ self.machine.process("low_battery")
+ break
+
+ def on_enter_returning_to_base(self):
+ print("\nLow battery, returning to charging station...")
+ self.machine.process("reach_base")
+
+ def on_enter_charging(self):
+ print("\n------ Charging ------")
+ self.battery = 100
+ print("Charging complete!")
+ self.machine.process("charge_complete")
+
+
+# Keep the test section structure the same, only modify the trigger method
+if __name__ == "__main__":
+ robot = Robot()
+ print(robot.machine.generate_plantuml())
+
+ print(f"Initial state: {robot.machine.get_current_state().name}")
+ print("------------")
+
+ # Trigger task detection event
+ robot.machine.process("detect_task")
+
+ print("\n------------")
+ print(f"Final state: {robot.machine.get_current_state().name}")
diff --git a/MissionPlanning/StateMachine/state_machine.py b/MissionPlanning/StateMachine/state_machine.py
new file mode 100644
index 0000000000..454759236e
--- /dev/null
+++ b/MissionPlanning/StateMachine/state_machine.py
@@ -0,0 +1,294 @@
+"""
+State Machine
+
+author: Wang Zheng (@Aglargil)
+
+Reference:
+
+- [State Machine]
+(https://en.wikipedia.org/wiki/Finite-state_machine)
+"""
+
+import string
+from urllib.request import urlopen, Request
+from base64 import b64encode
+from zlib import compress
+from io import BytesIO
+from collections.abc import Callable
+from matplotlib.image import imread
+from matplotlib import pyplot as plt
+
+
+def deflate_and_encode(plantuml_text):
+ """
+ zlib compress the plantuml text and encode it for the plantuml server.
+
+ Reference https://plantuml.com/en/text-encoding
+ """
+ plantuml_alphabet = (
+ string.digits + string.ascii_uppercase + string.ascii_lowercase + "-_"
+ )
+ base64_alphabet = (
+ string.ascii_uppercase + string.ascii_lowercase + string.digits + "+/"
+ )
+ b64_to_plantuml = bytes.maketrans(
+ base64_alphabet.encode("utf-8"), plantuml_alphabet.encode("utf-8")
+ )
+ zlibbed_str = compress(plantuml_text.encode("utf-8"))
+ compressed_string = zlibbed_str[2:-4]
+ return b64encode(compressed_string).translate(b64_to_plantuml).decode("utf-8")
+
+
+class State:
+ def __init__(self, name, on_enter=None, on_exit=None):
+ self.name = name
+ self.on_enter = on_enter
+ self.on_exit = on_exit
+
+ def enter(self):
+ print(f"entering <{self.name}>")
+ if self.on_enter:
+ self.on_enter()
+
+ def exit(self):
+ print(f"exiting <{self.name}>")
+ if self.on_exit:
+ self.on_exit()
+
+
+class StateMachine:
+ def __init__(self, name: str, model=object):
+ """Initialize the state machine.
+
+ Args:
+ name (str): Name of the state machine.
+ model (object, optional): Model object used to automatically look up callback functions
+ for states and transitions:
+ State callbacks: Automatically searches for 'on_enter_' and 'on_exit_' methods.
+ Transition callbacks: When action or guard parameters are strings, looks up corresponding methods in the model.
+
+ Example:
+ >>> class MyModel:
+ ... def on_enter_idle(self):
+ ... print("Entering idle state")
+ ... def on_exit_idle(self):
+ ... print("Exiting idle state")
+ ... def can_start(self):
+ ... return True
+ ... def on_start(self):
+ ... print("Starting operation")
+ >>> model = MyModel()
+ >>> machine = StateMachine("my_machine", model)
+ """
+ self._name = name
+ self._states = {}
+ self._events = {}
+ self._transition_table = {}
+ self._model = model
+ self._state: State = None
+
+ def _register_event(self, event: str):
+ self._events[event] = event
+
+ def _get_state(self, name):
+ return self._states[name]
+
+ def _get_event(self, name):
+ return self._events[name]
+
+ def _has_event(self, event: str):
+ return event in self._events
+
+ def add_transition(
+ self,
+ src_state: str | State,
+ event: str,
+ dst_state: str | State,
+ guard: str | Callable = None,
+ action: str | Callable = None,
+ ) -> None:
+ """Add a transition to the state machine.
+
+ Args:
+ src_state (str | State): The source state where the transition begins.
+ Can be either a state name or a State object.
+ event (str): The event that triggers this transition.
+ dst_state (str | State): The destination state where the transition ends.
+ Can be either a state name or a State object.
+ guard (str | Callable, optional): Guard condition for the transition.
+ If callable: Function that returns bool.
+ If str: Name of a method in the model class.
+ If returns True: Transition proceeds.
+ If returns False: Transition is skipped.
+ action (str | Callable, optional): Action to execute during transition.
+ If callable: Function to execute.
+ If str: Name of a method in the model class.
+ Executed after guard passes and before entering new state.
+
+ Example:
+ >>> machine.add_transition(
+ ... src_state="idle",
+ ... event="start",
+ ... dst_state="running",
+ ... guard="can_start",
+ ... action="on_start"
+ ... )
+ """
+ # Convert string parameters to objects if necessary
+ self.register_state(src_state)
+ self._register_event(event)
+ self.register_state(dst_state)
+
+ def get_state_obj(state):
+ return state if isinstance(state, State) else self._get_state(state)
+
+ def get_callable(func):
+ return func if callable(func) else getattr(self._model, func, None)
+
+ src_state_obj = get_state_obj(src_state)
+ dst_state_obj = get_state_obj(dst_state)
+
+ guard_func = get_callable(guard) if guard else None
+ action_func = get_callable(action) if action else None
+ self._transition_table[(src_state_obj.name, event)] = (
+ dst_state_obj,
+ guard_func,
+ action_func,
+ )
+
+ def state_transition(self, src_state: State, event: str):
+ if (src_state.name, event) not in self._transition_table:
+ raise ValueError(
+ f"|{self._name}| invalid transition: <{src_state.name}> : [{event}]"
+ )
+
+ dst_state, guard, action = self._transition_table[(src_state.name, event)]
+
+ def call_guard(guard):
+ if callable(guard):
+ return guard()
+ else:
+ return True
+
+ def call_action(action):
+ if callable(action):
+ action()
+
+ if call_guard(guard):
+ call_action(action)
+ if src_state.name != dst_state.name:
+ print(
+ f"|{self._name}| transitioning from <{src_state.name}> to <{dst_state.name}>"
+ )
+ src_state.exit()
+ self._state = dst_state
+ dst_state.enter()
+ else:
+ print(
+ f"|{self._name}| skipping transition from <{src_state.name}> to <{dst_state.name}> because guard failed"
+ )
+
+ def register_state(self, state: str | State, on_enter=None, on_exit=None):
+ """Register a state in the state machine.
+
+ Args:
+ state (str | State): The state to register. Can be either a string (state name)
+ or a State object.
+ on_enter (Callable, optional): Callback function to be executed when entering the state.
+ If state is a string and on_enter is None, it will look for
+ a method named 'on_enter_' in the model.
+ on_exit (Callable, optional): Callback function to be executed when exiting the state.
+ If state is a string and on_exit is None, it will look for
+ a method named 'on_exit_' in the model.
+ Example:
+ >>> machine.register_state("idle", on_enter=on_enter_idle, on_exit=on_exit_idle)
+ >>> machine.register_state(State("running", on_enter=on_enter_running, on_exit=on_exit_running))
+ """
+ if isinstance(state, str):
+ if on_enter is None:
+ on_enter = getattr(self._model, "on_enter_" + state, None)
+ if on_exit is None:
+ on_exit = getattr(self._model, "on_exit_" + state, None)
+ self._states[state] = State(state, on_enter, on_exit)
+ return
+
+ self._states[state.name] = state
+
+ def set_current_state(self, state: State | str):
+ if isinstance(state, str):
+ self._state = self._get_state(state)
+ else:
+ self._state = state
+
+ def get_current_state(self):
+ return self._state
+
+ def process(self, event: str) -> None:
+ """Process an event in the state machine.
+
+ Args:
+ event: Event name.
+
+ Example:
+ >>> machine.process("start")
+ """
+ if self._state is None:
+ raise ValueError("State machine is not initialized")
+
+ if self._has_event(event):
+ self.state_transition(self._state, event)
+ else:
+ raise ValueError(f"Invalid event: {event}")
+
+ def generate_plantuml(self) -> str:
+ """Generate PlantUML state diagram representation of the state machine.
+
+ Returns:
+ str: PlantUML state diagram code.
+ """
+ if self._state is None:
+ raise ValueError("State machine is not initialized")
+
+ plant_uml = ["@startuml"]
+ plant_uml.append("[*] --> " + self._state.name)
+
+ # Generate transitions
+ for (src_state, event), (
+ dst_state,
+ guard,
+ action,
+ ) in self._transition_table.items():
+ transition = f"{src_state} --> {dst_state.name} : {event}"
+
+ # Add guard and action if present
+ conditions = []
+ if guard:
+ guard_name = guard.__name__ if callable(guard) else guard
+ conditions.append(f"[{guard_name}]")
+ if action:
+ action_name = action.__name__ if callable(action) else action
+ conditions.append(f"/ {action_name}")
+
+ if conditions:
+ transition += "\\n" + " ".join(conditions)
+
+ plant_uml.append(transition)
+
+ plant_uml.append("@enduml")
+ plant_uml_text = "\n".join(plant_uml)
+
+ try:
+ url = f"http://www.plantuml.com/plantuml/img/{deflate_and_encode(plant_uml_text)}"
+ headers = {"User-Agent": "Mozilla/5.0"}
+ request = Request(url, headers=headers)
+
+ with urlopen(request) as response:
+ content = response.read()
+
+ plt.imshow(imread(BytesIO(content), format="png"))
+ plt.axis("off")
+ plt.show()
+ except Exception as e:
+ print(f"Error showing PlantUML: {e}")
+
+ return plant_uml_text
diff --git a/PathPlanning/AStar/a_star_searching_from_two_side.py b/PathPlanning/AStar/a_star_searching_from_two_side.py
index 822f5ea500..f43cea71b4 100644
--- a/PathPlanning/AStar/a_star_searching_from_two_side.py
+++ b/PathPlanning/AStar/a_star_searching_from_two_side.py
@@ -78,43 +78,43 @@ def boundary_and_obstacles(start, goal, top_vertex, bottom_vertex, obs_number):
def find_neighbor(node, ob, closed):
- # generate neighbors in certain condition
- ob_list = ob.tolist()
- neighbor: list = []
+ # Convert obstacles to a set for faster lookup
+ ob_set = set(map(tuple, ob.tolist()))
+ neighbor_set = set()
+
+ # Generate neighbors within the 3x3 grid around the node
for x in range(node.coordinate[0] - 1, node.coordinate[0] + 2):
for y in range(node.coordinate[1] - 1, node.coordinate[1] + 2):
- if [x, y] not in ob_list:
- # find all possible neighbor nodes
- neighbor.append([x, y])
- # remove node violate the motion rule
- # 1. remove node.coordinate itself
- neighbor.remove(node.coordinate)
- # 2. remove neighbor nodes who cross through two diagonal
- # positioned obstacles since there is no enough space for
- # robot to go through two diagonal positioned obstacles
-
- # top bottom left right neighbors of node
- top_nei = [node.coordinate[0], node.coordinate[1] + 1]
- bottom_nei = [node.coordinate[0], node.coordinate[1] - 1]
- left_nei = [node.coordinate[0] - 1, node.coordinate[1]]
- right_nei = [node.coordinate[0] + 1, node.coordinate[1]]
- # neighbors in four vertex
- lt_nei = [node.coordinate[0] - 1, node.coordinate[1] + 1]
- rt_nei = [node.coordinate[0] + 1, node.coordinate[1] + 1]
- lb_nei = [node.coordinate[0] - 1, node.coordinate[1] - 1]
- rb_nei = [node.coordinate[0] + 1, node.coordinate[1] - 1]
-
- # remove the unnecessary neighbors
- if top_nei and left_nei in ob_list and lt_nei in neighbor:
- neighbor.remove(lt_nei)
- if top_nei and right_nei in ob_list and rt_nei in neighbor:
- neighbor.remove(rt_nei)
- if bottom_nei and left_nei in ob_list and lb_nei in neighbor:
- neighbor.remove(lb_nei)
- if bottom_nei and right_nei in ob_list and rb_nei in neighbor:
- neighbor.remove(rb_nei)
- neighbor = [x for x in neighbor if x not in closed]
- return neighbor
+ coord = (x, y)
+ if coord not in ob_set and coord != tuple(node.coordinate):
+ neighbor_set.add(coord)
+
+ # Define neighbors in cardinal and diagonal directions
+ top_nei = (node.coordinate[0], node.coordinate[1] + 1)
+ bottom_nei = (node.coordinate[0], node.coordinate[1] - 1)
+ left_nei = (node.coordinate[0] - 1, node.coordinate[1])
+ right_nei = (node.coordinate[0] + 1, node.coordinate[1])
+ lt_nei = (node.coordinate[0] - 1, node.coordinate[1] + 1)
+ rt_nei = (node.coordinate[0] + 1, node.coordinate[1] + 1)
+ lb_nei = (node.coordinate[0] - 1, node.coordinate[1] - 1)
+ rb_nei = (node.coordinate[0] + 1, node.coordinate[1] - 1)
+
+ # Remove neighbors that violate diagonal motion rules
+ if top_nei in ob_set and left_nei in ob_set:
+ neighbor_set.discard(lt_nei)
+ if top_nei in ob_set and right_nei in ob_set:
+ neighbor_set.discard(rt_nei)
+ if bottom_nei in ob_set and left_nei in ob_set:
+ neighbor_set.discard(lb_nei)
+ if bottom_nei in ob_set and right_nei in ob_set:
+ neighbor_set.discard(rb_nei)
+
+ # Filter out neighbors that are in the closed set
+ closed_set = set(map(tuple, closed))
+ neighbor_set -= closed_set
+
+ return list(neighbor_set)
+
def find_node_index(coordinate, node_list):
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/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py
index ff662170e7..ad994732a5 100644
--- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py
+++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py
@@ -220,20 +220,20 @@ def main():
# set obstacle positions
ox, oy = [], []
for i in range(-10, 60):
- ox.append(i)
+ ox.append(float(i))
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(-10, 61):
ox.append(-10.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
diff --git a/PathPlanning/BugPlanning/bug.py b/PathPlanning/BugPlanning/bug.py
index 62fec7c109..34890cb55a 100644
--- a/PathPlanning/BugPlanning/bug.py
+++ b/PathPlanning/BugPlanning/bug.py
@@ -1,7 +1,7 @@
"""
Bug Planning
author: Sarim Mehdi(muhammadsarim.mehdi@studio.unibo.it)
-Source: https://sites.google.com/site/ece452bugalgorithms/
+Source: https://web.archive.org/web/20201103052224/https://sites.google.com/site/ece452bugalgorithms/
"""
import numpy as np
diff --git a/PathPlanning/Catmull_RomSplinePath/blending_functions.py b/PathPlanning/Catmull_RomSplinePath/blending_functions.py
new file mode 100644
index 0000000000..f3ef5dd323
--- /dev/null
+++ b/PathPlanning/Catmull_RomSplinePath/blending_functions.py
@@ -0,0 +1,34 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+def blending_function_1(t):
+ return -t + 2*t**2 - t**3
+
+def blending_function_2(t):
+ return 2 - 5*t**2 + 3*t**3
+
+def blending_function_3(t):
+ return t + 4*t**2 - 3*t**3
+
+def blending_function_4(t):
+ return -t**2 + t**3
+
+def plot_blending_functions():
+ t = np.linspace(0, 1, 100)
+
+ plt.plot(t, blending_function_1(t), label='b1')
+ plt.plot(t, blending_function_2(t), label='b2')
+ plt.plot(t, blending_function_3(t), label='b3')
+ plt.plot(t, blending_function_4(t), label='b4')
+
+ plt.title("Catmull-Rom Blending Functions")
+ plt.xlabel("t")
+ plt.ylabel("Value")
+ plt.legend()
+ plt.grid(True)
+ plt.axhline(y=0, color='k', linestyle='--')
+ plt.axvline(x=0, color='k', linestyle='--')
+ plt.show()
+
+if __name__ == "__main__":
+ plot_blending_functions()
\ No newline at end of file
diff --git a/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py b/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py
new file mode 100644
index 0000000000..79916330c9
--- /dev/null
+++ b/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py
@@ -0,0 +1,86 @@
+"""
+Path Planner with Catmull-Rom Spline
+Author: Surabhi Gupta (@this_is_surabhi)
+Source: http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf
+"""
+
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+def catmull_rom_point(t, p0, p1, p2, p3):
+ """
+ Parameters
+ ----------
+ t : float
+ Parameter value (0 <= t <= 1) (0 <= t <= 1)
+ p0, p1, p2, p3 : np.ndarray
+ Control points for the spline segment
+
+ Returns
+ -------
+ np.ndarray
+ Calculated point on the spline
+ """
+ return 0.5 * ((2 * p1) +
+ (-p0 + p2) * t +
+ (2 * p0 - 5 * p1 + 4 * p2 - p3) * t**2 +
+ (-p0 + 3 * p1 - 3 * p2 + p3) * t**3)
+
+
+def catmull_rom_spline(control_points, num_points):
+ """
+ Parameters
+ ----------
+ control_points : list
+ List of control points
+ num_points : int
+ Number of points to generate on the spline
+
+ Returns
+ -------
+ tuple
+ x and y coordinates of the spline points
+ """
+ t_vals = np.linspace(0, 1, num_points)
+ spline_points = []
+
+ control_points = np.array(control_points)
+
+ for i in range(len(control_points) - 1):
+ if i == 0:
+ p1, p2, p3 = control_points[i:i+3]
+ p0 = p1
+ elif i == len(control_points) - 2:
+ p0, p1, p2 = control_points[i-1:i+2]
+ p3 = p2
+ else:
+ p0, p1, p2, p3 = control_points[i-1:i+3]
+
+ for t in t_vals:
+ point = catmull_rom_point(t, p0, p1, p2, p3)
+ spline_points.append(point)
+
+ return np.array(spline_points).T
+
+
+def main():
+ print(__file__ + " start!!")
+
+ way_points = [[-1.0, -2.0], [1.0, -1.0], [3.0, -2.0], [4.0, -1.0], [3.0, 1.0], [1.0, 2.0], [0.0, 2.0]]
+ n_course_point = 100
+ spline_x, spline_y = catmull_rom_spline(way_points, n_course_point)
+
+ plt.plot(spline_x,spline_y, '-r', label="Catmull-Rom Spline Path")
+ plt.plot(np.array(way_points).T[0], np.array(way_points).T[1], '-og', label="Way points")
+ plt.title("Catmull-Rom Spline Path")
+ plt.grid(True)
+ plt.legend()
+ plt.axis("equal")
+ plt.show()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
index 5a0fd17a4e..c05f76c84e 100644
--- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
+++ b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
@@ -8,6 +8,7 @@
import math
import numpy as np
+from utils.angle import angle_mod
dt = 0.05 # [s]
L = 0.9 # [m]
@@ -39,7 +40,7 @@ def update(state, a, delta):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
if __name__ == '__main__': # pragma: no cover
diff --git a/PathPlanning/CubicSpline/cubic_spline_planner.py b/PathPlanning/CubicSpline/cubic_spline_planner.py
index 7cc7bedc79..2391f67c39 100644
--- a/PathPlanning/CubicSpline/cubic_spline_planner.py
+++ b/PathPlanning/CubicSpline/cubic_spline_planner.py
@@ -76,6 +76,11 @@ def calc_position(self, x):
if `x` is outside the data point's `x` range, return None.
+ Parameters
+ ----------
+ x : float
+ x position to calculate y.
+
Returns
-------
y : float
@@ -99,6 +104,11 @@ def calc_first_derivative(self, x):
if x is outside the input x, return None
+ Parameters
+ ----------
+ x : float
+ x position to calculate first derivative.
+
Returns
-------
dy : float
@@ -121,6 +131,11 @@ def calc_second_derivative(self, x):
if x is outside the input x, return None
+ Parameters
+ ----------
+ x : float
+ x position to calculate second derivative.
+
Returns
-------
ddy : float
@@ -137,6 +152,31 @@ def calc_second_derivative(self, x):
ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
return ddy
+ def calc_third_derivative(self, x):
+ """
+ Calc third derivative at given x.
+
+ if x is outside the input x, return None
+
+ Parameters
+ ----------
+ x : float
+ x position to calculate third derivative.
+
+ Returns
+ -------
+ dddy : float
+ third derivative for given x.
+ """
+ if x < self.x[0]:
+ return None
+ elif x > self.x[-1]:
+ return None
+
+ i = self.__search_index(x)
+ dddy = 6.0 * self.d[i]
+ return dddy
+
def __search_index(self, x):
"""
search data segment index
@@ -287,6 +327,33 @@ def calc_curvature(self, s):
k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
return k
+ def calc_curvature_rate(self, s):
+ """
+ calc curvature rate
+
+ Parameters
+ ----------
+ s : float
+ distance from the start point. if `s` is outside the data point's
+ range, return None.
+
+ Returns
+ -------
+ k : float
+ curvature rate for given s.
+ """
+ dx = self.sx.calc_first_derivative(s)
+ dy = self.sy.calc_first_derivative(s)
+ ddx = self.sx.calc_second_derivative(s)
+ ddy = self.sy.calc_second_derivative(s)
+ dddx = self.sx.calc_third_derivative(s)
+ dddy = self.sy.calc_third_derivative(s)
+ a = dx * ddy - dy * ddx
+ b = dx * dddy - dy * dddx
+ c = dx * ddx + dy * ddy
+ d = dx * dx + dy * dy
+ return (b * d - 3.0 * a * c) / (d * d * d)
+
def calc_yaw(self, s):
"""
calc yaw
diff --git a/PathPlanning/DStar/dstar.py b/PathPlanning/DStar/dstar.py
index f8954d776c..b62b939f54 100644
--- a/PathPlanning/DStar/dstar.py
+++ b/PathPlanning/DStar/dstar.py
@@ -9,6 +9,7 @@
"""
import math
+
from sys import maxsize
import matplotlib.pyplot as plt
@@ -103,7 +104,7 @@ def process_state(self):
if y.h <= k_old and x.h > y.h + x.cost(y):
x.parent = y
x.h = y.h + x.cost(y)
- elif k_old == x.h:
+ if k_old == x.h:
for y in self.map.get_neighbors(x):
if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \
or y.parent != x and y.h > x.h + x.cost(y):
@@ -116,7 +117,7 @@ def process_state(self):
self.insert(y, x.h + x.cost(y))
else:
if y.parent != x and y.h > x.h + x.cost(y):
- self.insert(y, x.h)
+ self.insert(x, x.h)
else:
if y.parent != x and x.h > y.h + x.cost(y) \
and y.t == "close" and y.h > k_old:
@@ -173,6 +174,8 @@ def run(self, start, end):
s.set_state("e")
tmp = start
+ AddNewObstacle(self.map) # add new obstacle after the first search finished
+
while tmp != end:
tmp.set_state("*")
rx.append(tmp.x)
@@ -195,6 +198,15 @@ def modify(self, state):
if k_min >= state.h:
break
+def AddNewObstacle(map:Map):
+ ox, oy = [], []
+ for i in range(5, 21):
+ ox.append(i)
+ oy.append(40)
+ map.set_obstacle([(i, j) for i, j in zip(ox, oy)])
+ if show_animation:
+ plt.pause(0.001)
+ plt.plot(ox, oy, ".g")
def main():
m = Map(100, 100)
@@ -217,7 +229,6 @@ def main():
for i in range(0, 40):
ox.append(40)
oy.append(60 - i)
- print([(i, j) for i, j in zip(ox, oy)])
m.set_obstacle([(i, j) for i, j in zip(ox, oy)])
start = [10, 10]
diff --git a/PathPlanning/DStarLite/d_star_lite.py b/PathPlanning/DStarLite/d_star_lite.py
index 86e777d7b7..1a44d84fa5 100644
--- a/PathPlanning/DStarLite/d_star_lite.py
+++ b/PathPlanning/DStarLite/d_star_lite.py
@@ -2,7 +2,7 @@
D* Lite grid planning
author: vss2sn (28676655+vss2sn@users.noreply.github.com)
Link to papers:
-D* Lite (Link: http://idm-lab.org/bib/abstracts/papers/aaai02b.pd)
+D* Lite (Link: http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf)
Improved Fast Replanning for Robot Navigation in Unknown Terrain
(Link: http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf)
Implemented maintaining similarity with the pseudocode for understanding.
@@ -63,9 +63,7 @@ def __init__(self, ox: list, oy: list):
self.y_max = int(abs(max(oy) - self.y_min_world))
self.obstacles = [Node(x - self.x_min_world, y - self.y_min_world)
for x, y in zip(ox, oy)]
- self.obstacles_xy = np.array(
- [[obstacle.x, obstacle.y] for obstacle in self.obstacles]
- )
+ self.obstacles_xy = {(obstacle.x, obstacle.y) for obstacle in self.obstacles}
self.start = Node(0, 0)
self.goal = Node(0, 0)
self.U = list() # type: ignore
@@ -73,7 +71,7 @@ def __init__(self, ox: list, oy: list):
self.kold = 0.0
self.rhs = self.create_grid(float("inf"))
self.g = self.create_grid(float("inf"))
- self.detected_obstacles_xy = np.empty((0, 2))
+ self.detected_obstacles_xy: set[tuple[int, int]] = set()
self.xy = np.empty((0, 2))
if show_animation:
self.detected_obstacles_for_plotting_x = list() # type: ignore
@@ -84,18 +82,8 @@ def create_grid(self, val: float):
return np.full((self.x_max, self.y_max), val)
def is_obstacle(self, node: Node):
- x = np.array([node.x])
- y = np.array([node.y])
- obstacle_x_equal = self.obstacles_xy[:, 0] == x
- obstacle_y_equal = self.obstacles_xy[:, 1] == y
- is_in_obstacles = (obstacle_x_equal & obstacle_y_equal).any()
-
- is_in_detected_obstacles = False
- if self.detected_obstacles_xy.shape[0] > 0:
- is_x_equal = self.detected_obstacles_xy[:, 0] == x
- is_y_equal = self.detected_obstacles_xy[:, 1] == y
- is_in_detected_obstacles = (is_x_equal & is_y_equal).any()
-
+ is_in_obstacles = (node.x, node.y) in self.obstacles_xy
+ is_in_detected_obstacles = (node.x, node.y) in self.detected_obstacles_xy
return is_in_obstacles or is_in_detected_obstacles
def c(self, node1: Node, node2: Node):
@@ -157,7 +145,7 @@ def initialize(self, start: Node, goal: Node):
self.g = self.create_grid(math.inf)
self.rhs[self.goal.x][self.goal.y] = 0
self.U.append((self.goal, self.calculate_key(self.goal)))
- self.detected_obstacles_xy = np.empty((0, 2))
+ self.detected_obstacles_xy = set()
def update_vertex(self, u: Node):
if not compare_coordinates(u, self.goal):
@@ -215,12 +203,7 @@ def detect_changes(self):
compare_coordinates(spoofed_obstacle, self.goal):
continue
changed_vertices.append(spoofed_obstacle)
- self.detected_obstacles_xy = np.concatenate(
- (
- self.detected_obstacles_xy,
- [[spoofed_obstacle.x, spoofed_obstacle.y]]
- )
- )
+ self.detected_obstacles_xy.add((spoofed_obstacle.x, spoofed_obstacle.y))
if show_animation:
self.detected_obstacles_for_plotting_x.append(
spoofed_obstacle.x + self.x_min_world)
@@ -241,12 +224,7 @@ def detect_changes(self):
compare_coordinates(new_obs, self.goal):
return changed_vertices
changed_vertices.append(Node(x, y))
- self.detected_obstacles_xy = np.concatenate(
- (
- self.detected_obstacles_xy,
- [[x, y]]
- )
- )
+ self.detected_obstacles_xy.add((x, y))
if show_animation:
self.detected_obstacles_for_plotting_x.append(x +
self.x_min_world)
diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py
index 004e49f15a..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):
"""
@@ -221,20 +221,20 @@ def main():
# set obstacle positions
ox, oy = [], []
for i in range(-10, 60):
- ox.append(i)
+ ox.append(float(i))
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(-10, 61):
ox.append(-10.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(-10, 40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
@@ -246,7 +246,7 @@ def main():
plt.grid(True)
plt.axis("equal")
- dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
+ dijkstra = DijkstraPlanner(ox, oy, grid_size, robot_radius)
rx, ry = dijkstra.planning(sx, sy, gx, gy)
if show_animation: # pragma: no cover
diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py
index 468d3b9f97..9ccd18b7c2 100644
--- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py
+++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py
@@ -9,7 +9,7 @@
More information on Dynamic Movement Primitives available at:
https://arxiv.org/abs/2102.03861
-https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full
+https://www.frontiersin.org/journals/computational-neuroscience/articles/10.3389/fncom.2013.00138/full
"""
@@ -18,7 +18,7 @@
import numpy as np
-class DMP(object):
+class DMP:
def __init__(self, training_data, data_period, K=156.25, B=25):
"""
@@ -215,21 +215,21 @@ def show_DMP_purpose(self):
T_vals = np.linspace(T_orig, 2*T_orig, 20)
for new_q0_value in q0_vals:
- plot_title = "Initial Position = [%s, %s]" % \
- (round(new_q0_value[0], 2), round(new_q0_value[1], 2))
+ plot_title = (f"Initial Position = [{round(new_q0_value[0], 2)},"
+ f" {round(new_q0_value[1], 2)}]")
_, path = self.recreate_trajectory(new_q0_value, g_orig, T_orig)
self.view_trajectory(path, title=plot_title, demo=True)
for new_g_value in g_vals:
- plot_title = "Goal Position = [%s, %s]" % \
- (round(new_g_value[0], 2), round(new_g_value[1], 2))
+ plot_title = (f"Goal Position = [{round(new_g_value[0], 2)},"
+ f" {round(new_g_value[1], 2)}]")
_, path = self.recreate_trajectory(q0_orig, new_g_value, T_orig)
self.view_trajectory(path, title=plot_title, demo=True)
for new_T_value in T_vals:
- plot_title = "Period = %s [sec]" % round(new_T_value, 2)
+ plot_title = f"Period = {round(new_T_value, 2)} [sec]"
_, path = self.recreate_trajectory(q0_orig, g_orig, new_T_value)
self.view_trajectory(path, title=plot_title, demo=True)
@@ -257,5 +257,4 @@ def example_DMP():
if __name__ == '__main__':
-
example_DMP()
diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py
new file mode 100644
index 0000000000..77d4e6e399
--- /dev/null
+++ b/PathPlanning/ElasticBands/elastic_bands.py
@@ -0,0 +1,300 @@
+"""
+Elastic Bands
+
+author: Wang Zheng (@Aglargil)
+
+Reference:
+
+- [Elastic Bands: Connecting Path Planning and Control]
+(http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf)
+"""
+
+import numpy as np
+import sys
+import pathlib
+import matplotlib.pyplot as plt
+from matplotlib.patches import Circle
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
+from Mapping.DistanceMap.distance_map import compute_sdf_scipy
+
+# Elastic Bands Params
+MAX_BUBBLE_RADIUS = 100
+MIN_BUBBLE_RADIUS = 10
+RHO0 = 20.0 # Maximum distance for applying repulsive force
+KC = 0.05 # Contraction force gain
+KR = -0.1 # Repulsive force gain
+LAMBDA = 0.7 # Overlap constraint factor
+STEP_SIZE = 3.0 # Step size for calculating gradient
+
+# Visualization Params
+ENABLE_PLOT = True
+# ENABLE_INTERACTIVE is True allows user to add obstacles by left clicking
+# and add path points by right clicking and start planning by middle clicking
+ENABLE_INTERACTIVE = False
+# ENABLE_SAVE_DATA is True allows saving the path and obstacles which added
+# by user in interactive mode to file
+ENABLE_SAVE_DATA = False
+MAX_ITER = 50
+
+
+class Bubble:
+ def __init__(self, position, radius):
+ self.pos = np.array(position) # Bubble center coordinates [x, y]
+ self.radius = radius # Safety distance radius ρ(b)
+ if self.radius > MAX_BUBBLE_RADIUS:
+ self.radius = MAX_BUBBLE_RADIUS
+ if self.radius < MIN_BUBBLE_RADIUS:
+ self.radius = MIN_BUBBLE_RADIUS
+
+
+class ElasticBands:
+ def __init__(
+ self,
+ initial_path,
+ obstacles,
+ rho0=RHO0,
+ kc=KC,
+ kr=KR,
+ lambda_=LAMBDA,
+ step_size=STEP_SIZE,
+ ):
+ self.distance_map = compute_sdf_scipy(obstacles)
+ self.bubbles = [
+ Bubble(p, self.compute_rho(p)) for p in initial_path
+ ] # Initialize bubble chain
+ self.kc = kc # Contraction force gain
+ self.kr = kr # Repulsive force gain
+ self.rho0 = rho0 # Maximum distance for applying repulsive force
+ self.lambda_ = lambda_ # Overlap constraint factor
+ self.step_size = step_size # Step size for calculating gradient
+ self._maintain_overlap()
+
+ def compute_rho(self, position):
+ """Compute the distance field value at the position"""
+ return self.distance_map[int(position[0]), int(position[1])]
+
+ def contraction_force(self, i):
+ """Calculate internal contraction force for the i-th bubble"""
+ if i == 0 or i == len(self.bubbles) - 1:
+ return np.zeros(2)
+
+ prev = self.bubbles[i - 1].pos
+ next_ = self.bubbles[i + 1].pos
+ current = self.bubbles[i].pos
+
+ # f_c = kc * ( (prev-current)/|prev-current| + (next-current)/|next-current| )
+ dir_prev = (prev - current) / (np.linalg.norm(prev - current) + 1e-6)
+ dir_next = (next_ - current) / (np.linalg.norm(next_ - current) + 1e-6)
+ return self.kc * (dir_prev + dir_next)
+
+ def repulsive_force(self, i):
+ """Calculate external repulsive force for the i-th bubble"""
+ h = self.step_size # Step size
+ b = self.bubbles[i].pos
+ rho = self.bubbles[i].radius
+
+ if rho >= self.rho0:
+ return np.zeros(2)
+
+ # Finite difference approximation of the gradient ∂ρ/∂b
+ dx = np.array([h, 0])
+ dy = np.array([0, h])
+ grad_x = (self.compute_rho(b - dx) - self.compute_rho(b + dx)) / (2 * h)
+ grad_y = (self.compute_rho(b - dy) - self.compute_rho(b + dy)) / (2 * h)
+ grad = np.array([grad_x, grad_y])
+
+ return self.kr * (self.rho0 - rho) * grad
+
+ def update_bubbles(self):
+ """Update bubble positions"""
+ new_bubbles = []
+ for i in range(len(self.bubbles)):
+ if i == 0 or i == len(self.bubbles) - 1:
+ new_bubbles.append(self.bubbles[i]) # Fixed start and end points
+ continue
+
+ f_total = self.contraction_force(i) + self.repulsive_force(i)
+ v = self.bubbles[i - 1].pos - self.bubbles[i + 1].pos
+
+ # Remove tangential component
+ f_star = f_total - f_total * v * v / (np.linalg.norm(v) ** 2 + 1e-6)
+
+ alpha = self.bubbles[i].radius # Adaptive step size
+ new_pos = self.bubbles[i].pos + alpha * f_star
+ new_pos = np.clip(new_pos, 0, 499)
+ new_radius = self.compute_rho(new_pos)
+
+ # Update bubble and maintain overlap constraint
+ new_bubble = Bubble(new_pos, new_radius)
+ new_bubbles.append(new_bubble)
+
+ self.bubbles = new_bubbles
+ self._maintain_overlap()
+
+ def _maintain_overlap(self):
+ """Maintain bubble chain continuity (simplified insertion/deletion mechanism)"""
+ # Insert bubbles
+ i = 0
+ while i < len(self.bubbles) - 1:
+ bi, bj = self.bubbles[i], self.bubbles[i + 1]
+ dist = np.linalg.norm(bi.pos - bj.pos)
+ if dist > self.lambda_ * (bi.radius + bj.radius):
+ new_pos = (bi.pos + bj.pos) / 2
+ rho = self.compute_rho(
+ new_pos
+ ) # Calculate new radius using environment model
+ self.bubbles.insert(i + 1, Bubble(new_pos, rho))
+ i += 2 # Skip the processed region
+ else:
+ i += 1
+
+ # Delete redundant bubbles
+ i = 1
+ while i < len(self.bubbles) - 1:
+ prev = self.bubbles[i - 1]
+ next_ = self.bubbles[i + 1]
+ dist = np.linalg.norm(prev.pos - next_.pos)
+ if dist <= self.lambda_ * (prev.radius + next_.radius):
+ del self.bubbles[i] # Delete if redundant
+ else:
+ i += 1
+
+
+class ElasticBandsVisualizer:
+ def __init__(self):
+ self.obstacles = np.zeros((500, 500))
+ self.obstacles_points = []
+ self.path_points = []
+ self.elastic_band = None
+ self.running = True
+
+ if ENABLE_PLOT:
+ self.fig, self.ax = plt.subplots(figsize=(8, 8))
+ self.fig.canvas.mpl_connect("close_event", self.on_close)
+ self.ax.set_xlim(0, 500)
+ self.ax.set_ylim(0, 500)
+
+ if ENABLE_INTERACTIVE:
+ self.path_points = [] # Add a list to store path points
+ # Connect mouse events
+ self.fig.canvas.mpl_connect("button_press_event", self.on_click)
+ else:
+ self.path_points = np.load(pathlib.Path(__file__).parent / "path.npy")
+ self.obstacles_points = np.load(
+ pathlib.Path(__file__).parent / "obstacles.npy"
+ )
+ for x, y in self.obstacles_points:
+ self.add_obstacle(x, y)
+ self.plan_path()
+
+ self.plot_background()
+
+ def on_close(self, event):
+ """Handle window close event"""
+ self.running = False
+ plt.close("all") # Close all figure windows
+
+ def plot_background(self):
+ """Plot the background grid"""
+ if not ENABLE_PLOT or not self.running:
+ return
+
+ self.ax.cla()
+ self.ax.set_xlim(0, 500)
+ self.ax.set_ylim(0, 500)
+ self.ax.grid(True)
+
+ if ENABLE_INTERACTIVE:
+ self.ax.set_title(
+ "Elastic Bands Path Planning\n"
+ "Left click: Add obstacles\n"
+ "Right click: Add path points\n"
+ "Middle click: Start planning",
+ pad=20,
+ )
+ else:
+ self.ax.set_title("Elastic Bands Path Planning", pad=20)
+
+ if self.path_points:
+ self.ax.plot(
+ [p[0] for p in self.path_points],
+ [p[1] for p in self.path_points],
+ "yo",
+ markersize=8,
+ )
+
+ self.ax.imshow(self.obstacles.T, origin="lower", cmap="binary", alpha=0.8)
+ self.ax.plot([], [], color="black", label="obstacles")
+ if self.elastic_band is not None:
+ path = [b.pos.tolist() for b in self.elastic_band.bubbles]
+ path = np.array(path)
+ self.ax.plot(path[:, 0], path[:, 1], "b-", linewidth=2, label="path")
+
+ for bubble in self.elastic_band.bubbles:
+ circle = Circle(
+ bubble.pos, bubble.radius, fill=False, color="g", alpha=0.3
+ )
+ self.ax.add_patch(circle)
+ self.ax.plot(bubble.pos[0], bubble.pos[1], "bo", markersize=10)
+ self.ax.plot([], [], color="green", label="bubbles")
+
+ self.ax.legend(loc="upper right")
+ plt.draw()
+ plt.pause(0.01)
+
+ def add_obstacle(self, x, y):
+ """Add an obstacle at the given coordinates"""
+ size = 30 # Side length of the square
+ half_size = size // 2
+ x_start = max(0, x - half_size)
+ x_end = min(self.obstacles.shape[0], x + half_size)
+ y_start = max(0, y - half_size)
+ y_end = min(self.obstacles.shape[1], y + half_size)
+ self.obstacles[x_start:x_end, y_start:y_end] = 1
+
+ def on_click(self, event):
+ """Handle mouse click events"""
+ if event.inaxes != self.ax:
+ return
+
+ x, y = int(event.xdata), int(event.ydata)
+
+ if event.button == 1: # Left click to add obstacles
+ self.add_obstacle(x, y)
+ self.obstacles_points.append([x, y])
+
+ elif event.button == 3: # Right click to add path points
+ self.path_points.append([x, y])
+
+ elif event.button == 2: # Middle click to end path input and start planning
+ if len(self.path_points) >= 2:
+ if ENABLE_SAVE_DATA:
+ np.save(
+ pathlib.Path(__file__).parent / "path.npy", self.path_points
+ )
+ np.save(
+ pathlib.Path(__file__).parent / "obstacles.npy",
+ self.obstacles_points,
+ )
+ self.plan_path()
+
+ self.plot_background()
+
+ def plan_path(self):
+ """Plan the path"""
+
+ initial_path = self.path_points
+ # Create an elastic band object and optimize
+ self.elastic_band = ElasticBands(initial_path, self.obstacles)
+ for _ in range(MAX_ITER):
+ self.elastic_band.update_bubbles()
+ self.path_points = [b.pos for b in self.elastic_band.bubbles]
+ self.plot_background()
+
+
+if __name__ == "__main__":
+ _ = ElasticBandsVisualizer()
+ if ENABLE_PLOT:
+ plt.show(block=True)
diff --git a/PathPlanning/ElasticBands/obstacles.npy b/PathPlanning/ElasticBands/obstacles.npy
new file mode 100644
index 0000000000..af4376afcf
Binary files /dev/null and b/PathPlanning/ElasticBands/obstacles.npy differ
diff --git a/PathPlanning/ElasticBands/path.npy b/PathPlanning/ElasticBands/path.npy
new file mode 100644
index 0000000000..be7c253d65
Binary files /dev/null and b/PathPlanning/ElasticBands/path.npy differ
diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
index dc07d3c84b..3f685e512f 100644
--- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py
+++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
@@ -5,7 +5,7 @@
author: Joe Dinius, Ph.D (https://jwdinius.github.io)
Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
(https://ieeexplore.ieee.org/document/4339545/)
diff --git a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
index d034cb8a32..e72d33261e 100644
--- a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
+++ b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
@@ -6,7 +6,7 @@
Atsushi Sakai (@Atsushi_twi)
Refs:
-- https://jwdinius.github.io/blog/2018/eta3traj
+- https://jwdinius.github.io/blog/2018/eta3traj/
- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
(https://ieeexplore.ieee.org/document/4339545/)
@@ -26,11 +26,10 @@
class MaxVelocityNotReached(Exception):
def __init__(self, actual_vel, max_vel):
- self.message = 'Actual velocity {} does not equal desired max velocity {}!'.format(
- actual_vel, max_vel)
+ self.message = f'Actual velocity {actual_vel} does not equal desired max velocity {max_vel}!'
-class eta3_trajectory(Eta3Path):
+class Eta3SplineTrajectory(Eta3Path):
"""
eta3_trajectory
@@ -42,7 +41,7 @@ def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.
# ensure that all inputs obey the assumptions of the model
assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \
and a0 <= max_accel and v0 <= max_vel
- super(eta3_trajectory, self).__init__(segments=segments)
+ super(__class__, self).__init__(segments=segments)
self.total_length = sum([s.segment_length for s in self.segments])
self.max_vel = float(max_vel)
self.v0 = float(v0)
@@ -166,7 +165,7 @@ def velocity_profile(self):
try:
assert np.isclose(self.vels[index], 0)
except AssertionError as e:
- print('The final velocity {} is not zero'.format(self.vels[index]))
+ print(f'The final velocity {self.vels[index]} is not zero')
raise e
self.seg_lengths[index] = s_sf
@@ -301,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)
@@ -335,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)
@@ -401,8 +400,8 @@ def test3(max_vel=2.0):
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# construct the whole path
- traj = eta3_trajectory(trajectory_segments,
- max_vel=max_vel, max_accel=0.5, max_jerk=1)
+ traj = Eta3SplineTrajectory(trajectory_segments,
+ max_vel=max_vel, max_accel=0.5, max_jerk=1)
# interpolate at several points along the path
times = np.linspace(0, traj.total_time, 1001)
diff --git a/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py
new file mode 100644
index 0000000000..482712ceaf
--- /dev/null
+++ b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py
@@ -0,0 +1,144 @@
+"""
+
+A converter between Cartesian and Frenet coordinate systems
+
+author: Wang Zheng (@Aglargil)
+
+Reference:
+
+- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
+(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
+
+"""
+
+import math
+
+
+class CartesianFrenetConverter:
+ """
+ A class for converting states between Cartesian and Frenet coordinate systems
+ """
+
+ @ staticmethod
+ def cartesian_to_frenet(rs, rx, ry, rtheta, rkappa, rdkappa, x, y, v, a, theta, kappa):
+ """
+ Convert state from Cartesian coordinate to Frenet coordinate
+
+ Parameters
+ ----------
+ rs: reference line s-coordinate
+ rx, ry: reference point coordinates
+ rtheta: reference point heading
+ rkappa: reference point curvature
+ rdkappa: reference point curvature rate
+ x, y: current position
+ v: velocity
+ a: acceleration
+ theta: heading angle
+ kappa: curvature
+
+ Returns
+ -------
+ s_condition: [s(t), s'(t), s''(t)]
+ d_condition: [d(s), d'(s), d''(s)]
+ """
+ dx = x - rx
+ dy = y - ry
+
+ cos_theta_r = math.cos(rtheta)
+ sin_theta_r = math.sin(rtheta)
+
+ cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx
+ d = math.copysign(math.hypot(dx, dy), cross_rd_nd)
+
+ delta_theta = theta - rtheta
+ tan_delta_theta = math.tan(delta_theta)
+ cos_delta_theta = math.cos(delta_theta)
+
+ one_minus_kappa_r_d = 1 - rkappa * d
+ d_dot = one_minus_kappa_r_d * tan_delta_theta
+
+ kappa_r_d_prime = rdkappa * d + rkappa * d_dot
+
+ d_ddot = (-kappa_r_d_prime * tan_delta_theta +
+ one_minus_kappa_r_d / (cos_delta_theta * cos_delta_theta) *
+ (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa))
+
+ s = rs
+ s_dot = v * cos_delta_theta / one_minus_kappa_r_d
+
+ delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa
+ s_ddot = (a * cos_delta_theta -
+ s_dot * s_dot *
+ (d_dot * delta_theta_prime - kappa_r_d_prime)) / one_minus_kappa_r_d
+
+ return [s, s_dot, s_ddot], [d, d_dot, d_ddot]
+
+ @ staticmethod
+ def frenet_to_cartesian(rs, rx, ry, rtheta, rkappa, rdkappa, s_condition, d_condition):
+ """
+ Convert state from Frenet coordinate to Cartesian coordinate
+
+ Parameters
+ ----------
+ rs: reference line s-coordinate
+ rx, ry: reference point coordinates
+ rtheta: reference point heading
+ rkappa: reference point curvature
+ rdkappa: reference point curvature rate
+ s_condition: [s(t), s'(t), s''(t)]
+ d_condition: [d(s), d'(s), d''(s)]
+
+ Returns
+ -------
+ x, y: position
+ theta: heading angle
+ kappa: curvature
+ v: velocity
+ a: acceleration
+ """
+ if abs(rs - s_condition[0]) >= 1.0e-6:
+ raise ValueError(
+ "The reference point s and s_condition[0] don't match")
+
+ cos_theta_r = math.cos(rtheta)
+ sin_theta_r = math.sin(rtheta)
+
+ x = rx - sin_theta_r * d_condition[0]
+ y = ry + cos_theta_r * d_condition[0]
+
+ one_minus_kappa_r_d = 1 - rkappa * d_condition[0]
+
+ tan_delta_theta = d_condition[1] / one_minus_kappa_r_d
+ delta_theta = math.atan2(d_condition[1], one_minus_kappa_r_d)
+ cos_delta_theta = math.cos(delta_theta)
+
+ theta = CartesianFrenetConverter.normalize_angle(delta_theta + rtheta)
+
+ kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1]
+
+ kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *
+ cos_delta_theta * cos_delta_theta) / one_minus_kappa_r_d + rkappa) * \
+ cos_delta_theta / one_minus_kappa_r_d
+
+ d_dot = d_condition[1] * s_condition[1]
+ v = math.sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *
+ s_condition[1] * s_condition[1] + d_dot * d_dot)
+
+ delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa
+
+ a = (s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +
+ s_condition[1] * s_condition[1] / cos_delta_theta *
+ (d_condition[1] * delta_theta_prime - kappa_r_d_prime))
+
+ return x, y, theta, kappa, v, a
+
+ @ staticmethod
+ def normalize_angle(angle):
+ """
+ Normalize angle to [-pi, pi]
+ """
+ a = math.fmod(angle + math.pi, 2.0 * math.pi)
+ if a < 0.0:
+ a += 2.0 * math.pi
+ return a - math.pi
diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
index 331df36309..4b82fb70fd 100644
--- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
+++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
@@ -4,7 +4,7 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
@@ -17,43 +17,314 @@
import numpy as np
import matplotlib.pyplot as plt
import copy
-import math
import sys
import pathlib
+
sys.path.append(str(pathlib.Path(__file__).parent.parent))
-from QuinticPolynomialsPlanner.quintic_polynomials_planner import \
- QuinticPolynomial
+from QuinticPolynomialsPlanner.quintic_polynomials_planner import QuinticPolynomial
from CubicSpline import cubic_spline_planner
-SIM_LOOP = 500
+from enum import Enum, auto
+from FrenetOptimalTrajectory.cartesian_frenet_converter import (
+ CartesianFrenetConverter,
+)
+
+
+class LateralMovement(Enum):
+ HIGH_SPEED = auto()
+ LOW_SPEED = auto()
+
+
+class LongitudinalMovement(Enum):
+ MERGING_AND_STOPPING = auto()
+ VELOCITY_KEEPING = auto()
+
+
+# Default Parameters
+
+LATERAL_MOVEMENT = LateralMovement.HIGH_SPEED
+LONGITUDINAL_MOVEMENT = LongitudinalMovement.VELOCITY_KEEPING
-# Parameter
MAX_SPEED = 50.0 / 3.6 # maximum speed [m/s]
-MAX_ACCEL = 2.0 # maximum acceleration [m/ss]
+MAX_ACCEL = 5.0 # maximum acceleration [m/ss]
MAX_CURVATURE = 1.0 # maximum curvature [1/m]
-MAX_ROAD_WIDTH = 7.0 # maximum road width [m]
-D_ROAD_W = 1.0 # road width sampling length [m]
DT = 0.2 # time tick [s]
MAX_T = 5.0 # max prediction time [m]
MIN_T = 4.0 # min prediction time [m]
-TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]
-D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]
N_S_SAMPLE = 1 # sampling number of target speed
-ROBOT_RADIUS = 2.0 # robot radius [m]
# cost weights
K_J = 0.1
K_T = 0.1
+K_S_DOT = 1.0
K_D = 1.0
+K_S = 1.0
K_LAT = 1.0
K_LON = 1.0
+SIM_LOOP = 500
show_animation = True
-class QuarticPolynomial:
+if LATERAL_MOVEMENT == LateralMovement.LOW_SPEED:
+ MAX_ROAD_WIDTH = 1.0 # maximum road width [m]
+ D_ROAD_W = 0.2 # road width sampling length [m]
+ TARGET_SPEED = 3.0 / 3.6 # maximum speed [m/s]
+ D_T_S = 0.5 / 3.6 # target speed sampling length [m/s]
+ # Waypoints
+ WX = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0]
+ WY = [0.0, 0.0, 1.0, 0.0, -1.0, -2.0]
+ OBSTACLES = np.array([[3.0, 1.0], [5.0, -0.0], [6.0, 0.5], [8.0, -1.5]])
+ ROBOT_RADIUS = 0.5 # robot radius [m]
+
+ # Initial state parameters
+ INITIAL_SPEED = 1.0 / 3.6 # current speed [m/s]
+ INITIAL_ACCEL = 0.0 # current acceleration [m/ss]
+ INITIAL_LAT_POSITION = 0.5 # current lateral position [m]
+ INITIAL_LAT_SPEED = 0.0 # current lateral speed [m/s]
+ INITIAL_LAT_ACCELERATION = 0.0 # current lateral acceleration [m/s]
+ INITIAL_COURSE_POSITION = 0.0 # current course position
+
+ ANIMATION_AREA = 5.0 # Animation area length [m]
+
+ STOP_S = 4.0 # Merge and stop distance [m]
+ D_S = 0.3 # Stop point sampling length [m]
+ N_STOP_S_SAMPLE = 3 # Stop point sampling number
+else:
+ MAX_ROAD_WIDTH = 7.0 # maximum road width [m]
+ D_ROAD_W = 1.0 # road width sampling length [m]
+ TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]
+ D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]
+ # Waypoints
+ WX = [0.0, 10.0, 20.5, 35.0, 70.5]
+ WY = [0.0, -6.0, 5.0, 6.5, 0.0]
+ # Obstacle list
+ OBSTACLES = np.array(
+ [[20.0, 10.0], [30.0, 6.0], [30.0, 8.0], [35.0, 8.0], [50.0, 3.0]]
+ )
+ ROBOT_RADIUS = 2.0 # robot radius [m]
+
+ # Initial state parameters
+ INITIAL_SPEED = 10.0 / 3.6 # current speed [m/s]
+ INITIAL_ACCEL = 0.0 # current acceleration [m/ss]
+ INITIAL_LAT_POSITION = 2.0 # current lateral position [m]
+ INITIAL_LAT_SPEED = 0.0 # current lateral speed [m/s]
+ INITIAL_LAT_ACCELERATION = 0.0 # current lateral acceleration [m/s]
+ INITIAL_COURSE_POSITION = 0.0 # current course position
+
+ ANIMATION_AREA = 20.0 # Animation area length [m]
+ STOP_S = 25.0 # Merge and stop distance [m]
+ D_S = 2 # Stop point sampling length [m]
+ N_STOP_S_SAMPLE = 4 # Stop point sampling number
+
+
+class LateralMovementStrategy:
+ def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti):
+ """
+ Calculate the lateral trajectory
+ """
+ raise NotImplementedError("calc_lateral_trajectory not implemented")
+
+ def calc_cartesian_parameters(self, fp, csp):
+ """
+ Calculate the cartesian parameters (x, y, yaw, curvature, v, a)
+ """
+ raise NotImplementedError("calc_cartesian_parameters not implemented")
+
+
+class HighSpeedLateralMovementStrategy(LateralMovementStrategy):
+ def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti):
+ tp = copy.deepcopy(fp)
+ s0_d = fp.s_d[0]
+ s0_dd = fp.s_dd[0]
+ # d'(t) = d'(s) * s'(t)
+ # d''(t) = d''(s) * s'(t)^2 + d'(s) * s''(t)
+ lat_qp = QuinticPolynomial(
+ c_d, c_d_d * s0_d, c_d_dd * s0_d**2 + c_d_d * s0_dd, di, 0.0, 0.0, Ti
+ )
+
+ tp.d = []
+ tp.d_d = []
+ tp.d_dd = []
+ tp.d_ddd = []
+
+ # Calculate all derivatives in a single loop to reduce iterations
+ for i in range(len(fp.t)):
+ t = fp.t[i]
+ s_d = fp.s_d[i]
+ s_dd = fp.s_dd[i]
+
+ s_d_inv = 1.0 / (s_d + 1e-6) + 1e-6 # Avoid division by zero
+ s_d_inv_sq = s_d_inv * s_d_inv # Square of inverse
+
+ d = lat_qp.calc_point(t)
+ d_d = lat_qp.calc_first_derivative(t)
+ d_dd = lat_qp.calc_second_derivative(t)
+ d_ddd = lat_qp.calc_third_derivative(t)
+
+ tp.d.append(d)
+ # d'(s) = d'(t) / s'(t)
+ tp.d_d.append(d_d * s_d_inv)
+ # d''(s) = (d''(t) - d'(s) * s''(t)) / s'(t)^2
+ tp.d_dd.append((d_dd - tp.d_d[i] * s_dd) * s_d_inv_sq)
+ tp.d_ddd.append(d_ddd)
+
+ return tp
+
+ def calc_cartesian_parameters(self, fp, csp):
+ # calc global positions
+ for i in range(len(fp.s)):
+ ix, iy = csp.calc_position(fp.s[i])
+ if ix is None:
+ break
+ i_yaw = csp.calc_yaw(fp.s[i])
+ i_kappa = csp.calc_curvature(fp.s[i])
+ i_dkappa = csp.calc_curvature_rate(fp.s[i])
+ s_condition = [fp.s[i], fp.s_d[i], fp.s_dd[i]]
+ d_condition = [
+ fp.d[i],
+ fp.d_d[i],
+ fp.d_dd[i],
+ ]
+ x, y, theta, kappa, v, a = CartesianFrenetConverter.frenet_to_cartesian(
+ fp.s[i], ix, iy, i_yaw, i_kappa, i_dkappa, s_condition, d_condition
+ )
+ fp.x.append(x)
+ fp.y.append(y)
+ fp.yaw.append(theta)
+ fp.c.append(kappa)
+ fp.v.append(v)
+ fp.a.append(a)
+ return fp
+
+
+class LowSpeedLateralMovementStrategy(LateralMovementStrategy):
+ def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti):
+ s0 = fp.s[0]
+ s1 = fp.s[-1]
+ tp = copy.deepcopy(fp)
+ # d = d(s), d_d = d'(s), d_dd = d''(s)
+ # * shift s range from [s0, s1] to [0, s1 - s0]
+ lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, s1 - s0)
+
+ tp.d = [lat_qp.calc_point(s - s0) for s in fp.s]
+ tp.d_d = [lat_qp.calc_first_derivative(s - s0) for s in fp.s]
+ tp.d_dd = [lat_qp.calc_second_derivative(s - s0) for s in fp.s]
+ tp.d_ddd = [lat_qp.calc_third_derivative(s - s0) for s in fp.s]
+ return tp
+
+ def calc_cartesian_parameters(self, fp, csp):
+ # calc global positions
+ for i in range(len(fp.s)):
+ ix, iy = csp.calc_position(fp.s[i])
+ if ix is None:
+ break
+ i_yaw = csp.calc_yaw(fp.s[i])
+ i_kappa = csp.calc_curvature(fp.s[i])
+ i_dkappa = csp.calc_curvature_rate(fp.s[i])
+ s_condition = [fp.s[i], fp.s_d[i], fp.s_dd[i]]
+ d_condition = [fp.d[i], fp.d_d[i], fp.d_dd[i]]
+ x, y, theta, kappa, v, a = CartesianFrenetConverter.frenet_to_cartesian(
+ fp.s[i], ix, iy, i_yaw, i_kappa, i_dkappa, s_condition, d_condition
+ )
+ fp.x.append(x)
+ fp.y.append(y)
+ fp.yaw.append(theta)
+ fp.c.append(kappa)
+ fp.v.append(v)
+ fp.a.append(a)
+ return fp
+
+
+class LongitudinalMovementStrategy:
+ def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0):
+ """
+ Calculate the longitudinal trajectory
+ """
+ raise NotImplementedError("calc_longitudinal_trajectory not implemented")
+
+ def get_d_arrange(self, s0):
+ """
+ Get the d sample range
+ """
+ raise NotImplementedError("get_d_arrange not implemented")
+
+ def calc_destination_cost(self, fp):
+ """
+ Calculate the destination cost
+ """
+ raise NotImplementedError("calc_destination_cost not implemented")
+
+
+class VelocityKeepingLongitudinalMovementStrategy(LongitudinalMovementStrategy):
+ def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0):
+ fplist = []
+ for tv in np.arange(
+ TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S
+ ):
+ fp = FrenetPath()
+ lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)
+ fp.t = [t for t in np.arange(0.0, Ti, DT)]
+ fp.s = [lon_qp.calc_point(t) for t in fp.t]
+ fp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
+ fp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
+ fp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
+ fplist.append(fp)
+ return fplist
+
+ def get_d_arrange(self, s0):
+ return np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W)
+
+ def calc_destination_cost(self, fp):
+ ds = (TARGET_SPEED - fp.s_d[-1]) ** 2
+ return K_S_DOT * ds
+
+
+class MergingAndStoppingLongitudinalMovementStrategy(LongitudinalMovementStrategy):
+ def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0):
+ if s0 >= STOP_S:
+ return []
+ fplist = []
+ for s in np.arange(
+ STOP_S - D_S * N_STOP_S_SAMPLE, STOP_S + D_S * N_STOP_S_SAMPLE, D_S
+ ):
+ fp = FrenetPath()
+ lon_qp = QuinticPolynomial(s0, c_speed, c_accel, s, 0.0, 0.0, Ti)
+ fp.t = [t for t in np.arange(0.0, Ti, DT)]
+ fp.s = [lon_qp.calc_point(t) for t in fp.t]
+ fp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
+ fp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
+ fp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
+ fplist.append(fp)
+ return fplist
+
+ def get_d_arrange(self, s0):
+ # Only if s0 is less than STOP_S / 3, then we sample the road width
+ if s0 < STOP_S / 3:
+ return np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W)
+ else:
+ return [0.0]
+
+ def calc_destination_cost(self, fp):
+ ds = (STOP_S - fp.s[-1]) ** 2
+ return K_S * ds
+
+LATERAL_MOVEMENT_STRATEGY: LateralMovementStrategy
+LONGITUDINAL_MOVEMENT_STRATEGY: LongitudinalMovementStrategy
+
+if LATERAL_MOVEMENT == LateralMovement.HIGH_SPEED:
+ LATERAL_MOVEMENT_STRATEGY = HighSpeedLateralMovementStrategy()
+else:
+ LATERAL_MOVEMENT_STRATEGY = LowSpeedLateralMovementStrategy()
+
+if LONGITUDINAL_MOVEMENT == LongitudinalMovement.VELOCITY_KEEPING:
+ LONGITUDINAL_MOVEMENT_STRATEGY = VelocityKeepingLongitudinalMovementStrategy()
+else:
+ LONGITUDINAL_MOVEMENT_STRATEGY = MergingAndStoppingLongitudinalMovementStrategy()
+
+class QuarticPolynomial:
def __init__(self, xs, vxs, axs, vxe, axe, time):
# calc coefficient of quartic polynomial
@@ -61,29 +332,25 @@ def __init__(self, xs, vxs, axs, vxe, axe, time):
self.a1 = vxs
self.a2 = axs / 2.0
- A = np.array([[3 * time ** 2, 4 * time ** 3],
- [6 * time, 12 * time ** 2]])
- b = np.array([vxe - self.a1 - 2 * self.a2 * time,
- axe - 2 * self.a2])
+ A = np.array([[3 * time**2, 4 * time**3], [6 * time, 12 * time**2]])
+ b = np.array([vxe - self.a1 - 2 * self.a2 * time, axe - 2 * self.a2])
x = np.linalg.solve(A, b)
self.a3 = x[0]
self.a4 = x[1]
def calc_point(self, t):
- xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
- self.a3 * t ** 3 + self.a4 * t ** 4
+ xt = self.a0 + self.a1 * t + self.a2 * t**2 + self.a3 * t**3 + self.a4 * t**4
return xt
def calc_first_derivative(self, t):
- xt = self.a1 + 2 * self.a2 * t + \
- 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
+ xt = self.a1 + 2 * self.a2 * t + 3 * self.a3 * t**2 + 4 * self.a4 * t**3
return xt
def calc_second_derivative(self, t):
- xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
+ xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2
return xt
@@ -94,111 +361,85 @@ def calc_third_derivative(self, t):
class FrenetPath:
-
def __init__(self):
self.t = []
self.d = []
- self.d_d = []
- self.d_dd = []
- self.d_ddd = []
+ self.d_d = [] # d'(s)
+ self.d_dd = [] # d''(s)
+ self.d_ddd = [] # d'''(t) in low speed / d'''(s) in high speed
self.s = []
- self.s_d = []
- self.s_dd = []
- self.s_ddd = []
- self.cd = 0.0
- self.cv = 0.0
+ self.s_d = [] # s'(t)
+ self.s_dd = [] # s''(t)
+ self.s_ddd = [] # s'''(t)
self.cf = 0.0
self.x = []
self.y = []
self.yaw = []
+ self.v = []
+ self.a = []
self.ds = []
self.c = []
-
-def calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0):
+ def pop_front(self):
+ self.x.pop(0)
+ self.y.pop(0)
+ self.yaw.pop(0)
+ self.v.pop(0)
+ self.a.pop(0)
+ self.s.pop(0)
+ self.s_d.pop(0)
+ self.s_dd.pop(0)
+ self.s_ddd.pop(0)
+ self.d.pop(0)
+ self.d_d.pop(0)
+ self.d_dd.pop(0)
+ self.d_ddd.pop(0)
+
+
+def calc_frenet_paths(c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, s0):
frenet_paths = []
- # generate path to each offset goal
- for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
-
- # Lateral motion planning
- for Ti in np.arange(MIN_T, MAX_T, DT):
- fp = FrenetPath()
-
- # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
- lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
-
- fp.t = [t for t in np.arange(0.0, Ti, DT)]
- fp.d = [lat_qp.calc_point(t) for t in fp.t]
- fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
- fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
- fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
-
- # Longitudinal motion planning (Velocity keeping)
- for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
- TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
- tfp = copy.deepcopy(fp)
- lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)
-
- tfp.s = [lon_qp.calc_point(t) for t in fp.t]
- tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
- tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
- tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
-
- Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk
- Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
-
- # square of diff from target speed
- ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
-
- tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
- tfp.cv = K_J * Js + K_T * Ti + K_D * ds
- tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
-
- frenet_paths.append(tfp)
+ for Ti in np.arange(MIN_T, MAX_T, DT):
+ lon_paths = LONGITUDINAL_MOVEMENT_STRATEGY.calc_longitudinal_trajectory(
+ c_s_d, c_s_dd, Ti, s0
+ )
+
+ for fp in lon_paths:
+ for di in LONGITUDINAL_MOVEMENT_STRATEGY.get_d_arrange(s0):
+ tp = LATERAL_MOVEMENT_STRATEGY.calc_lateral_trajectory(
+ fp, di, c_d, c_d_d, c_d_dd, Ti
+ )
+
+ Jp = sum(np.power(tp.d_ddd, 2)) # square of jerk
+ Js = sum(np.power(tp.s_ddd, 2)) # square of jerk
+
+ lat_cost = K_J * Jp + K_T * Ti + K_D * tp.d[-1] ** 2
+ lon_cost = (
+ K_J * Js
+ + K_T * Ti
+ + LONGITUDINAL_MOVEMENT_STRATEGY.calc_destination_cost(tp)
+ )
+ tp.cf = K_LAT * lat_cost + K_LON * lon_cost
+ frenet_paths.append(tp)
return frenet_paths
def calc_global_paths(fplist, csp):
- for fp in fplist:
-
- # calc global positions
- for i in range(len(fp.s)):
- ix, iy = csp.calc_position(fp.s[i])
- if ix is None:
- break
- i_yaw = csp.calc_yaw(fp.s[i])
- di = fp.d[i]
- fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
- fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
- fp.x.append(fx)
- fp.y.append(fy)
-
- # calc yaw and ds
- for i in range(len(fp.x) - 1):
- dx = fp.x[i + 1] - fp.x[i]
- dy = fp.y[i + 1] - fp.y[i]
- fp.yaw.append(math.atan2(dy, dx))
- fp.ds.append(math.hypot(dx, dy))
-
- fp.yaw.append(fp.yaw[-1])
- fp.ds.append(fp.ds[-1])
-
- # calc curvature
- for i in range(len(fp.yaw) - 1):
- fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])
-
- return fplist
+ return [
+ LATERAL_MOVEMENT_STRATEGY.calc_cartesian_parameters(fp, csp) for fp in fplist
+ ]
def check_collision(fp, ob):
for i in range(len(ob[:, 0])):
- d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
- for (ix, iy) in zip(fp.x, fp.y)]
+ d = [
+ ((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
+ for (ix, iy) in zip(fp.x, fp.y)
+ ]
- collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
+ collision = any([di <= ROBOT_RADIUS**2 for di in d])
if collision:
return False
@@ -207,38 +448,41 @@ def check_collision(fp, ob):
def check_paths(fplist, ob):
- ok_ind = []
+ path_dict = {
+ "max_speed_error": [],
+ "max_accel_error": [],
+ "max_curvature_error": [],
+ "collision_error": [],
+ "ok": [],
+ }
for i, _ in enumerate(fplist):
- if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check
- continue
- elif any([abs(a) > MAX_ACCEL for a in
- fplist[i].s_dd]): # Max accel check
- continue
- elif any([abs(c) > MAX_CURVATURE for c in
- fplist[i].c]): # Max curvature check
- continue
+ if any([v > MAX_SPEED for v in fplist[i].v]): # Max speed check
+ path_dict["max_speed_error"].append(fplist[i])
+ elif any([abs(a) > MAX_ACCEL for a in fplist[i].a]): # Max accel check
+ path_dict["max_accel_error"].append(fplist[i])
+ elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check
+ path_dict["max_curvature_error"].append(fplist[i])
elif not check_collision(fplist[i], ob):
- continue
+ path_dict["collision_error"].append(fplist[i])
+ else:
+ path_dict["ok"].append(fplist[i])
+ return path_dict
- ok_ind.append(i)
- return [fplist[i] for i in ok_ind]
-
-
-def frenet_optimal_planning(csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob):
- fplist = calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0)
+def frenet_optimal_planning(csp, s0, c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, ob):
+ fplist = calc_frenet_paths(c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, s0)
fplist = calc_global_paths(fplist, csp)
- fplist = check_paths(fplist, ob)
+ fpdict = check_paths(fplist, ob)
# find minimum cost path
min_cost = float("inf")
best_path = None
- for fp in fplist:
+ for fp in fpdict["ok"]:
if min_cost >= fp.cf:
min_cost = fp.cf
best_path = fp
- return best_path
+ return [best_path, fpdict]
def generate_target_course(x, y):
@@ -259,40 +503,39 @@ def generate_target_course(x, y):
def main():
print(__file__ + " start!!")
- # way points
- wx = [0.0, 10.0, 20.5, 35.0, 70.5]
- wy = [0.0, -6.0, 5.0, 6.5, 0.0]
- # obstacle lists
- ob = np.array([[20.0, 10.0],
- [30.0, 6.0],
- [30.0, 8.0],
- [35.0, 8.0],
- [50.0, 3.0]
- ])
-
- tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)
-
- # initial state
- c_speed = 10.0 / 3.6 # current speed [m/s]
- c_accel = 0.0 # current acceleration [m/ss]
- c_d = 2.0 # current lateral position [m]
- c_d_d = 0.0 # current lateral speed [m/s]
- c_d_dd = 0.0 # current lateral acceleration [m/s]
- s0 = 0.0 # current course position
-
- area = 20.0 # animation area length [m]
+ tx, ty, tyaw, tc, csp = generate_target_course(WX, WY)
+
+ # Initialize state using global parameters
+ c_s_d = INITIAL_SPEED
+ c_s_dd = INITIAL_ACCEL
+ c_d = INITIAL_LAT_POSITION
+ c_d_d = INITIAL_LAT_SPEED
+ c_d_dd = INITIAL_LAT_ACCELERATION
+ s0 = INITIAL_COURSE_POSITION
+
+ area = ANIMATION_AREA
+
+ last_path = None
for i in range(SIM_LOOP):
- path = frenet_optimal_planning(
- csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob)
+ [path, fpdict] = frenet_optimal_planning(
+ csp, s0, c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, OBSTACLES
+ )
+
+ if path is None:
+ path = copy.deepcopy(last_path)
+ path.pop_front()
+ if len(path.x) <= 1:
+ print("Finish")
+ break
+ last_path = path
s0 = path.s[1]
c_d = path.d[1]
c_d_d = path.d_d[1]
c_d_dd = path.d_dd[1]
- c_speed = path.s_d[1]
- c_accel = path.s_dd[1]
-
+ c_s_d = path.s_d[1]
+ c_s_dd = path.s_dd[1]
if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0:
print("Goal")
break
@@ -301,15 +544,16 @@ def main():
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ "key_release_event",
+ lambda event: [exit(0) if event.key == "escape" else None],
+ )
plt.plot(tx, ty)
- plt.plot(ob[:, 0], ob[:, 1], "xk")
+ plt.plot(OBSTACLES[:, 0], OBSTACLES[:, 1], "xk")
plt.plot(path.x[1:], path.y[1:], "-or")
plt.plot(path.x[1], path.y[1], "vc")
plt.xlim(path.x[1] - area, path.x[1] + area)
plt.ylim(path.y[1] - area, path.y[1] + area)
- plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4])
+ plt.title("v[km/h]:" + str(path.v[1] * 3.6)[0:4])
plt.grid(True)
plt.pause(0.0001)
@@ -320,5 +564,5 @@ def main():
plt.show()
-if __name__ == '__main__':
+if __name__ == "__main__":
main()
diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
index 31dc917566..10ba98cd35 100644
--- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
+++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
@@ -13,7 +13,7 @@
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
from utils.angle import rot_mat_2d
-from Mapping.grid_map_lib.grid_map_lib import GridMap
+from Mapping.grid_map_lib.grid_map_lib import GridMap, FloatGrid
do_animation = True
@@ -41,8 +41,7 @@ def move_target_grid(self, c_x_index, c_y_index, grid_map):
n_y_index = c_y_index
# found safe grid
- if not grid_map.check_occupied_from_xy_index(n_x_index, n_y_index,
- occupied_val=0.5):
+ if not self.check_occupied(n_x_index, n_y_index, grid_map):
return n_x_index, n_y_index
else: # occupied
next_c_x_index, next_c_y_index = self.find_safe_turning_grid(
@@ -51,19 +50,20 @@ def move_target_grid(self, c_x_index, c_y_index, grid_map):
# moving backward
next_c_x_index = -self.moving_direction + c_x_index
next_c_y_index = c_y_index
- if grid_map.check_occupied_from_xy_index(next_c_x_index,
- next_c_y_index):
+ if self.check_occupied(next_c_x_index, next_c_y_index, grid_map, FloatGrid(1.0)):
# moved backward, but the grid is occupied by obstacle
return None, None
else:
# keep moving until end
- while not grid_map.check_occupied_from_xy_index(
- next_c_x_index + self.moving_direction,
- next_c_y_index, occupied_val=0.5):
+ while not self.check_occupied(next_c_x_index + self.moving_direction, next_c_y_index, grid_map):
next_c_x_index += self.moving_direction
self.swap_moving_direction()
return next_c_x_index, next_c_y_index
+ @staticmethod
+ def check_occupied(c_x_index, c_y_index, grid_map, occupied_val=FloatGrid(0.5)):
+ return grid_map.check_occupied_from_xy_index(c_x_index, c_y_index, occupied_val)
+
def find_safe_turning_grid(self, c_x_index, c_y_index, grid_map):
for (d_x_ind, d_y_ind) in self.turing_window:
@@ -72,17 +72,14 @@ def find_safe_turning_grid(self, c_x_index, c_y_index, grid_map):
next_y_ind = d_y_ind + c_y_index
# found safe grid
- if not grid_map.check_occupied_from_xy_index(next_x_ind,
- next_y_ind,
- occupied_val=0.5):
+ if not self.check_occupied(next_x_ind, next_y_ind, grid_map):
return next_x_ind, next_y_ind
return None, None
def is_search_done(self, grid_map):
for ix in self.x_indexes_goal_y:
- if not grid_map.check_occupied_from_xy_index(ix, self.goal_y,
- occupied_val=0.5):
+ if not self.check_occupied(ix, self.goal_y, grid_map):
return False
# all lower grid is occupied
@@ -168,7 +165,7 @@ def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
for iy in x_range:
for ix in y_range:
- if not grid_map.check_occupied_from_xy_index(ix, iy):
+ if not SweepSearcher.check_occupied(ix, iy, grid_map):
y_index = iy
x_indexes.append(ix)
if y_index:
@@ -185,7 +182,7 @@ def setup_grid_map(ox, oy, resolution, sweep_direction, offset_grid=10):
grid_map = GridMap(width, height, resolution, center_x, center_y)
grid_map.print_grid_map_info()
- grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
+ grid_map.set_value_from_polygon(ox, oy, FloatGrid(1.0), inside=False)
grid_map.expand_grid()
x_inds_goal_y = []
@@ -203,7 +200,7 @@ def setup_grid_map(ox, oy, resolution, sweep_direction, offset_grid=10):
def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
# search start grid
c_x_index, c_y_index = sweep_searcher.search_start_grid(grid_map)
- if not grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5):
+ if not grid_map.set_value_from_xy_index(c_x_index, c_y_index, FloatGrid(0.5)):
print("Cannot find start grid")
return [], []
@@ -235,7 +232,7 @@ def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
px.append(x)
py.append(y)
- grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5)
+ grid_map.set_value_from_xy_index(c_x_index, c_y_index, FloatGrid(0.5))
if grid_search_animation:
grid_map.plot_grid_map(ax=ax)
diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py
index 0367ab2775..959db74078 100644
--- a/PathPlanning/HybridAStar/car.py
+++ b/PathPlanning/HybridAStar/car.py
@@ -59,9 +59,9 @@ def rectangle_check(x, y, yaw, ox, oy):
rx, ry = converted_xy[0], converted_xy[1]
if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
- return False # no collision
+ return False # collision
- return True # collision
+ return True # no collision
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
@@ -97,7 +97,7 @@ def pi_2_pi(angle):
def move(x, y, yaw, distance, steer, L=WB):
x += distance * cos(yaw)
y += distance * sin(yaw)
- yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
+ yaw = pi_2_pi(yaw + distance * tan(steer) / L) # distance/2
return x, y, yaw
diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py
index 03db0dc167..0fa04189c6 100644
--- a/PathPlanning/HybridAStar/hybrid_a_star.py
+++ b/PathPlanning/HybridAStar/hybrid_a_star.py
@@ -105,12 +105,13 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1]
arc_l = XY_GRID_RESOLUTION * 1.5
- x_list, y_list, yaw_list = [], [], []
+ x_list, y_list, yaw_list, direction_list = [], [], [], []
for _ in np.arange(0, arc_l, MOTION_RESOLUTION):
x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
x_list.append(x)
y_list.append(y)
yaw_list.append(yaw)
+ direction_list.append(direction == 1)
if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
return None
@@ -134,7 +135,7 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
cost = current.cost + added_cost + arc_l
node = Node(x_ind, y_ind, yaw_ind, d, x_list,
- y_list, yaw_list, [d],
+ y_list, yaw_list, direction_list,
parent_index=calc_index(current, config),
cost=cost, steer=steer)
@@ -281,7 +282,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
while True:
if not openList:
print("Error: Cannot find path, No open set")
- return [], [], []
+ return Path([], [], [], [], 0)
cost, c_id = heapq.heappop(pq)
if c_id in openList:
diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index b6f9d234dd..0483949c99 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -6,7 +6,7 @@
Reference: Informed RRT*: Optimal Sampling-based Path planning Focused via
Direct Sampling of an Admissible Ellipsoidal Heuristic
-https://arxiv.org/pdf/1404.2334.pdf
+https://arxiv.org/pdf/1404.2334
"""
import sys
@@ -293,7 +293,7 @@ def draw_graph(self, x_center=None, c_best=None, c_min=None, e_theta=None,
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.goal.x, self.goal.y, "xr")
- plt.axis([-2, 15, -2, 15])
+ plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.grid(True)
plt.pause(0.01)
diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
index 531bf91ef5..5ef6d2e23f 100644
--- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
+++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
@@ -1,6 +1,7 @@
import math
import numpy as np
from scipy.interpolate import interp1d
+from utils.angle import angle_mod
# motion parameter
L = 1.0 # wheel base
@@ -18,7 +19,7 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def update(state, v, delta, dt, L):
diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py
index 97c0ad8b80..6084fc1a07 100644
--- a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py
+++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py
@@ -18,7 +18,7 @@
# optimization parameter
max_iter = 100
-h = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance
+h: np.ndarray = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance
cost_th = 0.1
show_animation = True
diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
index 8f136b5ee3..603a9d16cf 100644
--- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
+++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
@@ -4,7 +4,7 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
"""
diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
index 294c389023..8bacfd5d19 100644
--- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
+++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
@@ -273,20 +273,20 @@ def main(rng=None):
oy = []
for i in range(60):
- ox.append(i)
+ ox.append(float(i))
oy.append(0.0)
for i in range(60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(61):
ox.append(0.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
index 8ba11a23d2..86f9f662da 100644
--- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
+++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
@@ -4,9 +4,9 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
-- [Local Path planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
+- [Local Path planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
"""
diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py
index 55092e6e00..e6dd9b648b 100644
--- a/PathPlanning/RRT/rrt.py
+++ b/PathPlanning/RRT/rrt.py
@@ -199,7 +199,7 @@ def draw_graph(self, rnd=None):
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis("equal")
- plt.axis([-2, 15, -2, 15])
+ plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.grid(True)
plt.pause(0.01)
diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py
index 2ed6a366d1..ac68efe23f 100644
--- a/PathPlanning/RRT/rrt_with_pathsmoothing.py
+++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py
@@ -51,30 +51,93 @@ def get_target_point(path, targetL):
return [x, y, ti]
-def line_collision_check(first, second, obstacleList):
- # Line Equation
-
- x1 = first[0]
- y1 = first[1]
- x2 = second[0]
- y2 = second[1]
-
- try:
- a = y2 - y1
- b = -(x2 - x1)
- c = y2 * (x2 - x1) - x2 * (y2 - y1)
- except ZeroDivisionError:
- return False
-
- for (ox, oy, size) in obstacleList:
- d = abs(a * ox + b * oy + c) / (math.hypot(a, b))
- if d <= size:
- return False
-
- return True # OK
-
-
-def path_smoothing(path, max_iter, obstacle_list):
+def is_point_collision(x, y, obstacle_list, robot_radius):
+ """
+ Check whether a single point collides with any obstacle.
+
+ This function calculates the Euclidean distance between the given point (x, y)
+ and each obstacle center. If the distance is less than or equal to the sum of
+ the obstacle's radius and the robot's radius, a collision is detected.
+
+ Args:
+ x (float): X-coordinate of the point to check.
+ y (float): Y-coordinate of the point to check.
+ obstacle_list (List[Tuple[float, float, float]]): List of obstacles defined as (ox, oy, radius).
+ robot_radius (float): Radius of the robot, used to inflate the obstacles.
+
+ Returns:
+ bool: True if the point is in collision with any obstacle, False otherwise.
+ """
+ for (ox, oy, obstacle_radius) in obstacle_list:
+ d = math.hypot(ox - x, oy - y)
+ if d <= obstacle_radius + robot_radius:
+ return True # Collided
+ return False
+
+
+def line_collision_check(first, second, obstacle_list, robot_radius=0.0, sample_step=0.2):
+ """
+ Check if the line segment between `first` and `second` collides with any obstacle.
+ Considers the robot_radius by inflating the obstacle size.
+
+ Args:
+ first (List[float]): Start point of the line [x, y]
+ second (List[float]): End point of the line [x, y]
+ obstacle_list (List[Tuple[float, float, float]]): Obstacles as (x, y, radius)
+ robot_radius (float): Radius of robot
+ sample_step (float): Distance between sampling points along the segment
+
+ Returns:
+ bool: True if collision-free, False otherwise
+ """
+ x1, y1 = first[0], first[1]
+ x2, y2 = second[0], second[1]
+
+ dx = x2 - x1
+ dy = y2 - y1
+ length = math.hypot(dx, dy)
+
+ if length == 0:
+ # Degenerate case: point collision check
+ return not is_point_collision(x1, y1, obstacle_list, robot_radius)
+
+ steps = int(length / sample_step) + 1 # Sampling every sample_step along the segment
+
+ for i in range(steps + 1):
+ t = i / steps
+ x = x1 + t * dx
+ y = y1 + t * dy
+
+ if is_point_collision(x, y, obstacle_list, robot_radius):
+ return False # Collision found
+
+ return True # Safe
+
+
+def path_smoothing(path, max_iter, obstacle_list, robot_radius=0.0):
+ """
+ Smooths a given path by iteratively replacing segments with shortcut connections,
+ while ensuring the new segments are collision-free.
+
+ The algorithm randomly picks two points along the original path and attempts to
+ connect them with a straight line. If the line does not collide with any obstacles
+ (considering the robot's radius), the intermediate path points between them are
+ replaced with the direct connection.
+
+ Args:
+ path (List[List[float]]): The original path as a list of [x, y] coordinates.
+ max_iter (int): Number of iterations for smoothing attempts.
+ obstacle_list (List[Tuple[float, float, float]]): List of obstacles represented as
+ (x, y, radius).
+ robot_radius (float, optional): Radius of the robot, used to inflate obstacle size
+ during collision checking. Defaults to 0.0.
+
+ Returns:
+ List[List[float]]: The smoothed path as a list of [x, y] coordinates.
+
+ Example:
+ >>> smoothed = path_smoothing(path, 1000, obstacle_list, robot_radius=0.5)
+ """
le = get_path_length(path)
for i in range(max_iter):
@@ -94,7 +157,7 @@ def path_smoothing(path, max_iter, obstacle_list):
continue
# collision check
- if not line_collision_check(first, second, obstacle_list):
+ if not line_collision_check(first, second, obstacle_list, robot_radius):
continue
# Create New path
@@ -119,14 +182,16 @@ def main():
(3, 10, 2),
(7, 5, 2),
(9, 5, 2)
- ] # [x,y,size]
+ ] # [x,y,radius]
rrt = RRT(start=[0, 0], goal=[6, 10],
- rand_area=[-2, 15], obstacle_list=obstacleList)
+ rand_area=[-2, 15], obstacle_list=obstacleList,
+ robot_radius=0.3)
path = rrt.planning(animation=show_animation)
# Path smoothing
maxIter = 1000
- smoothedPath = path_smoothing(path, maxIter, obstacleList)
+ smoothedPath = path_smoothing(path, maxIter, obstacleList,
+ robot_radius=rrt.robot_radius)
# Draw final path
if show_animation:
diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py
index 428ba58a98..520d686a1d 100644
--- a/PathPlanning/RRT/sobol/sobol.py
+++ b/PathPlanning/RRT/sobol/sobol.py
@@ -13,7 +13,7 @@
PYTHON versions by Corrado Chisari
Original code is available at
- http://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html
+ https://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html
Note: the i4 prefix means that the function takes a numeric argument or
returns a number which is interpreted inside the function as a 4
@@ -370,8 +370,8 @@ def i4_sobol(dim_num, seed):
if (dim_num < 1 or dim_max < dim_num):
print('I4_SOBOL - Fatal error!')
print(' The spatial dimension DIM_NUM should satisfy:')
- print(' 1 <= DIM_NUM <= %d' % dim_max)
- print(' But this input value is DIM_NUM = %d' % dim_num)
+ print(f' 1 <= DIM_NUM <= {dim_max:d}')
+ print(f' But this input value is DIM_NUM = {dim_num:d}')
return None
dim_num_save = dim_num
@@ -443,7 +443,7 @@ def i4_sobol(dim_num, seed):
#
l_var = i4_bit_lo0(seed)
- elif (seed <= seed_save):
+ elif seed <= seed_save:
seed_save = 0
lastq = np.zeros(dim_num)
@@ -471,8 +471,8 @@ def i4_sobol(dim_num, seed):
if maxcol < l_var:
print('I4_SOBOL - Fatal error!')
print(' Too many calls!')
- print(' MAXCOL = %d\n' % maxcol)
- print(' L = %d\n' % l_var)
+ print(f' MAXCOL = {maxcol:d}\n')
+ print(f' L = {l_var:d}\n')
return None
@@ -819,7 +819,7 @@ def r8mat_write(filename, m, n, a):
with open(filename, 'w') as output:
for i in range(0, m):
for j in range(0, n):
- s = ' %g' % (a[i, j])
+ s = f' {a[i, j]:g}'
output.write(s)
output.write('\n')
diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
index c55d2629e5..618d1d99ba 100644
--- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
+++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
@@ -3,12 +3,18 @@
Reeds Shepp path planner sample code
author Atsushi Sakai(@Atsushi_twi)
+co-author Videh Patel(@videh25) : Added the missing RS paths
"""
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
import math
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
show_animation = True
@@ -40,6 +46,9 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
plt.plot(x, y)
+def pi_2_pi(x):
+ return angle_mod(x)
+
def mod2pi(x):
# Be consistent with fmod in cplusplus here.
v = np.mod(x, np.copysign(2.0 * math.pi, x))
@@ -50,22 +59,6 @@ def mod2pi(x):
v -= 2.0 * math.pi
return v
-
-def straight_left_straight(x, y, phi):
- phi = mod2pi(phi)
- # only take phi in (0.01*math.pi, 0.99*math.pi) for the sake of speed.
- # phi in (0, 0.01*math.pi) will make test2() in test_rrt_star_reeds_shepp.py
- # extremely time-consuming, since the value of xd, t will be very large.
- if math.pi * 0.01 < phi < math.pi * 0.99 and y != 0:
- xd = - y / math.tan(phi) + x
- t = xd - math.tan(phi / 2.0)
- u = phi
- v = np.sign(y) * math.hypot(x - xd, y) - math.tan(phi / 2.0)
- return True, t, u, v
-
- return False, 0.0, 0.0, 0.0
-
-
def set_path(paths, lengths, ctypes, step_size):
path = Path()
path.ctypes = ctypes
@@ -87,18 +80,6 @@ def set_path(paths, lengths, ctypes, step_size):
return paths
-def straight_curve_straight(x, y, phi, paths, step_size):
- flag, t, u, v = straight_left_straight(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["S", "L", "S"], step_size)
-
- flag, t, u, v = straight_left_straight(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["S", "R", "S"], step_size)
-
- return paths
-
-
def polar(x, y):
r = math.hypot(x, y)
theta = math.atan2(y, x)
@@ -107,117 +88,200 @@ def polar(x, y):
def left_straight_left(x, y, phi):
u, t = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
- if t >= 0.0:
+ if 0.0 <= t <= math.pi:
v = mod2pi(phi - t)
- if v >= 0.0:
- return True, t, u, v
+ if 0.0 <= v <= math.pi:
+ return True, [t, u, v], ['L', 'S', 'L']
+
+ return False, [], []
- return False, 0.0, 0.0, 0.0
+def left_straight_right(x, y, phi):
+ u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi))
+ u1 = u1 ** 2
+ if u1 >= 4.0:
+ u = math.sqrt(u1 - 4.0)
+ theta = math.atan2(2.0, u)
+ t = mod2pi(t1 + theta)
+ v = mod2pi(t - phi)
+
+ if (t >= 0.0) and (v >= 0.0):
+ return True, [t, u, v], ['L', 'S', 'R']
-def left_right_left(x, y, phi):
- u1, t1 = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
+ return False, [], []
+
+
+def left_x_right_x_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
if u1 <= 4.0:
- u = -2.0 * math.asin(0.25 * u1)
- t = mod2pi(t1 + 0.5 * u + math.pi)
- v = mod2pi(phi - t + u)
+ A = math.acos(0.25 * u1)
+ t = mod2pi(A + theta + math.pi/2)
+ u = mod2pi(math.pi - 2 * A)
+ v = mod2pi(phi - t - u)
+ return True, [t, -u, v], ['L', 'R', 'L']
+
+ return False, [], []
- if t >= 0.0 >= u:
- return True, t, u, v
- return False, 0.0, 0.0, 0.0
+def left_x_right_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
+ if u1 <= 4.0:
+ A = math.acos(0.25 * u1)
+ t = mod2pi(A + theta + math.pi/2)
+ u = mod2pi(math.pi - 2*A)
+ v = mod2pi(-phi + t + u)
+ return True, [t, -u, -v], ['L', 'R', 'L']
-def curve_curve_curve(x, y, phi, paths, step_size):
- flag, t, u, v = left_right_left(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["L", "R", "L"], step_size)
+ return False, [], []
- flag, t, u, v = left_right_left(-x, y, -phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"], step_size)
- flag, t, u, v = left_right_left(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["R", "L", "R"], step_size)
+def left_right_x_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- flag, t, u, v = left_right_left(-x, -y, phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"], step_size)
+ if u1 <= 4.0:
+ u = math.acos(1 - u1**2 * 0.125)
+ A = math.asin(2 * math.sin(u) / u1)
+ t = mod2pi(-A + theta + math.pi/2)
+ v = mod2pi(t - u - phi)
+ return True, [t, u, -v], ['L', 'R', 'L']
+
+ return False, [], []
+
+
+def left_right_x_left_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
+
+ # Solutions refering to (2 < u1 <= 4) are considered sub-optimal in paper
+ # Solutions do not exist for u1 > 4
+ if u1 <= 2:
+ A = math.acos((u1 + 2) * 0.25)
+ t = mod2pi(theta + A + math.pi/2)
+ u = mod2pi(A)
+ v = mod2pi(phi - t + 2*u)
+ if ((t >= 0) and (u >= 0) and (v >= 0)):
+ return True, [t, u, -u, -v], ['L', 'R', 'L', 'R']
+
+ return False, [], []
+
+
+def left_x_right_left_x_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
+ u2 = (20 - u1**2) / 16
+
+ if (0 <= u2 <= 1):
+ u = math.acos(u2)
+ A = math.asin(2 * math.sin(u) / u1)
+ t = mod2pi(theta + A + math.pi/2)
+ v = mod2pi(t - phi)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -u, -u, v], ['L', 'R', 'L', 'R']
- # backwards
- xb = x * math.cos(phi) + y * math.sin(phi)
- yb = x * math.sin(phi) - y * math.cos(phi)
+ return False, [], []
- flag, t, u, v = left_right_left(xb, yb, phi)
- if flag:
- paths = set_path(paths, [v, u, t], ["L", "R", "L"], step_size)
- flag, t, u, v = left_right_left(-xb, yb, -phi)
- if flag:
- paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"], step_size)
+def left_x_right90_straight_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- flag, t, u, v = left_right_left(xb, -yb, -phi)
- if flag:
- paths = set_path(paths, [v, u, t], ["R", "L", "R"], step_size)
+ if u1 >= 2.0:
+ u = math.sqrt(u1**2 - 4) - 2
+ A = math.atan2(2, math.sqrt(u1**2 - 4))
+ t = mod2pi(theta + A + math.pi/2)
+ v = mod2pi(t - phi + math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -math.pi/2, -u, -v], ['L', 'R', 'S', 'L']
- flag, t, u, v = left_right_left(-xb, -yb, phi)
- if flag:
- paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"], step_size)
+ return False, [], []
- return paths
+def left_straight_right90_x_left(x, y, phi):
+ zeta = x - math.sin(phi)
+ eeta = y - 1 + math.cos(phi)
+ u1, theta = polar(zeta, eeta)
-def curve_straight_curve(x, y, phi, paths, step_size):
- flag, t, u, v = left_straight_left(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["L", "S", "L"], step_size)
+ if u1 >= 2.0:
+ u = math.sqrt(u1**2 - 4) - 2
+ A = math.atan2(math.sqrt(u1**2 - 4), 2)
+ t = mod2pi(theta - A + math.pi/2)
+ v = mod2pi(t - phi - math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, u, math.pi/2, -v], ['L', 'S', 'R', 'L']
- flag, t, u, v = left_straight_left(-x, y, -phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"], step_size)
+ return False, [], []
- flag, t, u, v = left_straight_left(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["R", "S", "R"], step_size)
- flag, t, u, v = left_straight_left(-x, -y, phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"], step_size)
+def left_x_right90_straight_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- flag, t, u, v = left_straight_right(x, y, phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["L", "S", "R"], step_size)
+ if u1 >= 2.0:
+ t = mod2pi(theta + math.pi/2)
+ u = u1 - 2
+ v = mod2pi(phi - t - math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -math.pi/2, -u, -v], ['L', 'R', 'S', 'R']
- flag, t, u, v = left_straight_right(-x, y, -phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"], step_size)
+ return False, [], []
- flag, t, u, v = left_straight_right(x, -y, -phi)
- if flag:
- paths = set_path(paths, [t, u, v], ["R", "S", "L"], step_size)
- flag, t, u, v = left_straight_right(-x, -y, phi)
- if flag:
- paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"], step_size)
+def left_straight_left90_x_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
- return paths
+ if u1 >= 2.0:
+ t = mod2pi(theta)
+ u = u1 - 2
+ v = mod2pi(phi - t - math.pi/2)
+ if (t >= 0) and (v >= 0):
+ return True, [t, u, math.pi/2, -v], ['L', 'S', 'L', 'R']
+ return False, [], []
+
+
+def left_x_right90_straight_left90_x_right(x, y, phi):
+ zeta = x + math.sin(phi)
+ eeta = y - 1 - math.cos(phi)
+ u1, theta = polar(zeta, eeta)
-def left_straight_right(x, y, phi):
- u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi))
- u1 = u1 ** 2
if u1 >= 4.0:
- u = math.sqrt(u1 - 4.0)
- theta = math.atan2(2.0, u)
- t = mod2pi(t1 + theta)
+ u = math.sqrt(u1**2 - 4) - 4
+ A = math.atan2(2, math.sqrt(u1**2 - 4))
+ t = mod2pi(theta + A + math.pi/2)
v = mod2pi(t - phi)
+ if (t >= 0) and (v >= 0):
+ return True, [t, -math.pi/2, -u, -math.pi/2, v], ['L', 'R', 'S', 'L', 'R']
+
+ return False, [], []
+
- if t >= 0.0 and v >= 0.0:
- return True, t, u, v
+def timeflip(travel_distances):
+ return [-x for x in travel_distances]
- return False, 0.0, 0.0, 0.0
+
+def reflect(steering_directions):
+ def switch_dir(dirn):
+ if dirn == 'L':
+ return 'R'
+ elif dirn == 'R':
+ return 'L'
+ else:
+ return 'S'
+ return[switch_dir(dirn) for dirn in steering_directions]
def generate_path(q0, q1, max_curvature, step_size):
@@ -228,11 +292,52 @@ def generate_path(q0, q1, max_curvature, step_size):
s = math.sin(q0[2])
x = (c * dx + s * dy) * max_curvature
y = (-s * dx + c * dy) * max_curvature
+ step_size *= max_curvature
paths = []
- paths = straight_curve_straight(x, y, dth, paths, step_size)
- paths = curve_straight_curve(x, y, dth, paths, step_size)
- paths = curve_curve_curve(x, y, dth, paths, step_size)
+ path_functions = [left_straight_left, left_straight_right, # CSC
+ left_x_right_x_left, left_x_right_left, left_right_x_left, # CCC
+ left_right_x_left_right, left_x_right_left_x_right, # CCCC
+ left_x_right90_straight_left, left_x_right90_straight_right, # CCSC
+ left_straight_right90_x_left, left_straight_left90_x_right, # CSCC
+ left_x_right90_straight_left90_x_right] # CCSCC
+
+ for path_func in path_functions:
+ flag, travel_distances, steering_dirns = path_func(x, y, dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+ flag, travel_distances, steering_dirns = path_func(-x, y, -dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ travel_distances = timeflip(travel_distances)
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+ flag, travel_distances, steering_dirns = path_func(x, -y, -dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ steering_dirns = reflect(steering_dirns)
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+ flag, travel_distances, steering_dirns = path_func(-x, -y, dth)
+ if flag:
+ for distance in travel_distances:
+ if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+ print("Step size too large for Reeds-Shepp paths.")
+ return []
+ travel_distances = timeflip(travel_distances)
+ steering_dirns = reflect(steering_dirns)
+ paths = set_path(paths, travel_distances, steering_dirns, step_size)
return paths
@@ -249,7 +354,7 @@ def calc_interpolate_dists_list(lengths, step_size):
def generate_local_course(lengths, modes, max_curvature, step_size):
- interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size)
+ interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size * max_curvature)
origin_x, origin_y, origin_yaw = 0.0, 0.0, 0.0
@@ -296,10 +401,6 @@ def interpolate(dist, length, mode, max_curvature, origin_x, origin_y,
return x, y, yaw, 1 if length > 0.0 else -1
-def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
-
-
def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
q0 = [sx, sy, syaw]
q1 = [gx, gy, gyaw]
@@ -308,7 +409,7 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
for path in paths:
xs, ys, yaws, directions = generate_local_course(path.lengths,
path.ctypes, maxc,
- step_size * maxc)
+ step_size)
# convert global coordinate
path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for
@@ -355,6 +456,9 @@ def main():
curvature,
step_size)
+ if not xs:
+ assert False, "No path"
+
if show_animation: # pragma: no cover
plt.cla()
plt.plot(xs, ys, label="final course " + str(modes))
@@ -369,9 +473,6 @@ def main():
plt.axis("equal")
plt.show()
- if not xs:
- assert False, "No path"
-
if __name__ == '__main__':
main()
diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
index f4b1461b7a..554cd0f3b7 100644
--- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py
+++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py
@@ -8,12 +8,12 @@
https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning
/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py
-Ref:
+Reference:
- State Space Sampling of Feasible Motions for High-Performance Mobile Robot
Navigation in Complex Environments
-http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.187.8210&rep=rep1
-&type=pdf
+https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf
+&doi=e2256b5b24137f89e473f01df288cb3aa72e56a0
"""
import sys
@@ -22,7 +22,9 @@
import numpy as np
import math
import pathlib
+
sys.path.append(str(pathlib.Path(__file__).parent.parent))
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import ModelPredictiveTrajectoryGenerator.trajectory_generator as planner
import ModelPredictiveTrajectoryGenerator.motion_model as motion_model
diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py
new file mode 100644
index 0000000000..745cde45fb
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py
@@ -0,0 +1,49 @@
+from abc import ABC, abstractmethod
+from dataclasses import dataclass
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath
+import random
+import numpy.random as numpy_random
+
+# Seed randomness for reproducibility
+RANDOM_SEED = 50
+random.seed(RANDOM_SEED)
+numpy_random.seed(RANDOM_SEED)
+
+class SingleAgentPlanner(ABC):
+ """
+ Base class for single agent planners
+ """
+
+ @staticmethod
+ @abstractmethod
+ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
+ pass
+
+@dataclass
+class StartAndGoal:
+ # Index of this agent
+ index: int
+ # Start position of the robot
+ start: Position
+ # Goal position of the robot
+ goal: Position
+
+ def distance_start_to_goal(self) -> float:
+ return pow(self.goal.x - self.start.x, 2) + pow(self.goal.y - self.start.y, 2)
+
+class MultiAgentPlanner(ABC):
+ """
+ Base class for multi-agent planners
+ """
+
+ @staticmethod
+ @abstractmethod
+ def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]:
+ """
+ Plan for all agents. Returned paths are in order corresponding to the returned list of `StartAndGoal` objects
+ """
+ pass
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
new file mode 100644
index 0000000000..ccc2989001
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py
@@ -0,0 +1,370 @@
+"""
+This file implements a grid with a 3d reservation matrix with dimensions for x, y, and time. There
+is also infrastructure to generate dynamic obstacles that move around the grid. The obstacles' paths
+are stored in the reservation matrix on creation.
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+from enum import Enum
+from dataclasses import dataclass
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position
+
+@dataclass
+class Interval:
+ start_time: int
+ end_time: int
+
+class ObstacleArrangement(Enum):
+ # Random obstacle positions and movements
+ RANDOM = 0
+ # Obstacles start in a line in y at center of grid and move side-to-side in x
+ ARRANGEMENT1 = 1
+ # Static obstacle arrangement
+ NARROW_CORRIDOR = 2
+
+"""
+Generates a 2d numpy array with lists for elements.
+"""
+def empty_2d_array_of_lists(x: int, y: int) -> np.ndarray:
+ arr = np.empty((x, y), dtype=object)
+ # assign each element individually - np.full creates references to the same list
+ arr[:] = [[[] for _ in range(y)] for _ in range(x)]
+ return arr
+
+class Grid:
+ # Set in constructor
+ grid_size: np.ndarray
+ reservation_matrix: np.ndarray
+ obstacle_paths: list[list[Position]] = []
+ # Obstacles will never occupy these points. Useful to avoid impossible scenarios
+ obstacle_avoid_points: list[Position] = []
+
+ # Number of time steps in the simulation
+ time_limit: int
+
+ # Logging control
+ verbose = False
+
+ def __init__(
+ self,
+ grid_size: np.ndarray,
+ num_obstacles: int = 40,
+ obstacle_avoid_points: list[Position] = [],
+ obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM,
+ time_limit: int = 100,
+ ):
+ self.obstacle_avoid_points = obstacle_avoid_points
+ self.time_limit = time_limit
+ self.grid_size = grid_size
+ self.reservation_matrix = np.zeros((grid_size[0], grid_size[1], self.time_limit))
+
+ if num_obstacles > self.grid_size[0] * self.grid_size[1]:
+ raise Exception("Number of obstacles is greater than grid size!")
+
+ if obstacle_arrangement == ObstacleArrangement.RANDOM:
+ self.obstacle_paths = self.generate_dynamic_obstacles(num_obstacles)
+ elif obstacle_arrangement == ObstacleArrangement.ARRANGEMENT1:
+ self.obstacle_paths = self.obstacle_arrangement_1(num_obstacles)
+ elif obstacle_arrangement == ObstacleArrangement.NARROW_CORRIDOR:
+ self.obstacle_paths = self.generate_narrow_corridor_obstacles(num_obstacles)
+
+ for i, path in enumerate(self.obstacle_paths):
+ obs_idx = i + 1 # avoid using 0 - that indicates free space in the grid
+ for t, position in enumerate(path):
+ # Reserve old & new position at this time step
+ if t > 0:
+ self.reservation_matrix[path[t - 1].x, path[t - 1].y, t] = obs_idx
+ self.reservation_matrix[position.x, position.y, t] = obs_idx
+
+ """
+ Generate dynamic obstacles that move around the grid. Initial positions and movements are random
+ """
+ def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
+ obstacle_paths = []
+ for _ in range(0, obs_count):
+ # Sample until a free starting space is found
+ initial_position = self.sample_random_position()
+ while not self.valid_obstacle_position(initial_position, 0):
+ initial_position = self.sample_random_position()
+
+ positions = [initial_position]
+ if self.verbose:
+ print("Obstacle initial position: ", initial_position)
+
+ # Encourage obstacles to mostly stay in place - too much movement leads to chaotic planning scenarios
+ # that are not fun to watch
+ weights = [0.05, 0.05, 0.05, 0.05, 0.8]
+ diffs = [
+ Position(0, 1),
+ Position(0, -1),
+ Position(1, 0),
+ Position(-1, 0),
+ Position(0, 0),
+ ]
+
+ for t in range(1, self.time_limit - 1):
+ sampled_indices = np.random.choice(
+ len(diffs), size=5, replace=False, p=weights
+ )
+ rand_diffs = [diffs[i] for i in sampled_indices]
+
+ valid_position = None
+ for diff in rand_diffs:
+ new_position = positions[-1] + diff
+
+ if not self.valid_obstacle_position(new_position, t):
+ continue
+
+ valid_position = new_position
+ break
+
+ # Impossible situation for obstacle - stay in place
+ # -> this can happen if the oaths of other obstacles this one
+ if valid_position is None:
+ valid_position = positions[-1]
+
+ positions.append(valid_position)
+
+ obstacle_paths.append(positions)
+
+ return obstacle_paths
+
+ """
+ Generate a line of obstacles in y at the center of the grid that move side-to-side in x
+ Bottom half start moving right, top half start moving left. If `obs_count` is less than the length of
+ the grid, only the first `obs_count` obstacles will be generated.
+ """
+ def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]:
+ obstacle_paths = []
+ half_grid_x = self.grid_size[0] // 2
+ half_grid_y = self.grid_size[1] // 2
+
+ for y_idx in range(0, min(obs_count, self.grid_size[1])):
+ moving_right = y_idx < half_grid_y
+ position = Position(half_grid_x, y_idx)
+ path = [position]
+
+ for t in range(1, self.time_limit - 1):
+ # sit in place every other time step
+ if t % 2 == 0:
+ path.append(position)
+ continue
+
+ # first check if we should switch direction (at edge of grid)
+ if (moving_right and position.x == self.grid_size[0] - 1) or (
+ not moving_right and position.x == 0
+ ):
+ moving_right = not moving_right
+ # step in direction
+ position = Position(
+ position.x + (1 if moving_right else -1), position.y
+ )
+ path.append(position)
+
+ obstacle_paths.append(path)
+
+ return obstacle_paths
+
+ def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Position]]:
+ obstacle_paths = []
+
+ for y in range(0, self.grid_size[1]):
+ if y > obs_count:
+ break
+
+ if y == self.grid_size[1] // 2:
+ # Skip the middle row
+ continue
+
+ obstacle_path = []
+ x = self.grid_size[0] // 2 # middle of the grid
+ for t in range(0, self.time_limit - 1):
+ obstacle_path.append(Position(x, y))
+
+ obstacle_paths.append(obstacle_path)
+
+ return obstacle_paths
+
+ """
+ Check if the given position is valid at time t
+
+ input:
+ position (Position): (x, y) position
+ t (int): time step
+
+ output:
+ bool: True if position/time combination is valid, False otherwise
+ """
+ def valid_position(self, position: Position, t: int) -> bool:
+ # Check if position is in grid
+ if not self.inside_grid_bounds(position):
+ return False
+
+ # Check if position is not occupied at time t
+ return self.reservation_matrix[position.x, position.y, t] == 0
+
+ """
+ Returns True if the given position is valid at time t and is not in the set of obstacle_avoid_points
+ """
+ def valid_obstacle_position(self, position: Position, t: int) -> bool:
+ return (
+ self.valid_position(position, t)
+ and position not in self.obstacle_avoid_points
+ )
+
+ """
+ Returns True if the given position is within the grid's boundaries
+ """
+ def inside_grid_bounds(self, position: Position) -> bool:
+ return (
+ position.x >= 0
+ and position.x < self.grid_size[0]
+ and position.y >= 0
+ and position.y < self.grid_size[1]
+ )
+
+ """
+ Sample a random position that is within the grid's boundaries
+
+ output:
+ Position: (x, y) position
+ """
+ def sample_random_position(self) -> Position:
+ return Position(
+ np.random.randint(0, self.grid_size[0]),
+ np.random.randint(0, self.grid_size[1]),
+ )
+
+ """
+ Returns a tuple of (x_positions, y_positions) of the obstacles at time t
+ """
+ def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]:
+ x_positions = []
+ y_positions = []
+ for obs_path in self.obstacle_paths:
+ x_positions.append(obs_path[t].x)
+ y_positions.append(obs_path[t].y)
+ return (x_positions, y_positions)
+
+ """
+ Returns safe intervals for each cell.
+ """
+ def get_safe_intervals(self) -> np.ndarray:
+ intervals = empty_2d_array_of_lists(self.grid_size[0], self.grid_size[1])
+ for x in range(intervals.shape[0]):
+ for y in range(intervals.shape[1]):
+ intervals[x, y] = self.get_safe_intervals_at_cell(Position(x, y))
+
+ return intervals
+
+ """
+ Generate the safe intervals for a given cell. The intervals will be in order of start time.
+ ex: Interval (2, 3) will be before Interval (4, 5)
+ """
+ def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]:
+ vals = self.reservation_matrix[cell.x, cell.y, :]
+ # Find where the array is zero
+ zero_mask = (vals == 0)
+
+ # Identify transitions between zero and nonzero elements
+ diff = np.diff(zero_mask.astype(int))
+
+ # Start indices: where zeros begin (1 after a nonzero)
+ start_indices = np.where(diff == 1)[0] + 1
+
+ # End indices: where zeros stop (just before a nonzero)
+ end_indices = np.where(diff == -1)[0]
+
+ # Handle edge cases if the array starts or ends with zeros
+ if zero_mask[0]: # If the first element is zero, add index 0 to start_indices
+ start_indices = np.insert(start_indices, 0, 0)
+ if zero_mask[-1]: # If the last element is zero, add the last index to end_indices
+ end_indices = np.append(end_indices, len(vals) - 1)
+
+ # Create pairs of (first zero, last zero)
+ intervals = [Interval(int(start), int(end)) for start, end in zip(start_indices, end_indices)]
+
+ # Remove intervals where a cell is only free for one time step. Those intervals not provide enough time to
+ # move into and out of the cell each take 1 time step, and the cell is considered occupied during
+ # both the time step when it is entering the cell, and the time step when it is leaving the cell.
+ intervals = [interval for interval in intervals if interval.start_time != interval.end_time]
+ return intervals
+
+ """
+ Reserve an agent's path in the grid. Raises an exception if the agent's index is 0, or if a position is
+ already reserved by a different agent.
+ """
+ def reserve_path(self, node_path: NodePath, agent_index: int):
+ if agent_index == 0:
+ raise Exception("Agent index cannot be 0")
+
+ for i, node in enumerate(node_path.path):
+ reservation_finish_time = node.time + 1
+ if i < len(node_path.path) - 1:
+ reservation_finish_time = node_path.path[i + 1].time
+
+ self.reserve_position(node.position, agent_index, Interval(node.time, reservation_finish_time))
+
+ """
+ Reserve a position for the provided agent during the provided time interval.
+ Raises an exception if the agent's index is 0, or if the position is already reserved by a different agent during the interval.
+ """
+ def reserve_position(self, position: Position, agent_index: int, interval: Interval):
+ if agent_index == 0:
+ raise Exception("Agent index cannot be 0")
+
+ for t in range(interval.start_time, interval.end_time + 1):
+ current_reserver = self.reservation_matrix[position.x, position.y, t]
+ if current_reserver not in [0, agent_index]:
+ raise Exception(
+ f"Agent {agent_index} tried to reserve a position already reserved by another agent: {position} at time {t}, reserved by {current_reserver}"
+ )
+ self.reservation_matrix[position.x, position.y, t] = agent_index
+
+ """
+ Clears the initial reservation for an agent by clearing reservations at its start position with its index for
+ from time 0 to the time limit.
+ """
+ def clear_initial_reservation(self, position: Position, agent_index: int):
+ for t in range(self.time_limit):
+ if self.reservation_matrix[position.x, position.y, t] == agent_index:
+ self.reservation_matrix[position.x, position.y, t] = 0
+
+show_animation = True
+
+def main():
+ grid = Grid(
+ np.array([11, 11]),
+ num_obstacles=10,
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ if not show_animation:
+ return
+
+ fig = plt.figure(figsize=(8, 7))
+ ax = fig.add_subplot(
+ autoscale_on=False,
+ xlim=(0, grid.grid_size[0] - 1),
+ ylim=(0, grid.grid_size[1] - 1),
+ )
+ ax.set_aspect("equal")
+ ax.grid()
+ ax.set_xticks(np.arange(0, 11, 1))
+ ax.set_yticks(np.arange(0, 11, 1))
+ (obs_points,) = ax.plot([], [], "ro", ms=15)
+
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect(
+ "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+ )
+
+ for i in range(0, grid.time_limit - 1):
+ obs_positions = grid.get_obstacle_positions_at_time(i)
+ obs_points.set_data(obs_positions[0], obs_positions[1])
+ plt.pause(0.2)
+ plt.show()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/PathPlanning/TimeBasedPathPlanning/Node.py b/PathPlanning/TimeBasedPathPlanning/Node.py
new file mode 100644
index 0000000000..728eebb676
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/Node.py
@@ -0,0 +1,99 @@
+from dataclasses import dataclass
+from functools import total_ordering
+import numpy as np
+from typing import Sequence
+
+@dataclass(order=True)
+class Position:
+ x: int
+ y: int
+
+ def as_ndarray(self) -> np.ndarray:
+ return np.array([self.x, self.y])
+
+ def __add__(self, other):
+ if isinstance(other, Position):
+ return Position(self.x + other.x, self.y + other.y)
+ raise NotImplementedError(
+ f"Addition not supported for Position and {type(other)}"
+ )
+
+ def __sub__(self, other):
+ if isinstance(other, Position):
+ return Position(self.x - other.x, self.y - other.y)
+ raise NotImplementedError(
+ f"Subtraction not supported for Position and {type(other)}"
+ )
+
+ def __hash__(self):
+ return hash((self.x, self.y))
+
+@dataclass()
+# Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because
+# this class needs to override the __lt__ and __eq__ methods to ignore parent_index. Parent
+# index is just used to track the path found by the algorithm, and has no effect on the quality
+# of a node.
+@total_ordering
+class Node:
+ position: Position
+ time: int
+ heuristic: int
+ parent_index: int
+
+ """
+ This is what is used to drive node expansion. The node with the lowest value is expanded next.
+ This comparison prioritizes the node with the lowest cost-to-come (self.time) + cost-to-go (self.heuristic)
+ """
+ def __lt__(self, other: object):
+ if not isinstance(other, Node):
+ return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}")
+ return (self.time + self.heuristic) < (other.time + other.heuristic)
+
+ """
+ Note: cost and heuristic are not included in eq or hash, since they will always be the same
+ for a given (position, time) pair. Including either cost or heuristic would be redundant.
+ """
+ def __eq__(self, other: object):
+ if not isinstance(other, Node):
+ return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}")
+ return self.position == other.position and self.time == other.time
+
+ def __hash__(self):
+ return hash((self.position, self.time))
+
+class NodePath:
+ path: Sequence[Node]
+ positions_at_time: dict[int, Position]
+ # Number of nodes expanded while finding this path
+ expanded_node_count: int
+
+ def __init__(self, path: Sequence[Node], expanded_node_count: int):
+ self.path = path
+ self.expanded_node_count = expanded_node_count
+
+ self.positions_at_time = {}
+ for i, node in enumerate(path):
+ reservation_finish_time = node.time + 1
+ if i < len(path) - 1:
+ reservation_finish_time = path[i + 1].time
+
+ for t in range(node.time, reservation_finish_time):
+ self.positions_at_time[t] = node.position
+
+ """
+ Get the position of the path at a given time
+ """
+ def get_position(self, time: int) -> Position | None:
+ return self.positions_at_time.get(time)
+
+ """
+ Time stamp of the last node in the path
+ """
+ def goal_reached_time(self) -> int:
+ return self.path[-1].time
+
+ def __repr__(self):
+ repr_string = ""
+ for i, node in enumerate(self.path):
+ repr_string += f"{i}: {node}\n"
+ return repr_string
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py
new file mode 100644
index 0000000000..7cd1f615d8
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py
@@ -0,0 +1,135 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.backend_bases import KeyEvent
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath
+
+'''
+Plot a single agent path.
+'''
+def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath):
+ fig = plt.figure(figsize=(10, 7))
+ ax = fig.add_subplot(
+ autoscale_on=False,
+ xlim=(0, grid.grid_size[0] - 1),
+ ylim=(0, grid.grid_size[1] - 1),
+ )
+ ax.set_aspect("equal")
+ ax.grid()
+ ax.set_xticks(np.arange(0, grid.grid_size[0], 1))
+ ax.set_yticks(np.arange(0, grid.grid_size[1], 1))
+
+ (start_and_goal,) = ax.plot([], [], "mD", ms=15, label="Start and Goal")
+ start_and_goal.set_data([start.x, goal.x], [start.y, goal.y])
+ (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles")
+ (path_points,) = ax.plot([], [], "bo", ms=10, label="Path Found")
+ ax.legend(bbox_to_anchor=(1.05, 1))
+
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect(
+ "key_release_event",
+ lambda event: [exit(0) if event.key == "escape" else None]
+ if isinstance(event, KeyEvent) else None
+ )
+
+ for i in range(0, path.goal_reached_time()):
+ obs_positions = grid.get_obstacle_positions_at_time(i)
+ obs_points.set_data(obs_positions[0], obs_positions[1])
+ path_position = path.get_position(i)
+ if not path_position:
+ raise Exception(f"Path position not found for time {i}.")
+
+ path_points.set_data([path_position.x], [path_position.y])
+ plt.pause(0.2)
+ plt.show()
+
+'''
+Plot a series of agent paths.
+'''
+def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath]):
+ fig = plt.figure(figsize=(10, 7))
+
+ ax = fig.add_subplot(
+ autoscale_on=False,
+ xlim=(0, grid.grid_size[0] - 1),
+ ylim=(0, grid.grid_size[1] - 1),
+ )
+ ax.set_aspect("equal")
+ ax.grid()
+ ax.set_xticks(np.arange(0, grid.grid_size[0], 1))
+ ax.set_yticks(np.arange(0, grid.grid_size[1], 1))
+
+ # Plot start and goal positions for each agent
+ colors = [] # generated randomly in loop
+ markers = ['D', 's', '^', 'o', 'p'] # Different markers for visual distinction
+
+ # Create plots for start and goal positions
+ start_and_goal_plots = []
+ for i, path in enumerate(paths):
+ marker_idx = i % len(markers)
+ agent_id = start_and_goals[i].index
+ start = start_and_goals[i].start
+ goal = start_and_goals[i].goal
+
+ color = np.random.rand(3,)
+ colors.append(color)
+ sg_plot, = ax.plot([], [], markers[marker_idx], c=color, ms=15,
+ label=f"Agent {agent_id} Start/Goal")
+ sg_plot.set_data([start.x, goal.x], [start.y, goal.y])
+ start_and_goal_plots.append(sg_plot)
+
+ # Plot for obstacles
+ (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles")
+
+ # Create plots for each agent's path
+ path_plots = []
+ for i, path in enumerate(paths):
+ agent_id = start_and_goals[i].index
+ path_plot, = ax.plot([], [], "o", c=colors[i], ms=10,
+ label=f"Agent {agent_id} Path")
+ path_plots.append(path_plot)
+
+ ax.legend(bbox_to_anchor=(1.05, 1))
+
+ # For stopping simulation with the esc key
+ plt.gcf().canvas.mpl_connect(
+ "key_release_event",
+ lambda event: [exit(0) if event.key == "escape" else None]
+ if isinstance(event, KeyEvent) else None
+ )
+
+ # Find the maximum time across all paths
+ max_time = max(path.goal_reached_time() for path in paths)
+
+ # Animation loop
+ for i in range(0, max_time + 1):
+ # Update obstacle positions
+ obs_positions = grid.get_obstacle_positions_at_time(i)
+ obs_points.set_data(obs_positions[0], obs_positions[1])
+
+ # Update each agent's position
+ for (j, path) in enumerate(paths):
+ path_positions = []
+ if i <= path.goal_reached_time():
+ res = path.get_position(i)
+ if not res:
+ print(path)
+ print(i)
+ path_position = path.get_position(i)
+ if not path_position:
+ raise Exception(f"Path position not found for time {i}.")
+
+ # Verify position is valid
+ assert not path_position in obs_positions
+ assert not path_position in path_positions
+ path_positions.append(path_position)
+
+ path_plots[j].set_data([path_position.x], [path_position.y])
+
+ plt.pause(0.2)
+
+ plt.show()
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py
new file mode 100644
index 0000000000..2573965cf6
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py
@@ -0,0 +1,95 @@
+"""
+Priority Based Planner for multi agent path planning.
+The planner generates an order to plan in, and generates plans for the robots in that order. Each planned
+path is reserved in the grid, and all future plans must avoid that path.
+
+Algorithm outlined in section III of this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf
+"""
+
+import numpy as np
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Interval,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import MultiAgentPlanner, StartAndGoal
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths
+import time
+
+class PriorityBasedPlanner(MultiAgentPlanner):
+
+ @staticmethod
+ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]:
+ """
+ Generate a path from the start to the goal for each agent in the `start_and_goals` list.
+ Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans
+ corresponds to the order of the `start_and_goals` list.
+ """
+ print(f"Using single-agent planner: {single_agent_planner_class}")
+
+ # Reserve initial positions
+ for start_and_goal in start_and_goals:
+ grid.reserve_position(start_and_goal.start, start_and_goal.index, Interval(0, 10))
+
+ # Plan in descending order of distance from start to goal
+ start_and_goals = sorted(start_and_goals,
+ key=lambda item: item.distance_start_to_goal(),
+ reverse=True)
+
+ paths = []
+ for start_and_goal in start_and_goals:
+ if verbose:
+ print(f"\nPlanning for agent: {start_and_goal}" )
+
+ grid.clear_initial_reservation(start_and_goal.start, start_and_goal.index)
+ path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, verbose)
+
+ if path is None:
+ print(f"Failed to find path for {start_and_goal}")
+ return []
+
+ agent_index = start_and_goal.index
+ grid.reserve_path(path, agent_index)
+ paths.append(path)
+
+ return (start_and_goals, paths)
+
+verbose = False
+show_animation = True
+def main():
+ grid_side_length = 21
+
+ start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+ obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=250,
+ obstacle_avoid_points=obstacle_avoid_points,
+ # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ # obstacle_arrangement=ObstacleArrangement.RANDOM,
+ )
+
+ start_time = time.time()
+ start_and_goals, paths = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose)
+
+ runtime = time.time() - start_time
+ print(f"\nPlanning took: {runtime:.5f} seconds")
+
+ if verbose:
+ print(f"Paths:")
+ for path in paths:
+ print(f"{path}\n")
+
+ if not show_animation:
+ return
+
+ PlotNodePaths(grid, start_and_goals, paths)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
new file mode 100644
index 0000000000..446847ac6d
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py
@@ -0,0 +1,212 @@
+"""
+Safe interval path planner
+ This script implements a safe-interval path planner for a 2d grid with dynamic obstacles. It is faster than
+ SpaceTime A* because it reduces the number of redundant node expansions by pre-computing regions of adjacent
+ time steps that are safe ("safe intervals") at each position. This allows the algorithm to skip expanding nodes
+ that are in intervals that have already been visited earlier.
+
+ Reference: https://www.cs.cmu.edu/~maxim/files/sipp_icra11.pdf
+"""
+
+import numpy as np
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ Interval,
+ ObstacleArrangement,
+ Position,
+ empty_2d_array_of_lists,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath
+
+import heapq
+from dataclasses import dataclass
+from functools import total_ordering
+import time
+
+@dataclass()
+# Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because
+# this class needs to override the __lt__ and __eq__ methods to ignore parent_index. The Parent
+# index and interval member variables are just used to track the path found by the algorithm,
+# and has no effect on the quality of a node.
+@total_ordering
+class SIPPNode(Node):
+ interval: Interval
+
+@dataclass
+class EntryTimeAndInterval:
+ entry_time: int
+ interval: Interval
+
+class SafeIntervalPathPlanner(SingleAgentPlanner):
+
+ """
+ Generate a plan given the loaded problem statement. Raises an exception if it fails to find a path.
+ Arguments:
+ verbose (bool): set to True to print debug information
+ """
+ @staticmethod
+ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
+
+ safe_intervals = grid.get_safe_intervals()
+
+ open_set: list[SIPPNode] = []
+ first_node_interval = safe_intervals[start.x, start.y][0]
+ heapq.heappush(
+ open_set, SIPPNode(start, 0, SafeIntervalPathPlanner.calculate_heuristic(start, goal), -1, first_node_interval)
+ )
+
+ expanded_list: list[SIPPNode] = []
+ visited_intervals = empty_2d_array_of_lists(grid.grid_size[0], grid.grid_size[1])
+ while open_set:
+ expanded_node: SIPPNode = heapq.heappop(open_set)
+ if verbose:
+ print("Expanded node:", expanded_node)
+
+ if expanded_node.time + 1 >= grid.time_limit:
+ if verbose:
+ print(f"\tSkipping node that is past time limit: {expanded_node}")
+ continue
+
+ if expanded_node.position == goal:
+ if verbose:
+ print(f"Found path to goal after {len(expanded_list)} expansions")
+
+ path = []
+ path_walker: SIPPNode = expanded_node
+ while True:
+ path.append(path_walker)
+ if path_walker.parent_index == -1:
+ break
+ path_walker = expanded_list[path_walker.parent_index]
+
+ # reverse path so it goes start -> goal
+ path.reverse()
+ return NodePath(path, len(expanded_list))
+
+ expanded_idx = len(expanded_list)
+ expanded_list.append(expanded_node)
+ entry_time_and_node = EntryTimeAndInterval(expanded_node.time, expanded_node.interval)
+ add_entry_to_visited_intervals_array(entry_time_and_node, visited_intervals, expanded_node)
+
+ for child in SafeIntervalPathPlanner.generate_successors(grid, goal, expanded_node, expanded_idx, safe_intervals, visited_intervals):
+ heapq.heappush(open_set, child)
+
+ raise Exception("No path found")
+
+ """
+ Generate list of possible successors of the provided `parent_node` that are worth expanding
+ """
+ @staticmethod
+ def generate_successors(
+ grid: Grid, goal: Position, parent_node: SIPPNode, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray
+ ) -> list[SIPPNode]:
+ new_nodes = []
+ diffs = [
+ Position(0, 0),
+ Position(1, 0),
+ Position(-1, 0),
+ Position(0, 1),
+ Position(0, -1),
+ ]
+ for diff in diffs:
+ new_pos = parent_node.position + diff
+ if not grid.inside_grid_bounds(new_pos):
+ continue
+
+ current_interval = parent_node.interval
+
+ new_cell_intervals: list[Interval] = intervals[new_pos.x, new_pos.y]
+ for interval in new_cell_intervals:
+ # if interval starts after current ends, break
+ # assumption: intervals are sorted by start time, so all future intervals will hit this condition as well
+ if interval.start_time > current_interval.end_time:
+ break
+
+ # if interval ends before current starts, skip
+ if interval.end_time < current_interval.start_time:
+ continue
+
+ # if we have already expanded a node in this interval with a <= starting time, skip
+ better_node_expanded = False
+ for visited in visited_intervals[new_pos.x, new_pos.y]:
+ if interval == visited.interval and visited.entry_time <= parent_node.time + 1:
+ better_node_expanded = True
+ break
+ if better_node_expanded:
+ continue
+
+ # We know there is a node worth expanding. Generate successor at the earliest possible time the
+ # new interval can be entered
+ for possible_t in range(max(parent_node.time + 1, interval.start_time), min(current_interval.end_time, interval.end_time)):
+ if grid.valid_position(new_pos, possible_t):
+ new_nodes.append(SIPPNode(
+ new_pos,
+ # entry is max of interval start and parent node time + 1 (get there as soon as possible)
+ max(interval.start_time, parent_node.time + 1),
+ SafeIntervalPathPlanner.calculate_heuristic(new_pos, goal),
+ parent_node_idx,
+ interval,
+ ))
+ # break because all t's after this will make nodes with a higher cost, the same heuristic, and are in the same interval
+ break
+
+ return new_nodes
+
+ """
+ Calculate the heuristic for a given position - Manhattan distance to the goal
+ """
+ @staticmethod
+ def calculate_heuristic(position: Position, goal: Position) -> int:
+ diff = goal - position
+ return abs(diff.x) + abs(diff.y)
+
+
+"""
+Adds a new entry to the visited intervals array. If the entry is already present, the entry time is updated if the new
+entry time is better. Otherwise, the entry is added to `visited_intervals` at the position of `expanded_node`.
+"""
+def add_entry_to_visited_intervals_array(entry_time_and_interval: EntryTimeAndInterval, visited_intervals: np.ndarray, expanded_node: SIPPNode):
+ # if entry is present, update entry time if better
+ for existing_entry_and_interval in visited_intervals[expanded_node.position.x, expanded_node.position.y]:
+ if existing_entry_and_interval.interval == entry_time_and_interval.interval:
+ existing_entry_and_interval.entry_time = min(existing_entry_and_interval.entry_time, entry_time_and_interval.entry_time)
+
+ # Otherwise, append
+ visited_intervals[expanded_node.position.x, expanded_node.position.y].append(entry_time_and_interval)
+
+
+show_animation = True
+verbose = False
+
+def main():
+ start = Position(1, 18)
+ goal = Position(19, 19)
+ grid_side_length = 21
+
+ start_time = time.time()
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=250,
+ obstacle_avoid_points=[start, goal],
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ # obstacle_arrangement=ObstacleArrangement.RANDOM,
+ )
+
+ path = SafeIntervalPathPlanner.plan(grid, start, goal, verbose)
+ runtime = time.time() - start_time
+ print(f"Planning took: {runtime:.5f} seconds")
+
+ if verbose:
+ print(f"Path: {path}")
+
+ if not show_animation:
+ return
+
+ PlotNodePath(grid, start, goal, path)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
new file mode 100644
index 0000000000..b85569f5dc
--- /dev/null
+++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py
@@ -0,0 +1,139 @@
+"""
+Space-time A* Algorithm
+ This script demonstrates the Space-time A* algorithm for path planning in a grid world with moving obstacles.
+ This algorithm is different from normal 2D A* in one key way - the cost (often notated as g(n)) is
+ the number of time steps it took to get to a given node, instead of the number of cells it has
+ traversed. This ensures the path is time-optimal, while respecting any dynamic obstacles in the environment.
+
+ Reference: https://www.davidsilver.uk/wp-content/uploads/2020/03/coop-path-AIWisdom.pdf
+"""
+
+import numpy as np
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath
+import heapq
+from collections.abc import Generator
+import time
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath
+
+class SpaceTimeAStar(SingleAgentPlanner):
+
+ @staticmethod
+ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
+ open_set: list[Node] = []
+ heapq.heappush(
+ open_set, Node(start, 0, SpaceTimeAStar.calculate_heuristic(start, goal), -1)
+ )
+
+ expanded_list: list[Node] = []
+ expanded_set: set[Node] = set()
+ while open_set:
+ expanded_node: Node = heapq.heappop(open_set)
+ if verbose:
+ print("Expanded node:", expanded_node)
+
+ if expanded_node.time + 1 >= grid.time_limit:
+ if verbose:
+ print(f"\tSkipping node that is past time limit: {expanded_node}")
+ continue
+
+ if expanded_node.position == goal:
+ if verbose:
+ print(f"Found path to goal after {len(expanded_list)} expansions")
+
+ path = []
+ path_walker: Node = expanded_node
+ while True:
+ path.append(path_walker)
+ if path_walker.parent_index == -1:
+ break
+ path_walker = expanded_list[path_walker.parent_index]
+
+ # reverse path so it goes start -> goal
+ path.reverse()
+ return NodePath(path, len(expanded_set))
+
+ expanded_idx = len(expanded_list)
+ expanded_list.append(expanded_node)
+ expanded_set.add(expanded_node)
+
+ for child in SpaceTimeAStar.generate_successors(grid, goal, expanded_node, expanded_idx, verbose, expanded_set):
+ heapq.heappush(open_set, child)
+
+ raise Exception("No path found")
+
+ """
+ Generate possible successors of the provided `parent_node`
+ """
+ @staticmethod
+ def generate_successors(
+ grid: Grid, goal: Position, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node]
+ ) -> Generator[Node, None, None]:
+ diffs = [
+ Position(0, 0),
+ Position(1, 0),
+ Position(-1, 0),
+ Position(0, 1),
+ Position(0, -1),
+ ]
+ for diff in diffs:
+ new_pos = parent_node.position + diff
+ new_node = Node(
+ new_pos,
+ parent_node.time + 1,
+ SpaceTimeAStar.calculate_heuristic(new_pos, goal),
+ parent_node_idx,
+ )
+
+ if new_node in expanded_set:
+ continue
+
+ # Check if the new node is valid for the next 2 time steps - one step to enter, and another to leave
+ if all([grid.valid_position(new_pos, parent_node.time + dt) for dt in [1, 2]]):
+ if verbose:
+ print("\tNew successor node: ", new_node)
+ yield new_node
+
+ @staticmethod
+ def calculate_heuristic(position: Position, goal: Position) -> int:
+ diff = goal - position
+ return abs(diff.x) + abs(diff.y)
+
+
+show_animation = True
+verbose = False
+
+def main():
+ start = Position(1, 5)
+ goal = Position(19, 19)
+ grid_side_length = 21
+
+ start_time = time.time()
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=40,
+ obstacle_avoid_points=[start, goal],
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ path = SpaceTimeAStar.plan(grid, start, goal, verbose)
+
+ runtime = time.time() - start_time
+ print(f"Planning took: {runtime:.5f} seconds")
+
+ if verbose:
+ print(f"Path: {path}")
+
+ if not show_animation:
+ return
+
+ PlotNodePath(grid, start, goal, path)
+
+if __name__ == "__main__":
+ main()
diff --git a/Control/__init__.py b/PathPlanning/TimeBasedPathPlanning/__init__.py
similarity index 100%
rename from Control/__init__.py
rename to PathPlanning/TimeBasedPathPlanning/__init__.py
diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py
index 0a1f6f5526..a27e1b6928 100644
--- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py
+++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py
@@ -146,20 +146,20 @@ def main():
oy = []
for i in range(60):
- ox.append(i)
+ ox.append(float(i))
oy.append(0.0)
for i in range(60):
ox.append(60.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(61):
- ox.append(i)
+ ox.append(float(i))
oy.append(60.0)
for i in range(61):
ox.append(0.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(20.0)
- oy.append(i)
+ oy.append(float(i))
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
diff --git a/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py b/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py
index 8968c54dcc..c5a139454b 100644
--- a/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py
+++ b/PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py
@@ -4,7 +4,7 @@
author: Todd Tang
paper: Planning paths of complete coverage of an unstructured environment
by a mobile robot - Zelinsky et.al.
-link: http://pinkwink.kr/attachment/cfile3.uf@1354654A4E8945BD13FE77.pdf
+link: https://pinkwink.kr/attachment/cfile3.uf@1354654A4E8945BD13FE77.pdf
"""
import os
@@ -64,7 +64,7 @@ def transform(
is_visited = np.zeros_like(transform_matrix, dtype=bool)
is_visited[src[0], src[1]] = True
traversal_queue = [src]
- calculated = [(src[0] - 1) * n_cols + src[1]]
+ calculated = set([(src[0] - 1) * n_cols + src[1]])
def is_valid_neighbor(g_i, g_j):
return 0 <= g_i < n_rows and 0 <= g_j < n_cols \
@@ -86,7 +86,7 @@ def is_valid_neighbor(g_i, g_j):
if not is_visited[ni][nj] \
and ((ni - 1) * n_cols + nj) not in calculated:
traversal_queue.append((ni, nj))
- calculated.append((ni - 1) * n_cols + nj)
+ calculated.add((ni - 1) * n_cols + nj)
return transform_matrix
diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py
index bb5699b888..ee40e73504 100644
--- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py
+++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py
@@ -4,9 +4,9 @@
author Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
Shunichi09/nonlinear_control: Implementing the nonlinear model predictive
-control, sliding mode control https://github.com/Shunichi09/nonlinear_control
+control, sliding mode control https://github.com/Shunichi09/PythonLinearNonlinearControl
"""
diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
index 7e57abb80e..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,8 +11,9 @@
import numpy as np
import scipy.linalg as la
import pathlib
-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 =====
@@ -52,7 +53,7 @@ def update(state, a, delta):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def solve_dare(A, B, Q, R):
@@ -221,8 +222,9 @@ def do_simulation(cx, cy, cyaw, ck, speed_profile, goal):
if target_ind % 1 == 0 and show_animation:
plt.cla()
# for stopping simulation with the esc key.
- plt.gcf().canvas.mpl_connect('key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
@@ -290,6 +292,13 @@ def main():
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
+ plt.subplots(1)
+
+ plt.plot(t, np.array(v)*3.6, label="speed")
+ plt.grid(True)
+ plt.xlabel("Time [sec]")
+ plt.ylabel("Speed [m/s]")
+ plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py
index 95a1b92ce5..3c066917ff 100644
--- a/PathTracking/lqr_steer_control/lqr_steer_control.py
+++ b/PathTracking/lqr_steer_control/lqr_steer_control.py
@@ -11,8 +11,9 @@
import numpy as np
import sys
import pathlib
-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
@@ -54,14 +55,14 @@ def update(state, a, delta):
return state
-def PIDControl(target, current):
+def pid_control(target, current):
a = Kp * (target - current)
return a
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def solve_DARE(A, B, Q, R):
@@ -69,10 +70,11 @@ def solve_DARE(A, B, Q, R):
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X = Q
- maxiter = 150
+ Xn = Q
+ max_iter = 150
eps = 0.01
- for i in range(maxiter):
+ for i in range(max_iter):
Xn = A.T @ X @ A - A.T @ X @ B @ \
la.inv(R + B.T @ X @ B) @ B.T @ X @ A + Q
if (abs(Xn - X)).max() < eps:
@@ -177,7 +179,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
dl, target_ind, e, e_th = lqr_steering_control(
state, cx, cy, cyaw, ck, e, e_th)
- ai = PIDControl(speed_profile[target_ind], state.v)
+ ai = pid_control(speed_profile[target_ind], state.v)
state = update(state, ai, dl)
if abs(state.v) <= stop_speed:
@@ -201,8 +203,9 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
if target_ind % 1 == 0 and show_animation:
plt.cla()
# for stopping simulation with the esc key.
- plt.gcf().canvas.mpl_connect('key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
index 3015313198..eb2d7b6a73 100644
--- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
+++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
@@ -6,6 +6,7 @@
"""
import matplotlib.pyplot as plt
+import time
import cvxpy
import math
import numpy as np
@@ -13,6 +14,8 @@
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+
from PathPlanning.CubicSpline import cubic_spline_planner
NX = 4 # x = x, y, v, yaw
@@ -69,13 +72,7 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
def pi_2_pi(angle):
- while(angle > math.pi):
- angle = angle - 2.0 * math.pi
-
- while(angle < -math.pi):
- angle = angle + 2.0 * math.pi
-
- return angle
+ return angle_mod(angle)
def get_linear_model_matrix(v, phi, delta):
@@ -286,7 +283,7 @@ def linear_mpc_control(xref, xbar, x0, dref):
constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]
prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
- prob.solve(solver=cvxpy.ECOS, verbose=False)
+ prob.solve(solver=cvxpy.CLARABEL, verbose=False)
if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
ox = get_nparray_from_matrix(x.value[0, :])
@@ -550,6 +547,7 @@ def get_switch_back_course(dl):
def main():
print(__file__ + " start!!")
+ start = time.time()
dl = 1.0 # course tick
# cx, cy, cyaw, ck = get_straight_course(dl)
@@ -565,6 +563,9 @@ def main():
t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state)
+ elapsed_time = time.time() - start
+ print(f"calc time:{elapsed_time:.6f} [sec]")
+
if show_animation: # pragma: no cover
plt.close("all")
plt.subplots()
@@ -587,6 +588,7 @@ def main():
def main2():
print(__file__ + " start!!")
+ start = time.time()
dl = 1.0 # course tick
cx, cy, cyaw, ck = get_straight_course3(dl)
@@ -598,6 +600,9 @@ def main2():
t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state)
+ elapsed_time = time.time() - start
+ print(f"calc time:{elapsed_time:.6f} [sec]")
+
if show_animation: # pragma: no cover
plt.close("all")
plt.subplots()
diff --git a/Control/move_to_pose/__init__.py b/PathTracking/move_to_pose/__init__.py
similarity index 100%
rename from Control/move_to_pose/__init__.py
rename to PathTracking/move_to_pose/__init__.py
diff --git a/Control/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
similarity index 64%
rename from Control/move_to_pose/move_to_pose.py
rename to PathTracking/move_to_pose/move_to_pose.py
index 73604ccd7f..faf1264953 100644
--- a/Control/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -5,6 +5,7 @@
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)
Seied Muhammad Yazdian (@Muhammad-Yazdian)
+ Wang Zheng (@Aglargil)
P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
@@ -12,6 +13,11 @@
import matplotlib.pyplot as plt
import numpy as np
+import sys
+import pathlib
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
from random import random
@@ -70,15 +76,20 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
# [-pi, pi] to prevent unstable behavior e.g. difference going
# from 0 rad to 2*pi rad with slight turn
+ # The velocity v always has a constant sign which depends on the initial value of α.
rho = np.hypot(x_diff, y_diff)
- alpha = (np.arctan2(y_diff, x_diff)
- - theta + np.pi) % (2 * np.pi) - np.pi
- beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
v = self.Kp_rho * rho
- w = self.Kp_alpha * alpha - controller.Kp_beta * beta
+ alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta)
+ beta = angle_mod(theta_goal - theta - alpha)
if alpha > np.pi / 2 or alpha < -np.pi / 2:
+ # recalculate alpha to make alpha in the range of [-pi/2, pi/2]
+ alpha = angle_mod(np.arctan2(-y_diff, -x_diff) - theta)
+ beta = angle_mod(theta_goal - theta - alpha)
+ w = self.Kp_alpha * alpha - self.Kp_beta * beta
v = -v
+ else:
+ w = self.Kp_alpha * alpha - self.Kp_beta * beta
return rho, v, w
@@ -86,6 +97,7 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
# simulation parameters
controller = PathFinderController(9, 15, 3)
dt = 0.01
+MAX_SIM_TIME = 5 # seconds, robot will stop moving when time exceeds this value
# Robot specifications
MAX_LINEAR_SPEED = 15
@@ -102,18 +114,19 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
x_diff = x_goal - x
y_diff = y_goal - y
- x_traj, y_traj = [], []
+ x_traj, y_traj, v_traj, w_traj = [x], [y], [0], [0]
rho = np.hypot(x_diff, y_diff)
- while rho > 0.001:
+ t = 0
+ while rho > 0.001 and t < MAX_SIM_TIME:
+ t += dt
x_traj.append(x)
y_traj.append(y)
x_diff = x_goal - x
y_diff = y_goal - y
- rho, v, w = controller.calc_control_command(
- x_diff, y_diff, theta, theta_goal)
+ rho, v, w = controller.calc_control_command(x_diff, y_diff, theta, theta_goal)
if abs(v) > MAX_LINEAR_SPEED:
v = np.sign(v) * MAX_LINEAR_SPEED
@@ -121,18 +134,35 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
if abs(w) > MAX_ANGULAR_SPEED:
w = np.sign(w) * MAX_ANGULAR_SPEED
+ v_traj.append(v)
+ w_traj.append(w)
+
theta = theta + w * dt
x = x + v * np.cos(theta) * dt
y = y + v * np.sin(theta) * dt
if show_animation: # pragma: no cover
plt.cla()
- plt.arrow(x_start, y_start, np.cos(theta_start),
- np.sin(theta_start), color='r', width=0.1)
- plt.arrow(x_goal, y_goal, np.cos(theta_goal),
- np.sin(theta_goal), color='g', width=0.1)
+ plt.arrow(
+ x_start,
+ y_start,
+ np.cos(theta_start),
+ np.sin(theta_start),
+ color="r",
+ width=0.1,
+ )
+ plt.arrow(
+ x_goal,
+ y_goal,
+ np.cos(theta_goal),
+ np.sin(theta_goal),
+ color="g",
+ width=0.1,
+ )
plot_vehicle(x, y, theta, x_traj, y_traj)
+ return x_traj, y_traj, v_traj, w_traj
+
def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
# Corners of triangular vehicle when pointing to the right (0 radians)
@@ -145,16 +175,16 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
p2 = np.matmul(T, p2_i)
p3 = np.matmul(T, p3_i)
- plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
- plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
- plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')
+ plt.plot([p1[0], p2[0]], [p1[1], p2[1]], "k-")
+ plt.plot([p2[0], p3[0]], [p2[1], p3[1]], "k-")
+ plt.plot([p3[0], p1[0]], [p3[1], p1[1]], "k-")
- plt.plot(x_traj, y_traj, 'b--')
+ plt.plot(x_traj, y_traj, "b--")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+ )
plt.xlim(0, 20)
plt.ylim(0, 20)
@@ -163,28 +193,31 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
def transformation_matrix(x, y, theta):
- return np.array([
- [np.cos(theta), -np.sin(theta), x],
- [np.sin(theta), np.cos(theta), y],
- [0, 0, 1]
- ])
+ return np.array(
+ [
+ [np.cos(theta), -np.sin(theta), x],
+ [np.sin(theta), np.cos(theta), y],
+ [0, 0, 1],
+ ]
+ )
def main():
-
for i in range(5):
- x_start = 20 * random()
- y_start = 20 * random()
- theta_start = 2 * np.pi * random() - np.pi
+ x_start = 20.0 * random()
+ y_start = 20.0 * random()
+ theta_start: float = 2 * np.pi * random() - np.pi
x_goal = 20 * random()
y_goal = 20 * random()
theta_goal = 2 * np.pi * random() - np.pi
- print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
- (x_start, y_start, theta_start))
- print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
- (x_goal, y_goal, theta_goal))
+ print(
+ f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n"
+ )
+ print(
+ f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n"
+ )
move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
-if __name__ == '__main__':
+if __name__ == "__main__":
main()
diff --git a/Control/move_to_pose/move_to_pose_robot.py b/PathTracking/move_to_pose/move_to_pose_robot.py
similarity index 98%
rename from Control/move_to_pose/move_to_pose_robot.py
rename to PathTracking/move_to_pose/move_to_pose_robot.py
index d5c51caa12..fe9f0d06b3 100644
--- a/Control/move_to_pose/move_to_pose_robot.py
+++ b/PathTracking/move_to_pose/move_to_pose_robot.py
@@ -79,9 +79,9 @@ def set_start_target_poses(self, pose_start, pose_target):
Parameters
----------
pose_start : (Pose)
- Start postion of the robot (see the Pose class)
+ Start position of the robot (see the Pose class)
pose_target : (Pose)
- Target postion of the robot (see the Pose class)
+ Target position of the robot (see the Pose class)
"""
self.pose_start = copy.copy(pose_start)
self.pose_target = pose_target
diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py
index ff995033a9..48453927ab 100644
--- a/PathTracking/pure_pursuit/pure_pursuit.py
+++ b/PathTracking/pure_pursuit/pure_pursuit.py
@@ -10,6 +10,11 @@
import math
import matplotlib.pyplot as plt
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+
# Parameters
k = 0.1 # look forward gain
Lfc = 2.0 # [m] look-ahead distance
@@ -17,26 +22,37 @@
dt = 0.1 # [s] time tick
WB = 2.9 # [m] wheel base of vehicle
-show_animation = True
+# Vehicle parameters
+LENGTH = WB + 1.0 # Vehicle length
+WIDTH = 2.0 # Vehicle width
+WHEEL_LEN = 0.6 # Wheel length
+WHEEL_WIDTH = 0.2 # Wheel width
+MAX_STEER = math.pi / 4 # Maximum steering angle [rad]
-class State:
- def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+show_animation = True
+pause_simulation = False # Flag for pause simulation
+is_reverse_mode = False # Flag for reverse driving mode
+
+class State:
+ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, is_reverse=False):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
- self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw))
- self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw))
+ self.direction = -1 if is_reverse else 1 # Direction based on reverse flag
+ self.rear_x = self.x - self.direction * ((WB / 2) * math.cos(self.yaw))
+ self.rear_y = self.y - self.direction * ((WB / 2) * math.sin(self.yaw))
def update(self, a, delta):
self.x += self.v * math.cos(self.yaw) * dt
self.y += self.v * math.sin(self.yaw) * dt
- self.yaw += self.v / WB * math.tan(delta) * dt
+ self.yaw += self.direction * self.v / WB * math.tan(delta) * dt
+ self.yaw = angle_mod(self.yaw)
self.v += a * dt
- self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw))
- self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw))
+ self.rear_x = self.x - self.direction * ((WB / 2) * math.cos(self.yaw))
+ self.rear_y = self.y - self.direction * ((WB / 2) * math.sin(self.yaw))
def calc_distance(self, point_x, point_y):
dx = self.rear_x - point_x
@@ -51,6 +67,7 @@ def __init__(self):
self.y = []
self.yaw = []
self.v = []
+ self.direction = []
self.t = []
def append(self, t, state):
@@ -58,6 +75,7 @@ def append(self, t, state):
self.y.append(state.y)
self.yaw.append(state.yaw)
self.v.append(state.v)
+ self.direction.append(state.direction)
self.t.append(t)
@@ -117,14 +135,18 @@ def pure_pursuit_steer_control(state, trajectory, pind):
if ind < len(trajectory.cx):
tx = trajectory.cx[ind]
ty = trajectory.cy[ind]
- else: # toward goal
+ else:
tx = trajectory.cx[-1]
ty = trajectory.cy[-1]
ind = len(trajectory.cx) - 1
alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw
- delta = math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)
+ # Reverse steering angle when reversing
+ delta = state.direction * math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)
+
+ # Limit steering angle to max value
+ delta = np.clip(delta, -MAX_STEER, MAX_STEER)
return delta, ind
@@ -142,10 +164,111 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
+def plot_vehicle(x, y, yaw, steer=0.0, color='blue', is_reverse=False):
+ """
+ Plot vehicle model with four wheels
+ Args:
+ x, y: Vehicle center position
+ yaw: Vehicle heading angle
+ steer: Steering angle
+ color: Vehicle color
+ is_reverse: Flag for reverse mode
+ """
+ # Adjust heading angle in reverse mode
+ if is_reverse:
+ yaw = angle_mod(yaw + math.pi) # Rotate heading by 180 degrees
+ steer = -steer # Reverse steering direction
+
+ def plot_wheel(x, y, yaw, steer=0.0, color=color):
+ """Plot single wheel"""
+ wheel = np.array([
+ [-WHEEL_LEN/2, WHEEL_WIDTH/2],
+ [WHEEL_LEN/2, WHEEL_WIDTH/2],
+ [WHEEL_LEN/2, -WHEEL_WIDTH/2],
+ [-WHEEL_LEN/2, -WHEEL_WIDTH/2],
+ [-WHEEL_LEN/2, WHEEL_WIDTH/2]
+ ])
+
+ # Rotate wheel if steering
+ if steer != 0:
+ c, s = np.cos(steer), np.sin(steer)
+ rot_steer = np.array([[c, -s], [s, c]])
+ wheel = wheel @ rot_steer.T
+
+ # Apply vehicle heading rotation
+ c, s = np.cos(yaw), np.sin(yaw)
+ rot_yaw = np.array([[c, -s], [s, c]])
+ wheel = wheel @ rot_yaw.T
+
+ # Translate to position
+ wheel[:, 0] += x
+ wheel[:, 1] += y
+
+ # Plot wheel with color
+ plt.plot(wheel[:, 0], wheel[:, 1], color=color)
+
+ # Calculate vehicle body corners
+ corners = np.array([
+ [-LENGTH/2, WIDTH/2],
+ [LENGTH/2, WIDTH/2],
+ [LENGTH/2, -WIDTH/2],
+ [-LENGTH/2, -WIDTH/2],
+ [-LENGTH/2, WIDTH/2]
+ ])
+
+ # Rotation matrix
+ c, s = np.cos(yaw), np.sin(yaw)
+ Rot = np.array([[c, -s], [s, c]])
+
+ # Rotate and translate vehicle body
+ rotated = corners @ Rot.T
+ rotated[:, 0] += x
+ rotated[:, 1] += y
+
+ # Plot vehicle body
+ plt.plot(rotated[:, 0], rotated[:, 1], color=color)
+
+ # Plot wheels (darker color for front wheels)
+ front_color = 'darkblue'
+ rear_color = color
+
+ # Plot four wheels
+ # Front left
+ plot_wheel(x + LENGTH/4 * c - WIDTH/2 * s,
+ y + LENGTH/4 * s + WIDTH/2 * c,
+ yaw, steer, front_color)
+ # Front right
+ plot_wheel(x + LENGTH/4 * c + WIDTH/2 * s,
+ y + LENGTH/4 * s - WIDTH/2 * c,
+ yaw, steer, front_color)
+ # Rear left
+ plot_wheel(x - LENGTH/4 * c - WIDTH/2 * s,
+ y - LENGTH/4 * s + WIDTH/2 * c,
+ yaw, color=rear_color)
+ # Rear right
+ plot_wheel(x - LENGTH/4 * c + WIDTH/2 * s,
+ y - LENGTH/4 * s - WIDTH/2 * c,
+ yaw, color=rear_color)
+
+ # Add direction arrow
+ arrow_length = LENGTH/3
+ plt.arrow(x, y,
+ -arrow_length * math.cos(yaw) if is_reverse else arrow_length * math.cos(yaw),
+ -arrow_length * math.sin(yaw) if is_reverse else arrow_length * math.sin(yaw),
+ head_width=WIDTH/4, head_length=WIDTH/4,
+ fc='r', ec='r', alpha=0.5)
+
+# Keyboard event handler
+def on_key(event):
+ global pause_simulation
+ if event.key == ' ': # Space key
+ pause_simulation = not pause_simulation
+ elif event.key == 'escape':
+ exit(0)
def main():
# target course
- cx = np.arange(0, 50, 0.5)
+ cx = -1 * np.arange(0, 50, 0.5) if is_reverse_mode else np.arange(0, 50, 0.5)
cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
target_speed = 10.0 / 3.6 # [m/s]
@@ -153,7 +276,7 @@ def main():
T = 100.0 # max simulation time
# initial state
- state = State(x=-0.0, y=-3.0, yaw=0.0, v=0.0)
+ state = State(x=-0.0, y=-3.0, yaw=math.pi if is_reverse_mode else 0.0, v=0.0, is_reverse=is_reverse_mode)
lastIndex = len(cx) - 1
time = 0.0
@@ -173,22 +296,33 @@ def main():
time += dt
states.append(time, state)
-
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
- plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
- plot_arrow(state.x, state.y, state.yaw)
+ plt.gcf().canvas.mpl_connect('key_release_event', on_key)
+ # Pass is_reverse parameter
+ plot_vehicle(state.x, state.y, state.yaw, di, is_reverse=is_reverse_mode)
plt.plot(cx, cy, "-r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
+ plt.legend() # Add legend display
+
+ # Add pause state display
+ if pause_simulation:
+ plt.text(0.02, 0.95, 'PAUSED', transform=plt.gca().transAxes,
+ bbox=dict(facecolor='red', alpha=0.5))
+
plt.pause(0.001)
+ # Handle pause state
+ while pause_simulation:
+ plt.pause(0.1) # Reduce CPU usage
+ if not plt.get_fignums(): # Check if window is closed
+ exit(0)
+
# Test
assert lastIndex >= target_ind, "Cannot goal"
diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
similarity index 95%
rename from PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
rename to PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
index ff72454f34..fd04fb6d17 100644
--- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
+++ b/PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py
@@ -8,10 +8,14 @@
import matplotlib.pyplot as plt
import math
import numpy as np
-
+import sys
+import pathlib
from scipy import interpolate
from scipy import optimize
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+
Kp = 1.0 # speed proportional gain
# steering control parameter
KTH = 1.0
@@ -51,21 +55,21 @@ def __init__(self, x, y):
self.ddY = self.Y.derivative(2)
self.length = s[-1]
-
+
def calc_yaw(self, s):
dx, dy = self.dX(s), self.dY(s)
return np.arctan2(dy, dx)
-
+
def calc_curvature(self, s):
dx, dy = self.dX(s), self.dY(s)
ddx, ddy = self.ddX(s), self.ddY(s)
return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
-
+
def __find_nearest_point(self, s0, x, y):
def calc_distance(_s, *args):
_x, _y= self.X(_s), self.Y(_s)
return (_x - args[0])**2 + (_y - args[1])**2
-
+
def calc_distance_jacobian(_s, *args):
_x, _y = self.X(_s), self.Y(_s)
_dx, _dy = self.dX(_s), self.dY(_s)
@@ -76,7 +80,7 @@ def calc_distance_jacobian(_s, *args):
def calc_track_error(self, x, y, s0):
ret = self.__find_nearest_point(s0, x, y)
-
+
s = ret[0][0]
e = ret[1]
@@ -96,13 +100,7 @@ def pid_control(target, current):
return a
def pi_2_pi(angle):
- while(angle > math.pi):
- angle = angle - 2.0 * math.pi
-
- while(angle < -math.pi):
- angle = angle + 2.0 * math.pi
-
- return angle
+ return angle_mod(angle)
def rear_wheel_feedback_control(state, e, k, yaw_ref):
v = state.v
@@ -170,7 +168,7 @@ def simulate(path_ref, goal):
plt.plot(path_ref.X(s0), path_ref.Y(s0), "xg", label="target")
plt.axis("equal")
plt.grid(True)
- plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0))
+ plt.title(f"speed[km/h]:{round(state.v * 3.6, 2):.2f}, target s-param:{s0:.2f}")
plt.pause(0.0001)
return t, x, y, yaw, v, goal_flag
@@ -184,7 +182,7 @@ def calc_target_speed(state, yaw_ref):
if switch:
state.direction *= -1
return 0.0
-
+
if state.direction != 1:
return -target_speed
diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_control/stanley_control.py
similarity index 96%
rename from PathTracking/stanley_controller/stanley_controller.py
rename to PathTracking/stanley_control/stanley_control.py
index 9c475cbaf2..01c2ec0229 100644
--- a/PathTracking/stanley_controller/stanley_controller.py
+++ b/PathTracking/stanley_control/stanley_control.py
@@ -4,7 +4,7 @@
author: Atsushi Sakai (@Atsushi_twi)
-Ref:
+Reference:
- [Stanley: The robot that won the DARPA grand challenge](http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf)
- [Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf)
@@ -13,8 +13,9 @@
import matplotlib.pyplot as plt
import sys
import pathlib
-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
@@ -26,7 +27,7 @@
show_animation = True
-class State(object):
+class State:
"""
Class representing the state of a vehicle.
@@ -38,7 +39,7 @@ class State(object):
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
"""Instantiate the object."""
- super(State, self).__init__()
+ super().__init__()
self.x = x
self.y = y
self.yaw = yaw
@@ -106,13 +107,7 @@ def normalize_angle(angle):
:param angle: (float)
:return: (float) Angle in radian in [-pi, pi]
"""
- while angle > np.pi:
- angle -= 2.0 * np.pi
-
- while angle < -np.pi:
- angle += 2.0 * np.pi
-
- return angle
+ return angle_mod(angle)
def calc_target_index(state, cx, cy):
diff --git a/README.md b/README.md
index bc0198f78e..d1b801f219 100644
--- a/README.md
+++ b/README.md
@@ -5,9 +5,8 @@
[](https://ci.appveyor.com/project/AtsushiSakai/pythonrobotics)
-[](https://codecov.io/gh/AtsushiSakai/PythonRobotics)
-Python codes for robotics algorithm.
+Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index.html) for robotics algorithm.
# Table of Contents
@@ -74,9 +73,9 @@ Python codes for robotics algorithm.
* [1Password](#1password)
* [Authors](#authors)
-# What is this?
+# What is PythonRobotics?
-This is a Python code collection of robotics algorithms.
+PythonRobotics is a Python code collection and a [textbook](https://atsushisakai.github.io/PythonRobotics/index.html) of robotics algorithms.
Features:
@@ -86,16 +85,24 @@ Features:
3. Minimum dependency.
-See this paper for more details:
+See this documentation
+
+- [Getting Started — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/1_what_is_python_robotics.html)
+
+or this Youtube video:
+
+- [PythonRobotics project audio overview](https://www.youtube.com/watch?v=uMeRnNoJAfU)
+
+or this paper for more details:
- [\[1808\.10703\] PythonRobotics: a Python code collection of robotics algorithms](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib))
-# Requirements
+# Requirements to run the code
For running each sample code:
-- [Python 3.11.x](https://www.python.org/)
+- [Python 3.13.x](https://www.python.org/)
- [NumPy](https://numpy.org/)
@@ -111,19 +118,19 @@ For development:
- [pytest-xdist](https://pypi.org/project/pytest-xdist/) (for parallel unit tests)
-- [mypy](http://mypy-lang.org/) (for type check)
+- [mypy](https://mypy-lang.org/) (for type check)
- [sphinx](https://www.sphinx-doc.org/) (for document generation)
- [pycodestyle](https://pypi.org/project/pycodestyle/) (for code style check)
-# Documentation
+# Documentation (Textbook)
This README only shows some examples of this project.
If you are interested in other examples or mathematical backgrounds of each algorithm,
-You can check the full documentation online: [Welcome to PythonRobotics’s documentation\! — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/index.html)
+You can check the full documentation (textbook) online: [Welcome to PythonRobotics’s documentation\! — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/index.html)
All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation gifs of PythonRobotics](https://github.com/AtsushiSakai/PythonRoboticsGifs)
@@ -161,9 +168,9 @@ All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
## Particle filter localization
@@ -179,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/)
@@ -200,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/)
@@ -249,7 +256,7 @@ It can calculate a rotation matrix, and a translation vector between points and
-Ref:
+Reference
- [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
@@ -268,7 +275,7 @@ Black points are landmarks, blue crosses are estimated landmark positions by Fas
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -314,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*)
@@ -328,7 +335,7 @@ The animation shows a robot finding its path and rerouting to avoid obstacles as
Refs:
-- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pd)
+- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf)
- [Improved Fast Replanning for Robot Navigation in Unknown Terrain](http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf)
### Potential Field algorithm
@@ -339,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)
@@ -355,11 +362,11 @@ This script is a path planning code with state lattice planning.
This code uses the model predictive trajectory generator to solve boundary problem.
-Ref:
+Reference
-- [Optimal rough terrain trajectory generation for wheeled mobile robots](http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
+- [Optimal rough terrain trajectory generation for wheeled mobile robots](https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
-- [State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments](http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
+- [State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments](https://www.cs.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
### Biased polar sampling
@@ -383,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)
@@ -399,15 +406,15 @@ This is a path planning code with RRT\*
Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.
-Ref:
+Reference
- [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416)
-- [Sampling-based Algorithms for Optimal Motion Planning](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf)
+- [Sampling-based Algorithms for Optimal Motion Planning](https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf&doi=bddbc99f97173430aa49a0ada53ab5bade5902fa)
### RRT\* with reeds-shepp path
-)
+
Path planning for a car robot with RRT\* and reeds shepp path planner.
@@ -419,9 +426,9 @@ A double integrator motion model is used for LQR local planner.
-Ref:
+Reference
-- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](http://lis.csail.mit.edu/pubs/perez-icra12.pdf)
+- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](https://lis.csail.mit.edu/pubs/perez-icra12.pdf)
- [MahanFathi/LQR\-RRTstar: LQR\-RRT\* method is used for random motion planning of a simple pendulum in its phase plot](https://github.com/MahanFathi/LQR-RRTstar)
@@ -434,9 +441,9 @@ Motion planning with quintic polynomials.
It can calculate a 2D path, velocity, and acceleration profile based on quintic polynomials.
-Ref:
+Reference
-- [Local Path Planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
+- [Local Path Planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
## Reeds Shepp planning
@@ -444,7 +451,7 @@ A sample code with Reeds Shepp path planning.
-Ref:
+Reference
- [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html)
@@ -470,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)
@@ -483,9 +490,9 @@ Ref:
This is a simulation of moving to a pose control
-
+
-Ref:
+Reference
- [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
@@ -496,7 +503,7 @@ Path tracking simulation with Stanley steering control and PID speed control.
-Ref:
+Reference
- [Stanley: The robot that won the DARPA grand challenge](http://robots.stanford.edu/papers/thrun.stanley05.pdf)
@@ -510,7 +517,7 @@ Path tracking simulation with rear wheel feedback steering control and PID speed
-Ref:
+Reference
- [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
@@ -521,9 +528,9 @@ Path tracking simulation with LQR speed and steering control.
-Ref:
+Reference
-- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](http://ieeexplore.ieee.org/document/5940562/)
+- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](https://ieeexplore.ieee.org/document/5940562/)
## Model predictive speed and steering control
@@ -532,9 +539,9 @@ Path tracking simulation with iterative linear model predictive speed and steeri
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)
- [Real\-time Model Predictive Control \(MPC\), ACADO, Python \| Work\-is\-Playing](http://grauonline.de/wordpress/?page_id=3244)
@@ -544,9 +551,9 @@ A motion planning and path tracking simulation with NMPC of C-GMRES
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/cgmres_nmpc/cgmres_nmpc.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc.html)
# Arm Navigation
@@ -584,9 +591,9 @@ This is a 3d trajectory generation simulation for a rocket powered landing.
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
# Bipedal
@@ -614,7 +621,7 @@ This is a list of user's comment and references:[users\_comments](https://github
Any contribution is welcome!!
-Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/how_to_contribute.html)
+Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html)
# Citing
@@ -630,7 +637,7 @@ If you or your company would like to support this project, please consider:
- [Become a backer or sponsor on Patreon](https://www.patreon.com/myenigma)
-- [One-time donation via PayPal](https://www.paypal.me/myenigmapay/)
+- [One-time donation via PayPal](https://www.paypal.com/paypalme/myenigmapay/)
If you would like to support us in some other way, please contact with creating an issue.
@@ -640,7 +647,7 @@ If you would like to support us in some other way, please contact with creating
They are providing a free license of their IDEs for this OSS development.
-### [1Password](https://github.com/1Password/1password-teams-open-source)
+### [1Password](https://github.com/1Password/for-open-source)
They are providing a free license of their 1Password team license for this OSS project.
diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index bb648ce9d9..5c4417d7c3 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/SLAM/EKFSLAM/ekf_slam.py
@@ -8,6 +8,7 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2
@@ -28,10 +29,9 @@
def ekf_slam(xEst, PEst, u, z):
# Predict
- S = STATE_SIZE
- G, Fx = jacob_motion(xEst[0:S], u)
- xEst[0:S] = motion_model(xEst[0:S], u)
- PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
+ G, Fx = jacob_motion(xEst, u)
+ xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
+ PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
initP = np.eye(2)
# Update
@@ -119,7 +119,7 @@ def jacob_motion(x, u):
[0.0, 0.0, DT * u[0, 0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]], dtype=float)
- G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
+ G = np.eye(len(x)) + Fx.T @ jF @ Fx
return G, Fx,
@@ -192,7 +192,7 @@ def jacob_h(q, delta, x, i):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py
index 7b89f52df4..98f8a66417 100644
--- a/SLAM/FastSLAM1/fast_slam1.py
+++ b/SLAM/FastSLAM1/fast_slam1.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -71,19 +72,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
- # print(xEst)
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -234,28 +234,27 @@ def resampling(particles):
pw = np.array(pw)
n_eff = 1.0 / (pw @ pw.T) # Effective particle number
- # print(n_eff)
if n_eff < NTH: # resampling
w_cum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -274,34 +273,34 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, rfid):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
for i in range(len(rfid[:, 0])):
- dx = rfid[i, 0] - xTrue[0, 0]
- dy = rfid[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_with_noize = angle + np.random.randn() * Q_sim[
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_with_noize = angle + np.random.randn() * Q_SIM[
1, 1] ** 0.5 # add noise
zi = np.array([dn, pi_2_pi(angle_with_noize), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -321,7 +320,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -330,7 +329,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -339,17 +338,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -357,18 +356,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam1(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -376,16 +375,16 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event', lambda event:
[exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py
index aa77aa956a..d4cf0d84dd 100644
--- a/SLAM/FastSLAM2/fast_slam2.py
+++ b/SLAM/FastSLAM2/fast_slam2.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -34,16 +35,16 @@
class Particle:
- def __init__(self, N_LM):
+ def __init__(self, n_landmark):
self.w = 1.0 / N_PARTICLE
self.x = 0.0
self.y = 0.0
self.yaw = 0.0
self.P = np.eye(3)
# landmark x-y positions
- self.lm = np.zeros((N_LM, LM_SIZE))
+ self.lm = np.zeros((n_landmark, LM_SIZE))
# landmark position covariance
- self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))
+ self.lmP = np.zeros((n_landmark * LM_SIZE, LM_SIZE))
def fast_slam2(particles, u, z):
@@ -72,18 +73,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -265,21 +266,21 @@ def resampling(particles):
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -298,35 +299,35 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, RFID):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
- for i in range(len(RFID[:, 0])):
+ for i in range(len(rfid[:, 0])):
- dx = RFID[i, 0] - xTrue[0, 0]
- dy = RFID[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_noise = np.random.randn() * Q_sim[1, 1] ** 0.5
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_noise = np.random.randn() * Q_SIM[1, 1] ** 0.5
angle_with_noise = angle + angle_noise # add noise
zi = np.array([dn, pi_2_pi(angle_with_noise), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -346,7 +347,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -355,7 +356,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -364,17 +365,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -382,18 +383,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam2(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -401,21 +402,21 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for iz in range(len(z[:, 0])):
landmark_id = int(z[2, iz])
- plt.plot([xEst[0][0], RFID[landmark_id, 0]], [
- xEst[1][0], RFID[landmark_id, 1]], "-k")
+ plt.plot([x_est[0][0], rfid[landmark_id, 0]], [
+ x_est[1][0], rfid[landmark_id, 1]], "-k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/GraphBasedSLAM/data/README.rst b/SLAM/GraphBasedSLAM/data/README.rst
index c875cce73f..15bc5b6c03 100644
--- a/SLAM/GraphBasedSLAM/data/README.rst
+++ b/SLAM/GraphBasedSLAM/data/README.rst
@@ -3,4 +3,4 @@ Acknowledgments and References
Thanks to Luca Larlone for allowing inclusion of the `Intel dataset `_ in this repo.
-1. Carlone, L. and Censi, A., 2014. `From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization `_. IEEE Transactions on Robotics, 30(2), pp.475-492.
+1. Carlone, L. and Censi, A., 2014. `From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization `_. IEEE Transactions on Robotics, 30(2), pp.475-492.
diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py
index 4d66ef6732..edd20a959c 100644
--- a/SLAM/GraphBasedSLAM/graph_based_slam.py
+++ b/SLAM/GraphBasedSLAM/graph_based_slam.py
@@ -21,6 +21,7 @@
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
+from utils.angle import angle_mod
# Simulation parameter
Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
@@ -249,7 +250,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
diff --git a/SLAM/GraphBasedSLAM/graphslam/pose/se2.py b/SLAM/GraphBasedSLAM/graphslam/pose/se2.py
index e89317a06f..2a32e765f7 100644
--- a/SLAM/GraphBasedSLAM/graphslam/pose/se2.py
+++ b/SLAM/GraphBasedSLAM/graphslam/pose/se2.py
@@ -136,8 +136,8 @@ def inverse(self):
"""
return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]),
- self[0] * np.sin(self[2]) - self[1] * np.cos(
- [self[2]])], -self[2])
+ self[0] * np.sin(self[2]) - self[1] * np.cos(self[2])],
+ -self[2])
# ======================================================================= #
# #
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/ICPMatching/icp_matching.py
similarity index 100%
rename from SLAM/iterative_closest_point/iterative_closest_point.py
rename to SLAM/ICPMatching/icp_matching.py
diff --git a/appveyor.yml b/appveyor.yml
index 4964bab3da..72d89acf11 100644
--- a/appveyor.yml
+++ b/appveyor.yml
@@ -4,11 +4,11 @@ environment:
global:
# SDK v7.0 MSVC Express 2008's SetEnv.cmd script will fail if the
# /E:ON and /V:ON options are not enabled in the batch script intepreter
- # See: http://stackoverflow.com/a/13751649/163740
+ # See: https://stackoverflow.com/a/13751649/163740
CMD_IN_ENV: "cmd /E:ON /V:ON /C .\\appveyor\\run_with_env.cmd"
matrix:
- - PYTHON_DIR: C:\Python310-x64
+ - PYTHON_DIR: C:\Python313-x64
branches:
only:
diff --git a/docs/Makefile b/docs/Makefile
index 9296811e02..ae495eb36d 100644
--- a/docs/Makefile
+++ b/docs/Makefile
@@ -2,7 +2,8 @@
#
# You can set these variables from the command line.
-SPHINXOPTS =
+# SPHINXOPTS with -W means turn warnings into errors to fail the build when warnings are present.
+SPHINXOPTS = -W
SPHINXBUILD = sphinx-build
SPHINXPROJ = PythonRobotics
SOURCEDIR = .
diff --git a/docs/README.md b/docs/README.md
index 5c4145e8e3..fb7d4cc3bc 100644
--- a/docs/README.md
+++ b/docs/README.md
@@ -8,7 +8,7 @@ This folder contains documentation for the Python Robotics project.
#### Install Sphinx and Theme
```
-pip install sphinx sphinx-autobuild sphinx-rtd-theme
+pip install sphinx sphinx-autobuild sphinx-rtd-theme sphinx_rtd_dark_mode sphinx_copybutton sphinx_rtd_dark_mode
```
#### Building the Docs
diff --git a/docs/_static/img/source_link_1.png b/docs/_static/img/source_link_1.png
new file mode 100644
index 0000000000..53febda7cb
Binary files /dev/null and b/docs/_static/img/source_link_1.png differ
diff --git a/docs/_static/img/source_link_2.png b/docs/_static/img/source_link_2.png
new file mode 100644
index 0000000000..d5647cac60
Binary files /dev/null and b/docs/_static/img/source_link_2.png differ
diff --git a/docs/conf.py b/docs/conf.py
index df9b8fce22..eeabab11b1 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -3,7 +3,7 @@
#
# This file does only contain a selection of the most common options. For a
# full list see the documentation:
-# http://www.sphinx-doc.org/en/master/config
+# https://www.sphinx-doc.org/en/master/config
# -- Path setup --------------------------------------------------------------
@@ -13,13 +13,15 @@
#
import os
import sys
+from pathlib import Path
+
sys.path.insert(0, os.path.abspath('../'))
# -- Project information -----------------------------------------------------
project = 'PythonRobotics'
-copyright = '2018-2021, Atsushi Sakai'
+copyright = '2018-now, Atsushi Sakai'
author = 'Atsushi Sakai'
# The short X.Y version
@@ -41,7 +43,7 @@
'matplotlib.sphinxext.plot_directive',
'sphinx.ext.autodoc',
'sphinx.ext.mathjax',
- 'sphinx.ext.viewcode',
+ 'sphinx.ext.linkcode',
'sphinx.ext.napoleon',
'sphinx.ext.imgconverter',
'IPython.sphinxext.ipython_console_highlighting',
@@ -184,4 +186,45 @@
]
-# -- Extension configuration -------------------------------------------------
+# -- linkcode setting -------------------------------------------------
+
+import inspect
+import os
+import sys
+import functools
+
+GITHUB_REPO = "https://github.com/AtsushiSakai/PythonRobotics"
+GITHUB_BRANCH = "master"
+
+
+def linkcode_resolve(domain, info):
+ if domain != "py":
+ return None
+
+ modname = info["module"]
+ fullname = info["fullname"]
+
+ try:
+ module = __import__(modname, fromlist=[fullname])
+ obj = functools.reduce(getattr, fullname.split("."), module)
+ except (ImportError, AttributeError):
+ return None
+
+ try:
+ srcfile = inspect.getsourcefile(obj)
+ srcfile = get_relative_path_from_parent(srcfile, "PythonRobotics")
+ lineno = inspect.getsourcelines(obj)[1]
+ except Exception:
+ return None
+
+ return f"{GITHUB_REPO}/blob/{GITHUB_BRANCH}/{srcfile}#L{lineno}"
+
+
+def get_relative_path_from_parent(file_path: str, parent_dir: str):
+ path = Path(file_path).resolve()
+
+ try:
+ parent_path = next(p for p in path.parents if p.name == parent_dir)
+ return str(path.relative_to(parent_path))
+ except StopIteration:
+ raise ValueError(f"Parent directory '{parent_dir}' not found in {file_path}")
diff --git a/docs/doc_requirements.txt b/docs/doc_requirements.txt
index a8cd2bde40..b29f4ba289 100644
--- a/docs/doc_requirements.txt
+++ b/docs/doc_requirements.txt
@@ -1,6 +1,6 @@
-sphinx == 4.3.2 # For sphinx documentation
-sphinx_rtd_theme == 1.0.0
-IPython == 8.10.0 # For sphinx documentation
+sphinx == 7.2.6 # For sphinx documentation
+sphinx_rtd_theme == 2.0.0
+IPython == 8.20.0 # For sphinx documentation
sphinxcontrib-napoleon == 0.7 # For auto doc
sphinx-copybutton
-sphinx-rtd-dark-mode
\ No newline at end of file
+sphinx-rtd-dark-mode
diff --git a/docs/getting_started_main.rst b/docs/getting_started_main.rst
deleted file mode 100644
index 497b85a23a..0000000000
--- a/docs/getting_started_main.rst
+++ /dev/null
@@ -1,85 +0,0 @@
-.. _`getting started`:
-
-Getting Started
-===============
-
-What is this?
--------------
-
-This is an Open Source Software (OSS) project: PythonRobotics, which is a Python code collection of robotics algorithms.
-
-The focus of the project is on autonomous navigation, and the goal is for beginners in robotics to understand the basic ideas behind each algorithm.
-
-In this project, the algorithms which are practical and widely used in both academia and industry are selected.
-
-Each sample code is written in Python3 and only depends on some standard modules for readability and ease of use.
-
-It includes intuitive animations to understand the behavior of the simulation.
-
-
-See this paper for more details:
-
-- PythonRobotics: a Python code collection of robotics algorithms: https://arxiv.org/abs/1808.10703
-
-.. _`Requirements`:
-
-Requirements
--------------
-
-- `Python 3.11.x`_
-- `NumPy`_
-- `SciPy`_
-- `Matplotlib`_
-- `cvxpy`_
-
-For development:
-
-- `pytest`_ (for unit tests)
-- `pytest-xdist`_ (for parallel unit tests)
-- `mypy`_ (for type check)
-- `sphinx`_ (for document generation)
-- `ruff`_ (for code style check)
-
-.. _`Python 3.11.x`: https://www.python.org/
-.. _`NumPy`: https://numpy.org/
-.. _`SciPy`: https://scipy.org/
-.. _`Matplotlib`: https://matplotlib.org/
-.. _`cvxpy`: https://www.cvxpy.org/
-.. _`pytest`: https://docs.pytest.org/en/latest/
-.. _`pytest-xdist`: https://github.com/pytest-dev/pytest-xdist
-.. _`mypy`: https://mypy-lang.org/
-.. _`sphinx`: https://www.sphinx-doc.org/en/master/index.html
-.. _`ruff`: https://github.com/charliermarsh/ruff
-
-
-How to use
-----------
-
-1. Clone this repo and go into dir.
-
-.. code-block::
-
- >$ git clone https://github.com/AtsushiSakai/PythonRobotics.git
-
- >$ cd PythonRobotics
-
-
-2. Install the required libraries.
-
-using conda :
-
-.. code-block::
-
- >$ conda env create -f requirements/environment.yml
-
-using pip :
-
-.. code-block::
-
- >$ pip install -r requirements/requirements.txt
-
-
-3. Execute python script in each directory.
-
-4. Add star to this repo if you like it 😃.
-
diff --git a/docs/index_main.rst b/docs/index_main.rst
index 67bd9889f2..65634f32e8 100644
--- a/docs/index_main.rst
+++ b/docs/index_main.rst
@@ -6,47 +6,47 @@
Welcome to PythonRobotics's documentation!
==========================================
-Python codes for robotics algorithm. The project is on `GitHub`_.
+"PythonRobotics" is a Python code collections and textbook
+(This document) for robotics algorithm, which is developed on `GitHub`_.
-This is a Python code collection of robotics algorithms.
+See this section (:ref:`What is PythonRobotics?`) for more details of this project.
-Features:
+This project is `the one of the most popular open-source software (OSS) in
+the field of robotics on GitHub`_.
+This is `user comments about this project`_, and
+this graph shows GitHub star history of this project:
-1. Easy to read for understanding each algorithm's basic idea.
+.. image:: https://api.star-history.com/svg?repos=AtsushiSakai/PythonRobotics&type=Date
+ :alt: Star History
+ :width: 80%
+ :align: center
-2. Widely used and practical algorithms are selected.
-3. Minimum dependency.
-
-See this paper for more details:
-
-- `[1808.10703] PythonRobotics: a Python code collection of robotics
- algorithms`_ (`BibTeX`_)
-
-
-.. _`[1808.10703] PythonRobotics: a Python code collection of robotics algorithms`: https://arxiv.org/abs/1808.10703
-.. _BibTeX: https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib
.. _GitHub: https://github.com/AtsushiSakai/PythonRobotics
+.. _`user comments about this project`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/users_comments.md
+.. _`MIT license`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE
+.. _`the one of the most popular open-source software (OSS) in the field of robotics on GitHub`: https://github.com/topics/robotics
+----------------------------------
.. toctree::
:maxdepth: 2
- :caption: Contents
-
- getting_started
- modules/introduction
- modules/localization/localization
- modules/mapping/mapping
- modules/slam/slam
- modules/path_planning/path_planning
- modules/path_tracking/path_tracking
- modules/arm_navigation/arm_navigation
- modules/aerial_navigation/aerial_navigation
- modules/bipedal/bipedal
- modules/control/control
- modules/utils/utils
- modules/appendix/appendix
- how_to_contribute
+ :caption: Table of Contents
+
+ modules/0_getting_started/0_getting_started
+ modules/1_introduction/introduction
+ modules/2_localization/localization
+ modules/3_mapping/mapping
+ modules/4_slam/slam
+ modules/5_path_planning/path_planning
+ modules/6_path_tracking/path_tracking
+ modules/7_arm_navigation/arm_navigation
+ modules/8_aerial_navigation/aerial_navigation
+ modules/9_bipedal/bipedal
+ modules/10_inverted_pendulum/inverted_pendulum
+ modules/13_mission_planning/mission_planning
+ modules/11_utils/utils
+ modules/12_appendix/appendix
Indices and tables
diff --git a/docs/make.bat b/docs/make.bat
index 6aab964dcc..07dcebea41 100644
--- a/docs/make.bat
+++ b/docs/make.bat
@@ -22,7 +22,7 @@ if errorlevel 9009 (
echo.may add the Sphinx directory to PATH.
echo.
echo.If you don't have Sphinx installed, grab it from
- echo.http://sphinx-doc.org/
+ echo.https://sphinx-doc.org/
exit /b 1
)
diff --git a/docs/modules/0_getting_started/0_getting_started_main.rst b/docs/modules/0_getting_started/0_getting_started_main.rst
new file mode 100644
index 0000000000..cb2cba4784
--- /dev/null
+++ b/docs/modules/0_getting_started/0_getting_started_main.rst
@@ -0,0 +1,13 @@
+.. _`getting started`:
+
+Getting Started
+===============
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ 1_what_is_python_robotics
+ 2_how_to_run_sample_codes
+ 3_how_to_contribute
+ 4_how_to_read_textbook
diff --git a/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst b/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst
new file mode 100644
index 0000000000..2a7bd574f0
--- /dev/null
+++ b/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst
@@ -0,0 +1,130 @@
+.. _`What is PythonRobotics?`:
+
+What is PythonRobotics?
+------------------------
+
+This is an Open Source Software (OSS) project: PythonRobotics, which is a Python code collection of robotics algorithms.
+These codes are developed under `MIT license`_ and on `GitHub`_.
+
+.. _GitHub: https://github.com/AtsushiSakai/PythonRobotics
+.. _`MIT license`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE
+
+This project has three main philosophies below:
+
+Philosophy 1. Easy to understand each algorithm's basic idea.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The goal is for beginners in robotics to understand the basic ideas behind each algorithm.
+
+`Python`_ programming language is adopted in this project to achieve the goal.
+Python is a high-level programming language that is easy to read and write.
+If the code is not easy to read, it would be difficult to achieve our goal of
+allowing beginners to understand the algorithms.
+Python has great libraries for matrix operation, mathematical and scientific operation,
+and visualization, which makes code more readable because such operations
+don’t need to be re-implemented.
+Having the core Python packages allows the user to focus on the algorithms,
+rather than the implementations.
+
+PythonRobotics provides not only the code but also intuitive animations that
+visually demonstrate the process and behavior of each algorithm over time.
+This is an example of an animation gif file that shows the process of the
+path planning in a highway scenario for autonomous vehicle.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/high_speed_and_velocity_keeping_frenet_path.gif
+
+This animation is a gif file and is created using the `Matplotlib`_ library.
+All animation gif files are stored in the `PythonRoboticsGifs`_ repository and
+all animation movies are also uploaded to this `YouTube channel`_.
+
+PythonRobotics also provides a textbook that explains the basic ideas of each algorithm.
+The PythonRobotics textbook allows you to learn fundamental algorithms used in
+robotics with minimal mathematical formulas and textual explanations,
+based on PythonRobotics’ sample codes and animations.
+The contents of this document, like the code, are managed in the PythonRobotics
+`GitHub`_ repository and converted into HTML-based online documents using `Sphinx`_,
+which is a Python-based documentation builder.
+Please refer to this section ":ref:`How to read this textbook`" for information on
+how to read this document for learning.
+
+
+.. _`Python`: https://www.python.org/
+.. _`PythonRoboticsGifs`: https://github.com/AtsushiSakai/PythonRoboticsGifs
+.. _`YouTube channel`: https://youtube.com/playlist?list=PL12URV8HFpCozuz0SDxke6b2ae5UZvIwa&si=AH2fNPPYufPtK20S
+
+
+Philosophy 2. Widely used and practical algorithms are selected.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The second philosophy is that implemented algorithms have to be practical
+and widely used in both academia and industry.
+We believe learning these algorithms will be useful in many applications.
+For example, Kalman filters and particle filter for localization,
+grid mapping for mapping,
+dynamic programming based approaches and sampling based approaches for path planning,
+and optimal control based approach for path tracking.
+These algorithms are implemented and explained in the textbook in this project.
+
+Philosophy 3. Minimum dependency.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Each sample code of PythonRobotics is written in Python3 and only depends on
+some standard modules for readability and ease to setup and use.
+
+
+.. _`Requirements`:
+
+Requirements
+============
+
+PythonRobotics depends only on the following five libraries,
+including Python itself, to run each sample code.
+
+- `Python 3.12.x`_ (for Python runtime)
+- `NumPy`_ (for matrix operation)
+- `SciPy`_ (for scientific operation)
+- `cvxpy`_ (for convex optimization)
+- `Matplotlib`_ (for visualization)
+
+If you only need to run the code, the five libraries mentioned above are sufficient.
+However, for code development or creating documentation for the textbook,
+the following additional libraries are required.
+
+- `pytest`_ (for unit tests)
+- `pytest-xdist`_ (for parallel unit tests)
+- `mypy`_ (for type check)
+- `Sphinx`_ (for document generation)
+- `ruff`_ (for code style check)
+
+.. _`Python 3.12.x`: https://www.python.org/
+.. _`NumPy`: https://numpy.org/
+.. _`SciPy`: https://scipy.org/
+.. _`Matplotlib`: https://matplotlib.org/
+.. _`cvxpy`: https://www.cvxpy.org/
+.. _`pytest`: https://docs.pytest.org/en/latest/
+.. _`pytest-xdist`: https://github.com/pytest-dev/pytest-xdist
+.. _`mypy`: https://mypy-lang.org/
+.. _`Sphinx`: https://www.sphinx-doc.org/en/master/index.html
+.. _`ruff`: https://github.com/astral-sh/ruff
+
+For instructions on installing the above libraries, please refer to
+this section ":ref:`How to run sample codes`".
+
+Audio overview of this project
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+For an audio overview of this project, please refer to this `YouTube video`_.
+
+.. _`YouTube video`: https://www.youtube.com/watch?v=uMeRnNoJAfU
+
+Arxiv paper
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+We have published a paper on this project on Arxiv in 2018.
+
+See this paper for more details about this Project:
+
+- `PythonRobotics: a Python code collection of robotics algorithms`_ (`BibTeX`_)
+
+.. _`PythonRobotics: a Python code collection of robotics algorithms`: https://arxiv.org/abs/1808.10703
+.. _`BibTeX`: https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib
+
diff --git a/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst b/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst
new file mode 100644
index 0000000000..b92fc9bde0
--- /dev/null
+++ b/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst
@@ -0,0 +1,118 @@
+.. _`How to run sample codes`:
+
+How to run sample codes
+-------------------------
+
+In this chapter, we will explain the setup process for running each sample code
+in PythonRobotics and describe the contents of each directory.
+
+Steps to setup and run sample codes
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+1. Install `Python 3.12.x`_
+
+Note that older versions of Python3 might work, but we recommend using
+the specified version, because the sample codes are only tested with it.
+
+2. If you prefer to use conda for package management, install
+Anaconda or Miniconda to use `conda`_ command.
+
+3. Clone this repo and go into dir.
+
+.. code-block::
+
+ >$ git clone https://github.com/AtsushiSakai/PythonRobotics.git
+
+ >$ cd PythonRobotics
+
+
+4. Install the required libraries.
+
+We have prepared requirements management files for `conda`_ and `pip`_ under
+the requirements directory. Using these files makes it easy to install the necessary libraries.
+
+using conda :
+
+.. code-block::
+
+ >$ conda env create -f requirements/environment.yml
+
+using pip :
+
+.. code-block::
+
+ >$ pip install -r requirements/requirements.txt
+
+
+5. Execute python script in each directory.
+
+For example, to run the sample code of `Extented Kalman Filter` in the
+`localization` directory, execute the following command:
+
+.. code-block::
+
+ >$ cd localization/extended_kalman_filter
+
+ >$ python extended_kalman_filter.py
+
+Then, you can see this animation of the EKF algorithm based localization:
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif
+
+Please refer to the `Directory structure`_ section for more details on the contents of each directory.
+
+6. Add star to this repo if you like it 😃.
+
+.. _`Python 3.12.x`: https://www.python.org/
+.. _`conda`: https://docs.conda.io/projects/conda/en/stable/user-guide/install/index.html
+.. _`pip`: https://pip.pypa.io/en/stable/
+.. _`the requirements directory`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/requirements
+
+.. _`Directory structure`:
+
+Directory structure
+~~~~~~~~~~~~~~~~~~~~
+
+The top-level directory structure of PythonRobotics is as follows:
+
+Sample codes directories:
+
+- `AerialNavigation`_ : the sample codes of aerial navigation algorithms for drones and rocket landing.
+- `ArmNavigation`_ : the sample codes of arm navigation algorithms for robotic arms.
+- `Localization`_ : the sample codes of localization algorithms.
+- `Bipedal`_ : the sample codes of bipedal walking algorithms for legged robots.
+- `Control`_ : the sample codes of control algorithms for robotic systems.
+- `Mapping`_ : the sample codes of mapping or obstacle shape recognition algorithms.
+- `PathPlanning`_ : the sample codes of path planning algorithms.
+- `PathTracking`_ : the sample codes of path tracking algorithms for car like robots.
+- `SLAM`_ : the sample codes of SLAM algorithms.
+
+Other directories:
+
+- `docs`_ : This directory contains the documentation of PythonRobotics.
+- `requirements`_ : This directory contains the requirements management files.
+- `tests`_ : This directory contains the unit test files.
+- `utils`_ : This directory contains utility functions used in some sample codes in common.
+- `.github`_ : This directory contains the GitHub Actions configuration files.
+- `.circleci`_ : This directory contains the CircleCI configuration files.
+
+The structure of this document is the same as that of the sample code
+directories mentioned above.
+For more details, please refer to the :ref:`How to read this textbook` section.
+
+
+.. _`AerialNavigation`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/AerialNavigation
+.. _`ArmNavigation`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/ArmNavigation
+.. _`Localization`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Localization
+.. _`Bipedal`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Bipedal
+.. _`Control`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Control
+.. _`Mapping`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/Mapping
+.. _`PathPlanning`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning
+.. _`PathTracking`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathTracking
+.. _`SLAM`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/SLAM
+.. _`docs`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/docs
+.. _`requirements`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/requirements
+.. _`tests`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/tests
+.. _`utils`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/utils
+.. _`.github`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/.github
+.. _`.circleci`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/.circleci
diff --git a/docs/how_to_contribute_main.rst b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
similarity index 63%
rename from docs/how_to_contribute_main.rst
rename to docs/modules/0_getting_started/3_how_to_contribute_main.rst
index 8a2f73e9cd..9e773e930c 100644
--- a/docs/how_to_contribute_main.rst
+++ b/docs/modules/0_getting_started/3_how_to_contribute_main.rst
@@ -1,10 +1,39 @@
-How To Contribute
+How to contribute
=================
This document describes how to contribute this project.
+There are several ways to contribute to this project as below:
-Adding a new algorithm example
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+#. `Adding a new algorithm example`_
+#. `Reporting and fixing a defect`_
+#. `Adding missed documentations for existing examples`_
+#. `Supporting this project`_
+
+Before contributing
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Please check following items before contributing:
+
+Understanding this project
+---------------------------
+
+Please check this :ref:`What is PythonRobotics?` section and this paper
+`PythonRobotics: a Python code collection of robotics algorithms`_
+to understand the philosophies of this project.
+
+.. _`PythonRobotics: a Python code collection of robotics algorithms`: https://arxiv.org/abs/1808.10703
+
+Check your Python version.
+---------------------------
+
+We only accept a PR for Python 3.13.x or higher.
+
+We will not accept a PR for Python 2.x.
+
+.. _`Adding a new algorithm example`:
+
+1. Adding a new algorithm example
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This is a step by step manual to add a new algorithm example.
@@ -50,7 +79,7 @@ At the least, try to run the example code without animation in the unit test.
If you want to run the test suites locally, you can use the `runtests.sh` script by just executing it.
-The `test_diff_codestyle.py`_ check code style for your PR's codes.
+The `test_codestyle.py`_ check code style for your PR's codes.
.. _`how to write doc`:
@@ -67,9 +96,28 @@ Please check other documents for details.
You can build the doc locally based on `doc README`_.
-Note that the `reStructuredText`_ based doc should only focus on the mathematics and the algorithm of the example.
+For creating a gif animation, you can use this tool: `matplotrecorder`_.
+
+The created gif file should be stored in the `PythonRoboticsGifs`_ repository,
+so please create a PR to add it and refer to it in the doc.
+
+Note that the `reStructuredText`_ based doc should only focus on the
+mathematics and the algorithm of the example.
+
+Documentations related codes should be in the python script as the header
+comments of the script or docstrings of each function.
+
+Also, each document should have a link to the code in Github.
+You can easily add the link by using the `.. autoclass::`, `.. autofunction::`, and `.. automodule` by Sphinx's `autodoc`_ module.
+
+Using this `autodoc`_ module, the generated documentations have the link to the code in Github like:
+
+.. image:: /_static/img/source_link_1.png
+
+When you click the link, you will jump to the source code in Github like:
+
+.. image:: /_static/img/source_link_2.png
-Documentations related codes should be in the python script as the header comments of the script or docstrings of each function.
.. _`submit a pull request`:
@@ -92,9 +140,12 @@ After that, I will start the review.
Note that this is my hobby project; I appreciate your patience during the review process.
+
+
+.. _`Reporting and fixing a defect`:
-Reporting and fixing a defect
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+2. Reporting and fixing a defect
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Reporting and fixing a defect is also great contribution.
@@ -115,19 +166,24 @@ in the test code to show the issue was solved.
This doc `submit a pull request`_ can be helpful to submit a pull request.
-Adding missed documentations for existing examples
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. _`Adding missed documentations for existing examples`:
+
+3. Adding missed documentations for existing examples
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Adding the missed documentations for existing examples is also great contribution.
If you check the `Python Robotics Docs`_, you can notice that some of the examples
-only have a simulation gif or short overview descriptions,
+only have a simulation gif or short overview descriptions or just TBD.,
but no detailed algorithm or mathematical description.
+These documents need to be improved.
This doc `how to write doc`_ can be helpful to write documents.
-Supporting this project
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. _`Supporting this project`:
+
+4. Supporting this project
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Supporting this project financially is also a great contribution!!.
@@ -141,10 +197,12 @@ If you or your company would like to support this project, please consider:
If you would like to support us in some other way, please contact with creating an issue.
-Sponsors
----------
+Current Major Sponsors:
+
+#. `GitHub`_ : They are providing a GitHub Copilot Pro license for this OSS development.
+#. `JetBrains`_ : They are providing a free license of their IDEs for this OSS development.
+#. `1Password`_ : They are providing a free license of their 1Password team license for this OSS project.
-1. `JetBrains`_ : They are providing a free license of their IDEs for this OSS development.
.. _`Python Robotics Docs`: https://atsushisakai.github.io/PythonRobotics
@@ -155,10 +213,15 @@ Sponsors
.. _`reStructuredText`: https://www.sphinx-doc.org/en/master/usage/restructuredtext/basics.html
.. _`doc modules dir`: https://github.com/AtsushiSakai/PythonRobotics/tree/master/docs/modules
.. _`doc README`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/docs/README.md
-.. _`test_diff_codestyle.py`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_diff_codestyle.py
+.. _`test_codestyle.py`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_codestyle.py
.. _`JetBrains`: https://www.jetbrains.com/
+.. _`GitHub`: https://www.github.com/
.. _`Sponsor @AtsushiSakai on GitHub Sponsors`: https://github.com/sponsors/AtsushiSakai
.. _`Become a backer or sponsor on Patreon`: https://www.patreon.com/myenigma
-.. _`One-time donation via PayPal`: https://www.paypal.me/myenigmapay/
+.. _`One-time donation via PayPal`: https://www.paypal.com/paypalme/myenigmapay/
+.. _`1Password`: https://github.com/1Password/for-open-source
+.. _`matplotrecorder`: https://github.com/AtsushiSakai/matplotrecorder
+.. _`PythonRoboticsGifs`: https://github.com/AtsushiSakai/PythonRoboticsGifs
+.. _`autodoc`: https://www.sphinx-doc.org/en/master/usage/extensions/autodoc.html
diff --git a/docs/modules/0_getting_started/4_how_to_read_textbook_main.rst b/docs/modules/0_getting_started/4_how_to_read_textbook_main.rst
new file mode 100644
index 0000000000..1625c838af
--- /dev/null
+++ b/docs/modules/0_getting_started/4_how_to_read_textbook_main.rst
@@ -0,0 +1,14 @@
+.. _`How to read this textbook`:
+
+How to read this textbook
+--------------------------
+
+This document is structured to help you learn the fundamental concepts
+behind each sample code in PythonRobotics.
+
+If you already have some knowledge of robotics technologies, you can start
+by reading any document that interests you.
+
+However, if you have no prior knowledge of robotics technologies, it is
+recommended that you first read the :ref:`Introduction` section and then proceed
+to the documents related to the technical fields that interest you.
\ No newline at end of file
diff --git a/docs/modules/control/inverted_pendulum_control/inverted-pendulum.png b/docs/modules/10_inverted_pendulum/inverted-pendulum.png
similarity index 100%
rename from docs/modules/control/inverted_pendulum_control/inverted-pendulum.png
rename to docs/modules/10_inverted_pendulum/inverted-pendulum.png
diff --git a/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control_main.rst b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
similarity index 92%
rename from docs/modules/control/inverted_pendulum_control/inverted_pendulum_control_main.rst
rename to docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
index e41729fd61..58dc0f2e57 100644
--- a/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control_main.rst
+++ b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst
@@ -1,5 +1,7 @@
-Inverted Pendulum Control
------------------------------
+.. _`Inverted Pendulum`:
+
+Inverted Pendulum
+------------------
An inverted pendulum on a cart consists of a mass :math:`m` at the top of a pole of length :math:`l` pivoted on a
horizontally moving base as shown in the adjacent.
@@ -87,6 +89,12 @@ and :math:`P` is the unique positive definite solution to the discrete time
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation_lqr.gif
+Code Link
+^^^^^^^^^^^
+
+.. autofunction:: InvertedPendulum.inverted_pendulum_lqr_control.main
+
+
MPC control
~~~~~~~~~~~~~~~~~~~~~~~~~~~
The MPC controller minimize this cost function defined as:
@@ -99,3 +107,9 @@ subject to:
- Initial state
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation.gif
+
+Code Link
+^^^^^^^^^^^
+
+.. autofunction:: InvertedPendulum.inverted_pendulum_mpc_control.main
+
diff --git a/docs/modules/utils/plot/curvature_plot.png b/docs/modules/11_utils/plot/curvature_plot.png
similarity index 100%
rename from docs/modules/utils/plot/curvature_plot.png
rename to docs/modules/11_utils/plot/curvature_plot.png
diff --git a/docs/modules/utils/plot/plot_main.rst b/docs/modules/11_utils/plot/plot_main.rst
similarity index 100%
rename from docs/modules/utils/plot/plot_main.rst
rename to docs/modules/11_utils/plot/plot_main.rst
diff --git a/docs/modules/utils/utils_main.rst b/docs/modules/11_utils/utils_main.rst
similarity index 90%
rename from docs/modules/utils/utils_main.rst
rename to docs/modules/11_utils/utils_main.rst
index ff79a26205..95c982b077 100644
--- a/docs/modules/utils/utils_main.rst
+++ b/docs/modules/11_utils/utils_main.rst
@@ -1,4 +1,4 @@
-.. _utils:
+.. _`utils`:
Utilities
==========
diff --git a/docs/modules/appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png b/docs/modules/12_appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_2_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
similarity index 99%
rename from docs/modules/appendix/Kalmanfilter_basics_2_main.rst
rename to docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
index 9ae6fc5bcb..b7ff84e6f6 100644
--- a/docs/modules/appendix/Kalmanfilter_basics_2_main.rst
+++ b/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
@@ -331,7 +331,7 @@ are vectors and matrices, but the concepts are exactly the same:
- Use the process model to predict the next state (the prior)
- Form an estimate part way between the measurement and the prior
-References:
+Reference
~~~~~~~~~~~
1. Roger Labbe’s
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png b/docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
similarity index 100%
rename from docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
rename to docs/modules/12_appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
diff --git a/docs/modules/appendix/Kalmanfilter_basics_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst
similarity index 99%
rename from docs/modules/appendix/Kalmanfilter_basics_main.rst
rename to docs/modules/12_appendix/Kalmanfilter_basics_main.rst
index 6548377e07..a1d99d47ef 100644
--- a/docs/modules/appendix/Kalmanfilter_basics_main.rst
+++ b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst
@@ -552,7 +552,7 @@ a given (X,Y) value.
.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
-References:
+Reference
~~~~~~~~~~~
1. Roger Labbe’s
diff --git a/docs/modules/appendix/appendix_main.rst b/docs/modules/12_appendix/appendix_main.rst
similarity index 61%
rename from docs/modules/appendix/appendix_main.rst
rename to docs/modules/12_appendix/appendix_main.rst
index 8a29d84676..d0b9eeea3a 100644
--- a/docs/modules/appendix/appendix_main.rst
+++ b/docs/modules/12_appendix/appendix_main.rst
@@ -1,4 +1,4 @@
-.. _appendix:
+.. _`Appendix`:
Appendix
==============
@@ -7,6 +7,9 @@ Appendix
:maxdepth: 2
:caption: Contents
+ steering_motion_model
Kalmanfilter_basics
Kalmanfilter_basics_2
+ internal_sensors
+ external_sensors
diff --git a/docs/modules/12_appendix/external_sensors_main.rst b/docs/modules/12_appendix/external_sensors_main.rst
new file mode 100644
index 0000000000..b7caaf2c3a
--- /dev/null
+++ b/docs/modules/12_appendix/external_sensors_main.rst
@@ -0,0 +1,65 @@
+.. _`External Sensors for Robots`:
+
+External Sensors for Robots
+============================
+
+This project, `PythonRobotics`, focuses on algorithms, so hardware is not included.
+However, having basic knowledge of hardware in robotics is also important for understanding algorithms.
+Therefore, we will provide an overview.
+
+Introduction
+------------
+
+In recent years, the application of robotic technology has advanced, particularly in areas such as autonomous vehicles and disaster response robots. A crucial element in these technologies is external recognition—the robot's ability to understand its surrounding environment, identify safe zones, and detect moving objects using onboard sensors. Achieving effective external recognition involves various techniques, but equally important is the selection of appropriate sensors. Robots, like the sensors they employ, come in many forms, but external recognition sensors can be broadly categorized into three types. Developing an advanced external recognition system requires a thorough understanding of each sensor's principles and characteristics to determine their optimal application. This article summarizes the principles and features of these sensors for personal study purposes.
+
+Laser Sensors
+-------------
+
+Laser sensors measure distances by utilizing light, commonly referred to as Light Detection and Ranging (LIDAR). They operate by emitting light towards an object and calculating the distance based on the time it takes for the reflected light to return, using the speed of light as a constant.
+
+Radar Sensors
+-------------
+
+Radar measures distances using radio waves, commonly referred to as Radio Detection and Ranging (RADAR). It operates by transmitting radio signals towards an object and calculating the distance based on the time it takes for the reflected waves to return, using the speed of radio waves as a constant.
+
+
+Monocular Cameras
+-----------------
+
+Monocular cameras utilize a single camera to recognize the external environment. Compared to other sensors, they can detect color and brightness information, making them primarily useful for object recognition. However, they face challenges in independently measuring distances to surrounding objects and may struggle in low-light or dark conditions.
+
+Requirements for Cameras and Image Processing in Robotics
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+While camera sensors are widely used in applications like surveillance, deploying them in robotics necessitates meeting specific requirements:
+
+1. High dynamic range to adapt to various lighting conditions
+2. Wide measurement range
+3. Capability for three-dimensional measurement through techniques like motion stereo
+4. Real-time processing with high frame rates
+5. Cost-effectiveness
+
+Stereo Cameras
+--------------
+
+Stereo cameras employ multiple cameras to measure distances to surrounding objects. By knowing the positions and orientations of each camera and analyzing the disparity in the images (parallax), the distance to a specific point (the object represented by a particular pixel) can be calculated.
+
+Characteristics of Stereo Cameras
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Advantages of stereo cameras include the ability to obtain high-precision and high-density distance information at close range, depending on factors like camera resolution and the distance between cameras (baseline). This makes them suitable for indoor robots that require precise shape recognition of nearby objects. Additionally, stereo cameras are relatively cost-effective compared to other sensors, leading to their use in consumer products like Subaru's EyeSight system. However, stereo cameras are less effective for long-distance measurements due to a decrease in accuracy proportional to the square of the distance. They are also susceptible to environmental factors such as lighting conditions.
+
+Ultrasonic Sensors
+------------------
+
+Ultrasonic sensors are commonly used in indoor robots and some automotive autonomous driving systems. Their features include affordability compared to laser or radar sensors, the ability to detect very close objects, and the capability to sense materials like glass, which may be challenging for lasers or cameras. However, they have limitations such as shorter maximum measurement distances and lower resolution and accuracy.
+
+References
+----------
+
+- Wikipedia articles:
+
+ - `Lidar Sensors `_
+ - `Radar Sensors `_
+ - `Stereo Cameras `_
+ - `Ultrasonic Sensors `_
\ No newline at end of file
diff --git a/docs/modules/12_appendix/internal_sensors_main.rst b/docs/modules/12_appendix/internal_sensors_main.rst
new file mode 100644
index 0000000000..fa2594a2bf
--- /dev/null
+++ b/docs/modules/12_appendix/internal_sensors_main.rst
@@ -0,0 +1,61 @@
+.. _`Internal Sensors for Robots`:
+
+Internal Sensors for Robots
+============================
+
+This project, `PythonRobotics`, focuses on algorithms, so hardware is not included. However, having basic knowledge of hardware in robotics is also important for understanding algorithms. Therefore, we will provide an overview.
+
+Introduction
+-------------
+
+In robotic systems, internal sensors play a crucial role in monitoring the robot’s internal state, such as orientation, acceleration, angular velocity, altitude, and temperature. These sensors provide essential feedback that supports control, localization, and safety mechanisms. While external sensors perceive the environment, internal sensors give the robot self-awareness of its own motion and condition. Understanding the principles and characteristics of these sensors is vital to fully utilize their information within algorithms and decision-making systems. This section outlines the main internal sensors used in robotics.
+
+Global Navigation Satellite System (GNSS)
+-----------------------------------------
+
+GNSS is a satellite-based navigation system that provides global positioning and time information by analyzing signals from multiple satellites. It is commonly used in outdoor environments for absolute positioning. Although GNSS offers global coverage without infrastructure dependency, its performance is limited indoors or in obstructed areas, and it suffers from low update rates and susceptibility to signal noise. It is widely used in outdoor navigation for drones, vehicles, and delivery robots.
+
+Gyroscope
+----------
+
+A gyroscope measures angular velocity around the robot’s axes, enabling orientation and attitude estimation through detection of the Coriolis effect. Gyroscopes are compact, cost-effective, and provide high update rates, but they are prone to drift and require calibration or sensor fusion for long-term accuracy. These sensors are essential in drones, balancing robots, and IMU-based systems for motion tracking.
+
+Accelerometer
+---------------
+
+An accelerometer measures linear acceleration along one or more axes, typically by detecting mass displacement due to motion. It is small, affordable, and useful for detecting movement, tilt, or vibration. However, accelerometers are limited by noisy output and cannot independently determine position without integration and fusion. They are commonly applied in wearable robotics, step counters, and vibration sensing.
+
+Magnetometer
+--------------
+
+A magnetometer measures the direction and intensity of magnetic fields, enabling heading estimation based on Earth’s magnetic field. It is lightweight and useful for orientation, especially in GPS-denied environments, though it is sensitive to interference from electronics and requires calibration. Magnetometers are often used in conjunction with IMUs for navigation and directional awareness.
+
+Inertial Measurement Unit (IMU)
+--------------------------------
+
+An IMU integrates a gyroscope, accelerometer, and sometimes a magnetometer to estimate a robot's motion and orientation through sensor fusion techniques such as Kalman filters. IMUs are compact and provide high-frequency data, which is essential for localization and navigation in GPS-denied areas. Nonetheless, they accumulate drift over time and require complex filtering to maintain accuracy. IMUs are found in drones, mobile robots, and motion tracking systems.
+
+Pressure Sensor
+----------------
+
+Pressure sensors detect atmospheric or fluid pressure by measuring the force exerted on a diaphragm. They are effective for estimating altitude and monitoring environmental conditions, especially in drones and underwater robots. Although cost-effective and efficient, their accuracy may degrade due to temperature variation and limitations in low-altitude resolution.
+
+Temperature Sensor
+--------------------
+
+Temperature sensors monitor environmental or internal component temperatures, using changes in resistance or voltage depending on sensor type (e.g., RTD or thermocouple). They are simple and reliable for safety and thermal regulation, though they may respond slowly and be affected by nearby electronics. Common applications include battery and motor monitoring, safety systems, and ambient sensing.
+
+References
+----------
+
+- *Introduction to Autonomous Mobile Robots*, Roland Siegwart, Illah Nourbakhsh, Davide Scaramuzza
+- *Probabilistic Robotics*, Sebastian Thrun, Wolfram Burgard, Dieter Fox
+- Wikipedia articles:
+
+ - `Inertial Measurement Unit (IMU) `_
+ - `Accelerometer `_
+ - `Gyroscope `_
+ - `Magnetometer `_
+ - `Pressure sensor `_
+ - `Temperature sensor `_
+- `Adafruit Sensor Guides `_
\ No newline at end of file
diff --git a/docs/modules/12_appendix/steering_motion_model/steering_model.png b/docs/modules/12_appendix/steering_motion_model/steering_model.png
new file mode 100644
index 0000000000..c66dded87a
Binary files /dev/null and b/docs/modules/12_appendix/steering_motion_model/steering_model.png differ
diff --git a/docs/modules/12_appendix/steering_motion_model/turning_radius_calc1.png b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc1.png
new file mode 100644
index 0000000000..3de7ed8797
Binary files /dev/null and b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc1.png differ
diff --git a/docs/modules/12_appendix/steering_motion_model/turning_radius_calc2.png b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc2.png
new file mode 100644
index 0000000000..f7e776bf40
Binary files /dev/null and b/docs/modules/12_appendix/steering_motion_model/turning_radius_calc2.png differ
diff --git a/docs/modules/12_appendix/steering_motion_model_main.rst b/docs/modules/12_appendix/steering_motion_model_main.rst
new file mode 100644
index 0000000000..c697123fa2
--- /dev/null
+++ b/docs/modules/12_appendix/steering_motion_model_main.rst
@@ -0,0 +1,97 @@
+
+Steering Motion Model
+-----------------------
+
+Turning radius calculation by steering motion model
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The turning Radius represents the radius of the circle when the robot turns, as shown in the diagram below.
+
+.. image:: steering_motion_model/steering_model.png
+
+When the steering angle is tilted by :math:`\delta`,
+the turning radius :math:`R` can be calculated using the following equation,
+based on the geometric relationship between the wheelbase (WB),
+which is the distance between the rear wheel center and the front wheel center,
+and the assumption that the turning radius circle passes through the center of
+the rear wheels in the diagram above.
+
+:math:`R = \frac{WB}{tan\delta}`
+
+The curvature :math:`\kappa` is the reciprocal of the turning radius:
+
+:math:`\kappa = \frac{tan\delta}{WB}`
+
+In the diagram above, the angular difference :math:`\Delta \theta` in the vehicle’s heading between two points on the turning radius :math:`R`
+is the same as the angle of the vector connecting the two points from the center of the turn.
+
+From the formula for the length of an arc and the radius,
+
+:math:`\Delta \theta = \frac{s}{R}`
+
+Here, :math:`s` is the distance between two points on the turning radius.
+
+So, yaw rate :math:`\omega` can be calculated as follows.
+
+:math:`\omega = \frac{v}{R}`
+
+and
+
+:math:`\omega = v\kappa`
+
+here, :math:`v` is the velocity of the vehicle.
+
+
+Turning radius calculation by 2 consecutive positions of the robot trajectory
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+In this section, we will derive the formula for the turning radius from 2 consecutive positions of the robot trajectory.
+
+.. image:: steering_motion_model/turning_radius_calc1.png
+
+As shown in the upper diagram above, the robot moves from a point at time :math:`t` to a point at time :math:`t+1`.
+Each point is represented by a 2D position :math:`(x_t, y_t)` and an orientation :math:`\theta_t`.
+
+The distance between the two points is :math:`d = \sqrt{(x_{t+1} - x_t)^2 + (y_{t+1} - y_t)^2}`.
+
+The angle between the two vectors from the turning center to the two points is :math:`\theta = \theta_{t+1} - \theta_t`.
+Here, by drawing a perpendicular line from the center of the turning radius
+to a straight line of length :math:`d` connecting two points,
+the following equation can be derived from the resulting right triangle.
+
+:math:`sin\frac{\theta}{2} = \frac{d}{2R}`
+
+So, the turning radius :math:`R` can be calculated as follows.
+
+:math:`R = \frac{d}{2sin\frac{\theta}{2}}`
+
+The curvature :math:`\kappa` is the reciprocal of the turning radius.
+So, the curvature can be calculated as follows.
+
+:math:`\kappa = \frac{2sin\frac{\theta}{2}}{d}`
+
+Target speed by maximum steering speed
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+If the maximum steering speed is given as :math:`\dot{\delta}_{max}`,
+the maximum curvature change rate :math:`\dot{\kappa}_{max}` can be calculated as follows:
+
+:math:`\dot{\kappa}_{max} = \frac{tan\dot{\delta}_{max}}{WB}`
+
+From the curvature calculation by 2 consecutive positions of the robot trajectory,
+
+the maximum curvature change rate :math:`\dot{\kappa}_{max}` can be calculated as follows:
+
+:math:`\dot{\kappa}_{max} = \frac{\kappa_{t+1}-\kappa_{t}}{\Delta t}`
+
+If we can assume that the vehicle will not exceed the maximum curvature change rate,
+
+the target minimum velocity :math:`v_{min}` can be calculated as follows:
+
+:math:`v_{min} = \frac{d_{t+1}+d_{t}}{\Delta t} = \frac{d_{t+1}+d_{t}}{(\kappa_{t+1}-\kappa_{t})}\frac{tan\dot{\delta}_{max}}{WB}`
+
+
+Reference
+~~~~~~~~~~~
+
+- `Vehicle Dynamics and Control `_
diff --git a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
new file mode 100644
index 0000000000..22849f7c54
--- /dev/null
+++ b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
@@ -0,0 +1,104 @@
+Behavior Tree
+-------------
+
+Behavior Tree is a modular, hierarchical decision model that is widely used in robot control, and game development.
+It present some similarities to hierarchical state machines with the key difference that the main building block of a behavior is a task rather than a state.
+Behavior Tree have been shown to generalize several other control architectures (https://ieeexplore.ieee.org/document/7790863)
+
+Code Link
+~~~~~~~~~~~~~
+
+Control Node
+++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ControlNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SequenceNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SelectorNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.WhileDoElseNode
+
+Action Node
+++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ActionNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.EchoNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SleepNode
+
+Decorator Node
+++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.DecoratorNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.InverterNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.TimeoutNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.DelayNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ForceSuccessNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ForceFailureNode
+
+Behavior Tree Factory
++++++++++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.BehaviorTreeFactory
+ :members:
+
+Behavior Tree
++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.BehaviorTree
+ :members:
+
+Example
+~~~~~~~
+
+Visualize the behavior tree by `xml-tree-visual `_.
+
+.. image:: ./robot_behavior_case.svg
+
+Print the behavior tree
+
+.. code-block:: text
+
+ Behavior Tree
+ [Robot Main Controller]
+ [Battery Management]
+ (Low Battery Detection)
+
+
+
+ [Patrol Task]
+
+ [Move to Position A]
+
+ [Obstacle Handling A]
+ [Obstacle Present]
+
+
+
+
+ [Move to Position B]
+ (Short Wait)
+
+
+ (Limited Time Obstacle Handling)
+ [Obstacle Present]
+
+
+
+ [Conditional Move to C]
+
+ [Perform Position C Task]
+
+ (Ensure Completion)
+
+
+
+
+ Behavior Tree
diff --git a/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg b/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
new file mode 100644
index 0000000000..a3d43aed52
--- /dev/null
+++ b/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
@@ -0,0 +1,22 @@
+
\ No newline at end of file
diff --git a/docs/modules/13_mission_planning/mission_planning_main.rst b/docs/modules/13_mission_planning/mission_planning_main.rst
new file mode 100644
index 0000000000..c35eacd8d5
--- /dev/null
+++ b/docs/modules/13_mission_planning/mission_planning_main.rst
@@ -0,0 +1,13 @@
+.. _`Mission Planning`:
+
+Mission Planning
+================
+
+Mission planning includes tools such as finite state machines and behavior trees used to describe robot behavior and high level task planning.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ state_machine/state_machine
+ behavior_tree/behavior_tree
diff --git a/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png b/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png
new file mode 100644
index 0000000000..fbc1369cbc
Binary files /dev/null and b/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png differ
diff --git a/docs/modules/13_mission_planning/state_machine/state_machine_main.rst b/docs/modules/13_mission_planning/state_machine/state_machine_main.rst
new file mode 100644
index 0000000000..3f516d46a9
--- /dev/null
+++ b/docs/modules/13_mission_planning/state_machine/state_machine_main.rst
@@ -0,0 +1,74 @@
+State Machine
+-------------
+
+A state machine is a model used to describe the transitions of an object between different states. It clearly shows how an object changes state based on events and may trigger corresponding actions.
+
+Core Concepts
+~~~~~~~~~~~~~
+
+- **State**: A distinct mode or condition of the system (e.g. "Idle", "Running"). Managed by State class with optional on_enter/on_exit callbacks
+- **Event**: A trigger signal that may cause state transitions (e.g. "start", "stop")
+- **Transition**: A state change path from source to destination state triggered by an event
+- **Action**: An operation executed during transition (before entering new state)
+- **Guard**: A precondition that must be satisfied to allow transition
+
+Code Link
+~~~~~~~~~~~
+
+.. autoclass:: MissionPlanning.StateMachine.state_machine.StateMachine
+ :members: add_transition, process, register_state
+ :special-members: __init__
+
+PlantUML Support
+~~~~~~~~~~~~~~~~
+
+The ``generate_plantuml()`` method creates diagrams showing:
+
+- Current state (marked with [*] arrow)
+- All possible transitions
+- Guard conditions in [brackets]
+- Actions prefixed with /
+
+Example
+~~~~~~~
+
+state machine diagram:
++++++++++++++++++++++++
+.. image:: robot_behavior_case.png
+
+state transition table:
++++++++++++++++++++++++
+.. list-table:: State Transitions
+ :header-rows: 1
+ :widths: 20 15 20 20 20
+
+ * - Source State
+ - Event
+ - Target State
+ - Guard
+ - Action
+ * - patrolling
+ - detect_task
+ - executing_task
+ -
+ -
+ * - executing_task
+ - task_complete
+ - patrolling
+ -
+ - reset_task
+ * - executing_task
+ - low_battery
+ - returning_to_base
+ - is_battery_low
+ -
+ * - returning_to_base
+ - reach_base
+ - charging
+ -
+ -
+ * - charging
+ - charge_complete
+ - patrolling
+ -
+ -
\ No newline at end of file
diff --git a/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst b/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst
new file mode 100644
index 0000000000..ca595301a6
--- /dev/null
+++ b/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst
@@ -0,0 +1,107 @@
+Definition of Robotics
+----------------------
+
+This section explains the definition, history, key components, and applications of robotics.
+
+What is Robotics?
+^^^^^^^^^^^^^^^^^^
+
+Robot is a machine that can perform tasks automatically or semi-autonomously.
+Robotics is the study of robots.
+
+The word “robot” comes from the Czech word “robota,” which means “forced labor” or “drudgery.”
+It was first used in the 1920 science fiction play `R.U.R.`_ (Rossum’s Universal Robots)
+by the Czech writer `Karel Čapek`_.
+In the play, robots were artificial workers created to serve humans, but they eventually rebelled.
+
+Over time, “robot” came to refer to machines or automated systems that can perform tasks,
+often with some level of intelligence or autonomy.
+
+Currently, 2 millions robots are working in the world, and the number is increasing every year.
+In South Korea, where the adoption of robots is particularly rapid,
+50 robots are operating per 1,000 people.
+
+.. _`R.U.R.`: https://thereader.mitpress.mit.edu/origin-word-robot-rur/
+.. _`Karel Čapek`: https://en.wikipedia.org/wiki/Karel_%C4%8Capek
+
+The History of Robots
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This timeline highlights key milestones in the history of robotics:
+
+Ancient and Early Concepts (Before 1500s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The idea of **automated machines** has existed for thousands of years.
+Ancient civilizations imagined mechanical beings:
+
+- **Ancient Greece (4th Century BC)** – Greek engineer `Hero of Alexandria`_ designed early **automata** (self-operating machines) powered by water or air.
+- **Chinese and Arabic Automata (9th–13th Century)** – Inventors like `Ismail Al-Jazari`_ created intricate mechanical devices, including water clocks and automated moving peacocks driven by hydropower.
+
+.. _`Hero of Alexandria`: https://en.wikipedia.org/wiki/Hero_of_Alexandria
+.. _`Ismail Al-Jazari`: https://en.wikipedia.org/wiki/Ismail_al-Jazari
+
+The Birth of Modern Robotics (1500s–1800s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- `Leonardo da Vinci’s Robot`_ (1495) – Designed a humanoid knight with mechanical movement.
+- `Jacques de Vaucanson’s Digesting Duck`_ (1738) – Created robotic figures like a mechanical duck that could "eat" and "digest."
+- `Industrial Revolution`_ (18th–19th Century) – Machines began replacing human labor in factories, setting the foundation for automation.
+
+.. _`Leonardo da Vinci’s Robot`: https://en.wikipedia.org/wiki/Leonardo%27s_robot
+.. _`Jacques de Vaucanson’s Digesting Duck`: https://en.wikipedia.org/wiki/Jacques_de_Vaucanson
+.. _`Industrial Revolution`: https://en.wikipedia.org/wiki/Industrial_Revolution
+
+The Rise of Industrial Robots (1900s–1950s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **The Term “Robot” (1921)** – Czech writer `Karel Čapek`_ introduced the word *“robot”* in his play `R.U.R.`_ (Rossum’s Universal Robots).
+- **Early Cybernetics (1940s–1950s)** – Scientists like `Norbert Wiener`_ developed theories of self-regulating machines, influencing modern robotics (Cybernetics).
+
+.. _`Norbert Wiener`: https://en.wikipedia.org/wiki/Norbert_Wiener
+
+The Birth of Modern Robotics (1950s–1980s)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **First Industrial Robot (1961)** – `Unimate`_, created by `George Devol`_ and `Joseph Engelberger`_, was the first programmable robot used in a factory.
+- **Rise of AI & Autonomous Robots (1970s–1980s)** – Researchers developed mobile robots like `Shakey`_ (Stanford, 1966) and AI-based control systems.
+
+.. _`Unimate`: https://en.wikipedia.org/wiki/Unimate
+.. _`George Devol`: https://en.wikipedia.org/wiki/George_Devol
+.. _`Joseph Engelberger`: https://en.wikipedia.org/wiki/Joseph_Engelberger
+.. _`Shakey`: https://en.wikipedia.org/wiki/Shakey_the_robot
+
+Advanced Robotics and AI Integration (1990s–Present)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- **Industrial Robots** – Advanced robots like `Baxter`_ and `Amazon Robotics`_ revolutionized manufacturing and logistics.
+- **Autonomous Vehicles** – Self-driving cars for robo taxi like `Waymo`_ and autonomous haulage system in mining like `AHS`_ became more advanced and bisiness-ready.
+- **Exploration Robots** – Used for planetary exploration (e.g., NASA’s `Mars rovers`_).
+- **Medical Robotics** – Robots like `da Vinci Surgical System`_ revolutionized healthcare.
+- **Personal Robots** – Devices like `Roomba`_ (vacuum robot) and `Sophia`_ (AI humanoid) became popular.
+- **Service Robots** - Assistive robots like serving robots in restaurants and hotels like `Bellabot`_.
+- **Collaborative Robots (Drones)** – Collaborative robots like UAV (Unmanned Aerial Vehicle) in drone shows and delivery services.
+
+.. _`Baxter`: https://en.wikipedia.org/wiki/Baxter_(robot)
+.. _`Amazon Robotics`: https://en.wikipedia.org/wiki/Amazon_Robotics
+.. _`Mars rovers`: https://en.wikipedia.org/wiki/Mars_rover
+.. _`Waymo`: https://waymo.com/
+.. _`AHS`: https://www.futurebridge.com/industry/perspectives-industrial-manufacturing/autonomous-haulage-systems-the-future-of-mining-operations/
+.. _`da Vinci Surgical System`: https://en.wikipedia.org/wiki/Da_Vinci_Surgical_System
+.. _`Roomba`: https://en.wikipedia.org/wiki/Roomba
+.. _`Sophia`: https://en.wikipedia.org/wiki/Sophia_(robot)
+.. _`Bellabot`: https://www.pudurobotics.com/en
+
+Key Components of Robotics
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Robotics consists of several essential components:
+
+#. Sensors – Gather information from the environment (e.g., Cameras, LiDAR, GNSS, Gyro, Accelerometer, Wheel encoders).
+#. Actuators – Enable movement and interaction with the world (e.g., Motors, Hydraulic systems).
+#. Computers – Process sensor data and make decisions (e.g., Micro-controllers, CPUs, GPUs).
+#. Power Supply – Provides energy to run the robot (e.g., Batteries, Solar power).
+#. Software & Algorithms – Allow the robot to function and make intelligent decisions (e.g., ROS, Machine learning models, Localization, Mapping, Path planning, Control).
+
+This project, `PythonRobotics`, focuses on the software and algorithms part of robotics.
+If you are interested in `Sensors` hardware, you can check :ref:`Internal Sensors for Robots` or :ref:`External Sensors for Robots`.
diff --git a/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst b/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst
new file mode 100644
index 0000000000..c47c122853
--- /dev/null
+++ b/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst
@@ -0,0 +1,135 @@
+Python for Robotics
+----------------------
+
+A programing language, Python is used for this `PythonRobotics` project
+to achieve the purposes of this project described in the :ref:`What is PythonRobotics?`.
+
+This section explains the Python itself and features for science computing and robotics.
+
+Python for general-purpose programming
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+`Python `_ is an general-purpose programming language developed by
+`Guido van Rossum `_ from the late 1980s.
+
+It features as follows:
+
+#. High-level
+#. Interpreted
+#. Dynamic type system (also type annotation is supported)
+#. Emphasizes code readability
+#. Rapid prototyping
+#. Batteries included
+#. Interoperability for C and Fortran
+
+Due to these features, Python is one of the most popular programming language
+for educational purposes for programming beginners.
+
+Python for Scientific Computing
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Python itself was not designed for scientific computing.
+However, scientists quickly recognized its strengths.
+For example,
+
+#. High-level and interpreted features enable scientists to focus on their problems without dealing with low-level programming tasks like memory management.
+#. Code readability, rapid prototyping, and batteries included features enables scientists who are not professional programmers, to solve their problems easily.
+#. The interoperability to wrap C and Fortran libraries enables scientists to access already existed powerful and optimized scientific computing libraries.
+
+To address the more needs of scientific computing, many fundamental scientific computation libraries have been developed based on the upper features.
+
+- `NumPy `_ is the fundamental package for scientific computing with Python.
+- `SciPy `_ is a library that builds on NumPy and provides a large number of functions that operate on NumPy arrays and are useful for different types of scientific and engineering applications.
+- `Matplotlib `_ is a plotting library for the Python programming language and its numerical mathematics extension NumPy.
+- `Pandas `_ is a fast, powerful, flexible, and easy-to-use open-source data analysis and data manipulation library built on top of NumPy.
+- `SymPy `_ is a Python library for symbolic mathematics.
+- `CVXPy `_ is a Python-embedded modeling language for convex optimization problems.
+
+Also, more domain-specific libraries have been developed based on these fundamental libraries:
+
+- `Scikit-learn `_ is a free software machine learning library for the Python programming language.
+- `Scikit-image `_ is a collection of algorithms for image processing.
+- `Networkx `_ is a package for the creation, manipulation for complex networks.
+- `SunPy `_ is a community-developed free and open-source software package for solar physics.
+- `Astropy `_ is a community-developed free and open-source software package for astronomy.
+
+Currently, Python is one of the most popular programming languages for scientific computing.
+
+Python for Robotics
+^^^^^^^^^^^^^^^^^^^^
+
+Python has become an increasingly popular language in robotics.
+
+These are advantages of Python for Robotics:
+
+Simplicity and Readability
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python's syntax is clear and concise, making it easier to learn and write code.
+This is crucial in robotics where complex algorithms and control logic are involved.
+
+
+Extensive libraries for scientific computation.
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Scientific computation routine are fundamental for robotics.
+For example:
+
+- Matrix operation is needed for rigid body transformation, state estimation, and model based control.
+- Optimization is needed for optimization based SLAM, optimal path planning, and optimal control.
+- Visualization is needed for robot teleoperation, debugging, and simulation.
+
+ROS supports Python
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+`ROS`_ (Robot Operating System) is an open-source and widely used framework for robotics development.
+It is designed to help developping complicated robotic applications.
+ROS provides essential tools, libraries, and drivers to simplify robot programming and integration.
+
+Key Features of ROS:
+
+- Modular Architecture – Uses a node-based system where different components (nodes) communicate via messages.
+- Hardware Abstraction – Supports various robots, sensors, and actuators, making development more flexible.
+- Powerful Communication System – Uses topics, services, and actions for efficient data exchange between components.
+- Rich Ecosystem – Offers many pre-built packages for navigation, perception, and manipulation.
+- Multi-language Support – Primarily uses Python and C++, but also supports other languages.
+- Simulation & Visualization – Tools like Gazebo (for simulation) and RViz (for visualization) aid in development and testing.
+- Scalability & Community Support – Widely used in academia and industry, with a large open-source community.
+
+ROS has strong Python support (`rospy`_ for ROS1 and `rclpy`_ for ROS2).
+This allows developers to easily create nodes, manage communication between
+different parts of a robot system, and utilize various ROS tools.
+
+.. _`ROS`: https://www.ros.org/
+.. _`rospy`: http://wiki.ros.org/rospy
+.. _`rclpy`: https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html
+
+Cross-Platform Compatibility
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python code can run on various operating systems (Windows, macOS, Linux), providing flexibility in choosing hardware platforms for robotics projects.
+
+Large Community and Support
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python has a vast and active community, offering ample resources, tutorials, and support for developers. This is invaluable when tackling challenges in robotics development.
+
+Situations which Python is NOT suitable for Robotics
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+We explained the advantages of Python for robotics.
+However, Python is not always the best choice for robotics development.
+
+These are situations where Python is NOT suitable for robotics:
+
+High-speed real-time control
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python is an interpreted language, which means it is slower than compiled languages like C++.
+This can be a disadvantage when real-time control is required,
+such as in high-speed motion control or safety-critical systems.
+
+So, for these applications, we recommend to understand the each algorithm you
+needed using this project and implement it in other suitable languages like C++.
+
+Resource-constrained systems
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Python is a high-level language that requires more memory and processing power
+compared to low-level languages.
+So, it is difficult to run Python on resource-constrained systems like
+microcontrollers or embedded devices.
+In such cases, C or C++ is more suitable for these applications.
diff --git a/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst
new file mode 100644
index 0000000000..0ed51e961b
--- /dev/null
+++ b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst
@@ -0,0 +1,68 @@
+Technologies for Robotics
+-------------------------
+
+The field of robotics needs wide areas of technologies such as mechanical engineering,
+electrical engineering, computer science, and artificial intelligence (AI).
+This project, `PythonRobotics`, only focus on computer science and artificial intelligence.
+
+The technologies for robotics are categorized as following 3 categories:
+
+#. `Autonomous Navigation`_
+#. `Manipulation`_
+#. `Robot type specific technologies`_
+
+.. _`Autonomous Navigation`:
+
+Autonomous Navigation
+^^^^^^^^^^^^^^^^^^^^^^^^
+Autonomous navigation is a capability that can move to a goal over long
+periods of time without any external control by an operator.
+
+To achieve autonomous navigation, the robot needs to have the following technologies:
+- It needs to know where it is (localization)
+- Where it is safe (mapping)
+- Where is is safe and where the robot is in the map (Simultaneous Localization and Mapping (SLAM))
+- Where and how to move (path planning)
+- How to control its motion (path following).
+
+The autonomous system would not work correctly if any of these technologies is missing.
+
+In recent years, autonomous navigation technologies have received huge
+attention in many fields.
+For example, self-driving cars, drones, and autonomous mobile robots in indoor and outdoor environments.
+
+In this project, we provide many algorithms, sample codes,
+and documentations for autonomous navigation.
+
+#. :ref:`Localization`
+#. :ref:`Mapping`
+#. :ref:`SLAM`
+#. :ref:`Path planning`
+#. :ref:`Path tracking`
+
+
+
+.. _`Manipulation`:
+
+Manipulation
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+#. :ref:`Arm Navigation`
+
+.. _`Robot type specific technologies`:
+
+Robot type specific technologies
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+#. :ref:`Aerial Navigation`
+#. :ref:`Bipedal`
+#. :ref:`Inverted Pendulum`
+
+
+Additional Information
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+#. :ref:`utils`
+#. :ref:`Appendix`
+
+
diff --git a/docs/modules/1_introduction/introduction_main.rst b/docs/modules/1_introduction/introduction_main.rst
new file mode 100644
index 0000000000..1871dfc3b1
--- /dev/null
+++ b/docs/modules/1_introduction/introduction_main.rst
@@ -0,0 +1,18 @@
+.. _Introduction:
+
+Introduction
+============
+
+PythonRobotics is composed of two words: "Python" and "Robotics".
+Therefore, I will first explain these two topics, Robotics and Python.
+After that, I will provide an overview of the robotics technologies
+covered in PythonRobotics.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Table of Contents
+
+ 1_definition_of_robotics/definition_of_robotics
+ 2_python_for_robotics/python_for_robotics
+ 3_technologies_for_robotics/technologies_for_robotics
+
diff --git a/docs/modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst b/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
similarity index 65%
rename from docs/modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
rename to docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
index 2543d1186a..214e645d10 100644
--- a/docs/modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
+++ b/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst
@@ -5,3 +5,8 @@ Ensamble Kalman Filter Localization
This is a sensor fusion localization with Ensamble Kalman Filter(EnKF).
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Localization.ensemble_kalman_filter.ensemble_kalman_filter.enkf_localization
+
diff --git a/docs/modules/2_localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png b/docs/modules/2_localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png
new file mode 100644
index 0000000000..595b651bd2
Binary files /dev/null and b/docs/modules/2_localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png differ
diff --git a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png
similarity index 100%
rename from docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png
rename to docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png
diff --git a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
similarity index 51%
rename from docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
rename to docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
index 0ec920c9df..adb41e5e40 100644
--- a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
+++ b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
@@ -2,6 +2,9 @@
Extended Kalman Filter Localization
-----------------------------------
+Position Estimation Kalman Filter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
.. image:: extended_kalman_filter_localization_1_0.png
:width: 600px
@@ -9,6 +12,7 @@ Extended Kalman Filter Localization
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif
+
This is a sensor fusion localization with Extended Kalman Filter(EKF).
The blue line is true trajectory, the black line is dead reckoning
@@ -19,11 +23,40 @@ is estimated trajectory with EKF.
The red ellipse is estimated covariance ellipse with EKF.
-Code: `PythonRobotics/extended_kalman_filter.py at master ·
-AtsushiSakai/PythonRobotics `__
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.extended_kalman_filter.extended_kalman_filter.ekf_estimation
+
+Extended Kalman Filter algorithm
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Localization process using Extended Kalman Filter:EKF is
+
+=== Predict ===
+
+:math:`x_{Pred} = Fx_t+Bu_t`
+
+:math:`P_{Pred} = J_f P_t J_f^T + Q`
+
+=== Update ===
+
+:math:`z_{Pred} = Hx_{Pred}`
+
+:math:`y = z - z_{Pred}`
+
+:math:`S = J_g P_{Pred}.J_g^T + R`
+
+:math:`K = P_{Pred}.J_g^T S^{-1}`
+
+:math:`x_{t+1} = x_{Pred} + Ky`
+
+:math:`P_{t+1} = ( I - K J_g) P_{Pred}`
+
+
Filter design
-~~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In this simulation, the robot has a state vector includes 4 states at
time :math:`t`.
@@ -61,8 +94,9 @@ In the code, “observation” function generates the input and observation
vector with noise
`code `__
+
Motion Model
-~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~
The robot model is
@@ -100,7 +134,7 @@ Its Jacobian matrix is
Observation Model
~~~~~~~~~~~~~~~~~
-The robot can get x-y position infomation from GPS.
+The robot can get x-y position information from GPS.
So GPS Observation model is
@@ -120,32 +154,99 @@ Its Jacobian matrix is
:math:`\begin{equation*} = \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}`
-Extended Kalman Filter
-~~~~~~~~~~~~~~~~~~~~~~
-Localization process using Extended Kalman Filter:EKF is
+Kalman Filter with Speed Scale Factor Correction
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+This is a Extended kalman filter (EKF) localization with velocity correction.
-=== Predict ===
+This is for correcting the vehicle speed measured with scale factor errors due to factors such as wheel wear.
-:math:`x_{Pred} = Fx_t+Bu_t`
-:math:`P_{Pred} = J_f P_t J_f^T + Q`
+In this simulation, the robot has a state vector includes 5 states at
+time :math:`t`.
-=== Update ===
+.. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t, s_t]
-:math:`z_{Pred} = Hx_{Pred}`
+x, y are a 2D x-y position, :math:`\phi` is orientation, v is
+velocity, and s is a scale factor of velocity.
-:math:`y = z - z_{Pred}`
+In the code, “xEst” means the state vector.
+`code `__
-:math:`S = J_g P_{Pred}.J_g^T + R`
+The rest is the same as the Position Estimation Kalman Filter.
-:math:`K = P_{Pred}.J_g^T S^{-1}`
+.. image:: ekf_with_velocity_correction_1_0.png
+ :width: 600px
+
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.extended_kalman_filter.ekf_with_velocity_correction.ekf_estimation
+
+
+Motion Model
+~~~~~~~~~~~~
+
+The robot model is
+
+.. math:: \dot{x} = v s \cos(\phi)
+
+.. math:: \dot{y} = v s \sin(\phi)
+
+.. math:: \dot{\phi} = \omega
+
+So, the motion model is
+
+.. math:: \textbf{x}_{t+1} = f(\textbf{x}_t, \textbf{u}_t) = F\textbf{x}_t+B\textbf{u}_t
+
+where
+
+:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} B= \begin{bmatrix} cos(\phi) \Delta t s & 0\\ sin(\phi) \Delta t s & 0\\ 0 & \Delta t\\ 1 & 0\\ 0 & 0\\ \end{bmatrix} \end{equation*}`
+
+:math:`\Delta t` is a time interval.
+
+This is implemented at
+`code `__
+
+The motion function is that
+
+:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \\ w' \\ v' \end{bmatrix} = f(\textbf{x}, \textbf{u}) = \begin{bmatrix} x + v s \cos(\phi)\Delta t \\ y + v s \sin(\phi)\Delta t \\ \phi + \omega \Delta t \\ v \end{bmatrix} \end{equation*}`
+
+Its Jacobian matrix is
+
+:math:`\begin{equation*} J_f = \begin{bmatrix} \frac{\partial x'}{\partial x}& \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v} & \frac{\partial x'}{\partial s}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{\partial v} & \frac{\partial y'}{\partial s}\\ \frac{\partial \phi'}{\partial x}& \frac{\partial \phi'}{\partial y} & \frac{\partial \phi'}{\partial \phi} & \frac{\partial \phi'}{\partial v} & \frac{\partial \phi'}{\partial s}\\ \frac{\partial v'}{\partial x}& \frac{\partial v'}{\partial y} & \frac{\partial v'}{\partial \phi} & \frac{\partial v'}{\partial v} & \frac{\partial v'}{\partial s} \\ \frac{\partial s'}{\partial x}& \frac{\partial s'}{\partial y} & \frac{\partial s'}{\partial \phi} & \frac{\partial s'}{\partial v} & \frac{\partial s'}{\partial s} \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} = \begin{bmatrix} 1& 0 & -v s \sin(\phi) \Delta t & s \cos(\phi) \Delta t & \cos(\phi) v \Delta t\\ 0 & 1 & v s \cos(\phi) \Delta t & s \sin(\phi) \Delta t & v \sin(\phi) \Delta t\\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \end{bmatrix} \end{equation*}`
+
+
+Observation Model
+~~~~~~~~~~~~~~~~~
+
+The robot can get x-y position information from GPS.
+
+So GPS Observation model is
+
+.. math:: \textbf{z}_{t} = g(\textbf{x}_t) = H \textbf{x}_t
+
+where
+
+:math:`\begin{equation*} H = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}`
+
+The observation function states that
+
+:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \end{bmatrix} = g(\textbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}`
+
+Its Jacobian matrix is
+
+:math:`\begin{equation*} J_g = \begin{bmatrix} \frac{\partial x'}{\partial x} & \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v} & \frac{\partial x'}{\partial s}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{ \partial v} & \frac{\partial y'}{ \partial s}\\ \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} = \begin{bmatrix} 1& 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ \end{bmatrix} \end{equation*}`
-:math:`x_{t+1} = x_{Pred} + Ky`
-:math:`P_{t+1} = ( I - K J_g) P_{Pred}`
-Ref:
-~~~~
+Reference
+^^^^^^^^^^^
- `PROBABILISTIC-ROBOTICS.ORG `__
diff --git a/docs/modules/localization/histogram_filter_localization/1.png b/docs/modules/2_localization/histogram_filter_localization/1.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/1.png
rename to docs/modules/2_localization/histogram_filter_localization/1.png
diff --git a/docs/modules/localization/histogram_filter_localization/2.png b/docs/modules/2_localization/histogram_filter_localization/2.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/2.png
rename to docs/modules/2_localization/histogram_filter_localization/2.png
diff --git a/docs/modules/localization/histogram_filter_localization/3.png b/docs/modules/2_localization/histogram_filter_localization/3.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/3.png
rename to docs/modules/2_localization/histogram_filter_localization/3.png
diff --git a/docs/modules/localization/histogram_filter_localization/4.png b/docs/modules/2_localization/histogram_filter_localization/4.png
similarity index 100%
rename from docs/modules/localization/histogram_filter_localization/4.png
rename to docs/modules/2_localization/histogram_filter_localization/4.png
diff --git a/docs/modules/localization/histogram_filter_localization/histogram_filter_localization_main.rst b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst
similarity index 96%
rename from docs/modules/localization/histogram_filter_localization/histogram_filter_localization_main.rst
rename to docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst
index fafd578333..3a175b1316 100644
--- a/docs/modules/localization/histogram_filter_localization/histogram_filter_localization_main.rst
+++ b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst
@@ -16,6 +16,11 @@ The filter uses speed input and range observations from RFID for localization.
Initial position information is not needed.
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.histogram_filter.histogram_filter.histogram_filter_localization
+
Filtering algorithm
~~~~~~~~~~~~~~~~~~~~
@@ -107,7 +112,7 @@ There are two ways to calculate the final positions:
-References:
+Reference
~~~~~~~~~~~
- `_PROBABILISTIC ROBOTICS: `_
diff --git a/docs/modules/localization/localization_main.rst b/docs/modules/2_localization/localization_main.rst
similarity index 55%
rename from docs/modules/localization/localization_main.rst
rename to docs/modules/2_localization/localization_main.rst
index db6651f6b8..770a234b69 100644
--- a/docs/modules/localization/localization_main.rst
+++ b/docs/modules/2_localization/localization_main.rst
@@ -1,7 +1,8 @@
-.. _localization:
+.. _`Localization`:
Localization
============
+Localization is the ability of a robot to know its position and orientation with sensors such as Global Navigation Satellite System:GNSS etc. In localization, Bayesian filters such as Kalman filters, histogram filter, and particle filter are widely used[31]. Fig.2 shows localization simulations using histogram filter and particle filter.
.. toctree::
:maxdepth: 2
diff --git a/docs/modules/localization/particle_filter_localization/particle_filter_localization_main.rst b/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst
similarity index 87%
rename from docs/modules/localization/particle_filter_localization/particle_filter_localization_main.rst
rename to docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst
index c8652ce8a7..d648d8e080 100644
--- a/docs/modules/localization/particle_filter_localization/particle_filter_localization_main.rst
+++ b/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst
@@ -15,6 +15,12 @@ It is assumed that the robot can measure a distance from landmarks
This measurements are used for PF localization.
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.particle_filter.particle_filter.pf_localization
+
+
How to calculate covariance matrix from particles
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -30,8 +36,8 @@ The covariance matrix :math:`\Xi` from particle information is calculated by the
- :math:`\mu_j` is the :math:`j` th mean state of particles.
-References:
+Reference
~~~~~~~~~~~
- `_PROBABILISTIC ROBOTICS: `_
-- `Improving the particle filter in high dimensions using conjugate artificial process noise `_
+- `Improving the particle filter in high dimensions using conjugate artificial process noise `_
diff --git a/docs/modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
similarity index 81%
rename from docs/modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
rename to docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
index bb6b5b664b..a7a5aab61b 100644
--- a/docs/modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
+++ b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst
@@ -7,7 +7,13 @@ This is a sensor fusion localization with Unscented Kalman Filter(UKF).
The lines and points are same meaning of the EKF simulation.
-References:
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: Localization.unscented_kalman_filter.unscented_kalman_filter.ukf_estimation
+
+
+Reference
~~~~~~~~~~~
- `Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization `_
diff --git a/docs/modules/mapping/circle_fitting/circle_fitting_main.rst b/docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst
similarity index 78%
rename from docs/modules/mapping/circle_fitting/circle_fitting_main.rst
rename to docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst
index 1892d1f8f7..e243529a9c 100644
--- a/docs/modules/mapping/circle_fitting/circle_fitting_main.rst
+++ b/docs/modules/3_mapping/circle_fitting/circle_fitting_main.rst
@@ -11,3 +11,7 @@ The red crosses are observations from a ranging sensor.
The red circle is the estimated object shape using circle fitting.
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.circle_fitting.circle_fitting.circle_fitting
diff --git a/docs/modules/3_mapping/distance_map/distance_map.png b/docs/modules/3_mapping/distance_map/distance_map.png
new file mode 100644
index 0000000000..2d89252a70
Binary files /dev/null and b/docs/modules/3_mapping/distance_map/distance_map.png differ
diff --git a/docs/modules/3_mapping/distance_map/distance_map_main.rst b/docs/modules/3_mapping/distance_map/distance_map_main.rst
new file mode 100644
index 0000000000..ec60e752c9
--- /dev/null
+++ b/docs/modules/3_mapping/distance_map/distance_map_main.rst
@@ -0,0 +1,27 @@
+Distance Map
+------------
+
+This is an implementation of the Distance Map algorithm for path planning.
+
+The Distance Map algorithm computes the unsigned distance field (UDF) and signed distance field (SDF) from a boolean field representing obstacles.
+
+The UDF gives the distance from each point to the nearest obstacle. The SDF gives positive distances for points outside obstacles and negative distances for points inside obstacles.
+
+Example
+~~~~~~~
+
+The algorithm is demonstrated on a simple 2D grid with obstacles:
+
+.. image:: distance_map.png
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.DistanceMap.distance_map.compute_sdf
+
+.. autofunction:: Mapping.DistanceMap.distance_map.compute_udf
+
+References
+~~~~~~~~~~
+
+- `Distance Transforms of Sampled Functions `_ paper by Pedro F. Felzenszwalb and Daniel P. Huttenlocher.
\ No newline at end of file
diff --git a/docs/modules/mapping/gaussian_grid_map/gaussian_grid_map_main.rst b/docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst
similarity index 61%
rename from docs/modules/mapping/gaussian_grid_map/gaussian_grid_map_main.rst
rename to docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst
index b0f112a871..50033d2a20 100644
--- a/docs/modules/mapping/gaussian_grid_map/gaussian_grid_map_main.rst
+++ b/docs/modules/3_mapping/gaussian_grid_map/gaussian_grid_map_main.rst
@@ -6,3 +6,9 @@ Gaussian grid map
This is a 2D Gaussian grid mapping example.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/gaussian_grid_map/animation.gif
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.gaussian_grid_map.gaussian_grid_map.generate_gaussian_grid_map
+
diff --git a/docs/modules/mapping/k_means_object_clustering/k_means_object_clustering_main.rst b/docs/modules/3_mapping/k_means_object_clustering/k_means_object_clustering_main.rst
similarity index 63%
rename from docs/modules/mapping/k_means_object_clustering/k_means_object_clustering_main.rst
rename to docs/modules/3_mapping/k_means_object_clustering/k_means_object_clustering_main.rst
index e098ca5409..0ece604009 100644
--- a/docs/modules/mapping/k_means_object_clustering/k_means_object_clustering_main.rst
+++ b/docs/modules/3_mapping/k_means_object_clustering/k_means_object_clustering_main.rst
@@ -4,3 +4,9 @@ k-means object clustering
This is a 2D object clustering with k-means algorithm.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/kmeans_clustering/animation.gif
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.kmeans_clustering.kmeans_clustering.kmeans_clustering
+
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/grid_map_example.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/grid_map_example.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/grid_map_example.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/grid_map_example.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png
similarity index 100%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png
diff --git a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
similarity index 98%
rename from docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
rename to docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
index 1f62179efd..29f5878e48 100644
--- a/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
+++ b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst
@@ -196,3 +196,9 @@ Let’s use this flood fill on real data:
.. image:: lidar_to_grid_map_tutorial_14_1.png
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.lidar_to_grid_map.lidar_to_grid_map.main
+
+
diff --git a/docs/modules/3_mapping/mapping_main.rst b/docs/modules/3_mapping/mapping_main.rst
new file mode 100644
index 0000000000..34a3744893
--- /dev/null
+++ b/docs/modules/3_mapping/mapping_main.rst
@@ -0,0 +1,20 @@
+.. _`Mapping`:
+
+Mapping
+=======
+Mapping is the ability of a robot to understand its surroundings with external sensors such as LIDAR and camera. Robots have to recognize the position and shape of obstacles to avoid them. In mapping, grid mapping and machine learning algorithms are widely used[31][18]. Fig.3 shows mapping simulation results using grid mapping with 2D ray casting and 2D object clustering with k-means algorithm.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ gaussian_grid_map/gaussian_grid_map
+ ndt_map/ndt_map
+ ray_casting_grid_map/ray_casting_grid_map
+ lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial
+ point_cloud_sampling/point_cloud_sampling
+ k_means_object_clustering/k_means_object_clustering
+ circle_fitting/circle_fitting
+ rectangle_fitting/rectangle_fitting
+ normal_vector_estimation/normal_vector_estimation
+ distance_map/distance_map
diff --git a/docs/modules/3_mapping/ndt_map/grid_clustering.png b/docs/modules/3_mapping/ndt_map/grid_clustering.png
new file mode 100644
index 0000000000..c53590287a
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/grid_clustering.png differ
diff --git a/docs/modules/3_mapping/ndt_map/ndt_map1.png b/docs/modules/3_mapping/ndt_map/ndt_map1.png
new file mode 100644
index 0000000000..1f34b9a66a
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/ndt_map1.png differ
diff --git a/docs/modules/3_mapping/ndt_map/ndt_map2.png b/docs/modules/3_mapping/ndt_map/ndt_map2.png
new file mode 100644
index 0000000000..b997062979
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/ndt_map2.png differ
diff --git a/docs/modules/3_mapping/ndt_map/ndt_map_main.rst b/docs/modules/3_mapping/ndt_map/ndt_map_main.rst
new file mode 100644
index 0000000000..bfc3936314
--- /dev/null
+++ b/docs/modules/3_mapping/ndt_map/ndt_map_main.rst
@@ -0,0 +1,53 @@
+.. _ndt_map:
+
+Normal Distance Transform (NDT) map
+------------------------------------
+
+This is a NDT mapping example.
+
+Normal Distribution Transform (NDT) is a map representation that uses normal distribution for observation point modeling.
+
+Normal Distribution
+~~~~~~~~~~~~~~~~~~~~~
+
+Normal distribution consists of two parameters: mean :math:`\mu` and covariance :math:`\Sigma`.
+
+:math:`\mathbf{X} \sim \mathcal{N}(\boldsymbol{\mu}, \boldsymbol{\Sigma})`
+
+In the 2D case, :math:`\boldsymbol{\mu}` is a 2D vector and :math:`\boldsymbol{\Sigma}` is a 2x2 matrix.
+
+In the matrix form, the probability density function of thr normal distribution is:
+
+:math:`X=\frac{1}{\sqrt{(2 \pi)^2|\Sigma|}} \exp \left\{-\frac{1}{2}^t(x-\mu) \sum^{-1}(x-\mu)\right\}`
+
+Normal Distance Transform mapping steps
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+NDT mapping consists of two steps:
+
+When we have a new observation like this:
+
+.. figure:: raw_observations.png
+
+First, we need to cluster the observation points.
+This is done by using a grid based clustering algorithm.
+
+The result is:
+
+.. figure:: grid_clustering.png
+
+Then, we need to fit a normal distribution to each grid cluster.
+
+Black ellipse shows each NDT grid like this:
+
+.. figure:: ndt_map1.png
+
+.. figure:: ndt_map2.png
+
+API
+~~~~~
+
+.. autoclass:: Mapping.ndt_map.ndt_map.NDTMap
+ :members:
+ :class-doc-from: class
+
diff --git a/docs/modules/3_mapping/ndt_map/raw_observations.png b/docs/modules/3_mapping/ndt_map/raw_observations.png
new file mode 100644
index 0000000000..c1a0dd4778
Binary files /dev/null and b/docs/modules/3_mapping/ndt_map/raw_observations.png differ
diff --git a/docs/modules/mapping/normal_vector_estimation/normal_vector_calc.png b/docs/modules/3_mapping/normal_vector_estimation/normal_vector_calc.png
similarity index 100%
rename from docs/modules/mapping/normal_vector_estimation/normal_vector_calc.png
rename to docs/modules/3_mapping/normal_vector_estimation/normal_vector_calc.png
diff --git a/docs/modules/mapping/normal_vector_estimation/normal_vector_estimation_main.rst b/docs/modules/3_mapping/normal_vector_estimation/normal_vector_estimation_main.rst
similarity index 97%
rename from docs/modules/mapping/normal_vector_estimation/normal_vector_estimation_main.rst
rename to docs/modules/3_mapping/normal_vector_estimation/normal_vector_estimation_main.rst
index a4d1bf0df2..68a19e1534 100644
--- a/docs/modules/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/mapping/normal_vector_estimation/ransac_normal_vector_estimation.png b/docs/modules/3_mapping/normal_vector_estimation/ransac_normal_vector_estimation.png
similarity index 100%
rename from docs/modules/mapping/normal_vector_estimation/ransac_normal_vector_estimation.png
rename to docs/modules/3_mapping/normal_vector_estimation/ransac_normal_vector_estimation.png
diff --git a/docs/modules/mapping/point_cloud_sampling/farthest_point_sampling.png b/docs/modules/3_mapping/point_cloud_sampling/farthest_point_sampling.png
similarity index 100%
rename from docs/modules/mapping/point_cloud_sampling/farthest_point_sampling.png
rename to docs/modules/3_mapping/point_cloud_sampling/farthest_point_sampling.png
diff --git a/docs/modules/mapping/point_cloud_sampling/point_cloud_sampling_main.rst b/docs/modules/3_mapping/point_cloud_sampling/point_cloud_sampling_main.rst
similarity index 98%
rename from docs/modules/mapping/point_cloud_sampling/point_cloud_sampling_main.rst
rename to docs/modules/3_mapping/point_cloud_sampling/point_cloud_sampling_main.rst
index cbb5652f56..8cb08d4b78 100644
--- a/docs/modules/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/mapping/point_cloud_sampling/poisson_disk_sampling.png b/docs/modules/3_mapping/point_cloud_sampling/poisson_disk_sampling.png
similarity index 100%
rename from docs/modules/mapping/point_cloud_sampling/poisson_disk_sampling.png
rename to docs/modules/3_mapping/point_cloud_sampling/poisson_disk_sampling.png
diff --git a/docs/modules/mapping/point_cloud_sampling/voxel_point_sampling.png b/docs/modules/3_mapping/point_cloud_sampling/voxel_point_sampling.png
similarity index 100%
rename from docs/modules/mapping/point_cloud_sampling/voxel_point_sampling.png
rename to docs/modules/3_mapping/point_cloud_sampling/voxel_point_sampling.png
diff --git a/docs/modules/3_mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst b/docs/modules/3_mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
new file mode 100644
index 0000000000..bee2f64193
--- /dev/null
+++ b/docs/modules/3_mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
@@ -0,0 +1,11 @@
+Ray casting grid map
+--------------------
+
+This is a 2D ray casting grid mapping example.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/raycasting_grid_map/animation.gif
+
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: Mapping.ray_casting_grid_map.ray_casting_grid_map.generate_ray_casting_grid_map
diff --git a/docs/modules/mapping/rectangle_fitting/rectangle_fitting_main.rst b/docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst
similarity index 96%
rename from docs/modules/mapping/rectangle_fitting/rectangle_fitting_main.rst
rename to docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst
index 50a50141b2..06d85efe61 100644
--- a/docs/modules/mapping/rectangle_fitting/rectangle_fitting_main.rst
+++ b/docs/modules/3_mapping/rectangle_fitting/rectangle_fitting_main.rst
@@ -7,7 +7,7 @@ This is an object shape recognition using rectangle fitting.
This example code is based on this paper algorithm:
-- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University `_
+- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University `_
The algorithm consists of 2 steps as below.
@@ -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:
@@ -66,4 +66,4 @@ API
References
~~~~~~~~~~
-- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University `_
+- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University `_
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png b/docs/modules/4_slam/FastSLAM1/FastSLAM1_12_0.png
similarity index 100%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_12_0.png
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png b/docs/modules/4_slam/FastSLAM1/FastSLAM1_12_1.png
similarity index 100%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_12_1.png
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png b/docs/modules/4_slam/FastSLAM1/FastSLAM1_1_0.png
similarity index 100%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_1_0.png
diff --git a/docs/modules/slam/FastSLAM1/FastSLAM1_main.rst b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
similarity index 99%
rename from docs/modules/slam/FastSLAM1/FastSLAM1_main.rst
rename to docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
index a2aa521216..b6bafa0982 100644
--- a/docs/modules/slam/FastSLAM1/FastSLAM1_main.rst
+++ b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
@@ -22,6 +22,11 @@ The red points are particles of FastSLAM.
Black points are landmarks, blue crosses are estimated landmark
positions by FastSLAM.
+Code Link
+~~~~~~~~~~~~
+
+.. autofunction:: SLAM.FastSLAM1.fast_slam1.fast_slam1
+
Introduction
~~~~~~~~~~~~
diff --git a/docs/modules/slam/FastSLAM2/FastSLAM2_main.rst b/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
similarity index 85%
rename from docs/modules/slam/FastSLAM2/FastSLAM2_main.rst
rename to docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
index 9e79b496a3..59ed3b9f75 100644
--- a/docs/modules/slam/FastSLAM2/FastSLAM2_main.rst
+++ b/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
@@ -7,6 +7,12 @@ The animation has the same meanings as one of FastSLAM 1.0.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif
+Code Link
+~~~~~~~~~~~
+
+.. autofunction:: SLAM.FastSLAM2.fast_slam2.fast_slam2
+
+
References
~~~~~~~~~~
diff --git a/docs/modules/slam/ekf_slam/ekf_slam_1_0.png b/docs/modules/4_slam/ekf_slam/ekf_slam_1_0.png
similarity index 100%
rename from docs/modules/slam/ekf_slam/ekf_slam_1_0.png
rename to docs/modules/4_slam/ekf_slam/ekf_slam_1_0.png
diff --git a/docs/modules/slam/ekf_slam/ekf_slam_main.rst b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
similarity index 95%
rename from docs/modules/slam/ekf_slam/ekf_slam_main.rst
rename to docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
index 70a7d131ae..3967a7ae4d 100644
--- a/docs/modules/slam/ekf_slam/ekf_slam_main.rst
+++ b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst
@@ -21,6 +21,12 @@ This is a simulation of EKF SLAM.
- Blue line: ground truth
- Red line: EKF SLAM position estimation
+Code Link
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. autofunction:: SLAM.EKFSLAM.ekf_slam.ekf_slam
+
+
Introduction
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -63,27 +69,27 @@ Take care to note the difference between :math:`X` (state) and :math:`x`
original author: Atsushi Sakai (@Atsushi_twi)
notebook author: Andrew Tu (drewtu2)
"""
-
+
import math
import numpy as np
%matplotlib notebook
import matplotlib.pyplot as plt
-
-
+
+
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Change in covariance
-
+
# Simulation parameter
Qsim = np.diag([0.2, np.deg2rad(1.0)])**2 # Sensor Noise
Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 # Process Noise
-
+
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3 # State size [x,y,yaw]
LM_SIZE = 2 # LM state size [x,y]
-
+
show_animation = True
Algorithm Walk through
@@ -97,23 +103,22 @@ the estimated state and measurements
def ekf_slam(xEst, PEst, u, z):
"""
- Performs an iteration of EKF SLAM from the available information.
-
+ Performs an iteration of EKF SLAM from the available information.
+
:param xEst: the belief in last position
:param PEst: the uncertainty in last position
- :param u: the control function applied to the last position
+ :param u: the control function applied to the last position
:param z: measurements at this step
:returns: the next estimated position and associated covariance
"""
- S = STATE_SIZE
-
+
# Predict
- xEst, PEst, G, Fx = predict(xEst, PEst, u)
+ xEst, PEst = predict(xEst, PEst, u)
initP = np.eye(2)
-
+
# Update
- xEst, PEst = update(xEst, PEst, u, z, initP)
-
+ xEst, PEst = update(xEst, PEst, z, initP)
+
return xEst, PEst
@@ -170,26 +175,25 @@ the landmarks.
def predict(xEst, PEst, u):
"""
Performs the prediction step of EKF SLAM
-
+
:param xEst: nx1 state vector
:param PEst: nxn covariance matrix
:param u: 2x1 control vector
:returns: predicted state vector, predicted covariance, jacobian of control vector, transition fx
"""
- S = STATE_SIZE
- G, Fx = jacob_motion(xEst[0:S], u)
- xEst[0:S] = motion_model(xEst[0:S], u)
+ G, Fx = jacob_motion(xEst, u)
+ xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
# Fx is an an identity matrix of size (STATE_SIZE)
# sigma = G*sigma*G.T + Noise
- PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
- return xEst, PEst, G, Fx
+ PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
+ return xEst, PEst
.. code:: ipython3
def motion_model(x, u):
"""
Computes the motion model based on current state and input function.
-
+
:param x: 3x1 pose estimation
:param u: 2x1 control input [v; w]
:returns: the resulting state after the control function is applied
@@ -197,11 +201,11 @@ the landmarks.
F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
-
+
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])
-
+
x = (F @ x) + (B @ u)
return x
@@ -261,22 +265,21 @@ the changing uncertainty.
.. code:: ipython3
- def update(xEst, PEst, u, z, initP):
+ def update(xEst, PEst, z, initP):
"""
Performs the update step of EKF SLAM
-
+
:param xEst: nx1 the predicted pose of the system and the pose of the landmarks
:param PEst: nxn the predicted covariance
- :param u: 2x1 the control function
:param z: the measurements read at new position
:param initP: 2x2 an identity matrix acting as the initial covariance
:returns: the updated state and covariance for the system
"""
for iz in range(len(z[:, 0])): # for each observation
minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) # associate to a known landmark
-
+
nLM = calc_n_LM(xEst) # number of landmarks we currently know about
-
+
if minid == nLM: # Landmark is a NEW landmark
print("New LM")
# Extend state and covariance matrix
@@ -285,14 +288,14 @@ the changing uncertainty.
np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
xEst = xAug
PEst = PAug
-
+
lm = get_LM_Pos_from_state(xEst, minid)
y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
-
+
K = (PEst @ H.T) @ np.linalg.inv(S) # Calculate Kalman Gain
xEst = xEst + (K @ y)
PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst
-
+
xEst[2] = pi_2_pi(xEst[2])
return xEst, PEst
@@ -302,7 +305,7 @@ the changing uncertainty.
def calc_innovation(lm, xEst, PEst, z, LMid):
"""
Calculates the innovation based on expected position and landmark position
-
+
:param lm: landmark position
:param xEst: estimated position/state
:param PEst: estimated covariance
@@ -315,19 +318,19 @@ the changing uncertainty.
zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
# zp is the expected measurement based on xEst and the expected landmark position
-
+
y = (z - zp).T # y = innovation
y[1] = pi_2_pi(y[1])
-
+
H = jacobH(q, delta, xEst, LMid + 1)
S = H @ PEst @ H.T + Cx[0:2, 0:2]
-
+
return y, S, H
-
+
def jacobH(q, delta, x, i):
"""
Calculates the jacobian of the measurement function
-
+
:param q: the range from the system pose to the landmark
:param delta: the difference between a landmark position and the estimated system position
:param x: the state, including the estimated system position
@@ -337,17 +340,17 @@ the changing uncertainty.
sq = math.sqrt(q)
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
[delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])
-
+
G = G / q
nLM = calc_n_LM(x)
F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))
F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),
np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))
-
+
F = np.vstack((F1, F2))
-
+
H = G @ F
-
+
return H
Observation Step
@@ -370,17 +373,17 @@ reckoning and control functions are passed along here as well.
:param xd: the current noisy estimate of the system
:param u: the current control input
:param RFID: the true position of the landmarks
-
- :returns: Computes the true position, observations, dead reckoning (noisy) position,
+
+ :returns: Computes the true position, observations, dead reckoning (noisy) position,
and noisy control function
"""
xTrue = motion_model(xTrue, u)
-
+
# add noise to gps x-y
z = np.zeros((0, 3))
-
+
for i in range(len(RFID[:, 0])): # Test all beacons, only add the ones we can see (within MAX_RANGE)
-
+
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
@@ -390,12 +393,12 @@ reckoning and control functions are passed along here as well.
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise
zi = np.array([dn, anglen, i])
z = np.vstack((z, zi))
-
+
# add noise to input
ud = np.array([[
u[0, 0] + np.random.randn() * Rsim[0, 0],
u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T
-
+
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
@@ -409,32 +412,30 @@ reckoning and control functions are passed along here as well.
"""
n = int((len(x) - STATE_SIZE) / LM_SIZE)
return n
-
-
+
+
def jacob_motion(x, u):
"""
- Calculates the jacobian of motion model.
-
+ Calculates the jacobian of motion model.
+
:param x: The state, including the estimated position of the system
:param u: The control function
:returns: G: Jacobian
Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix
"""
-
+
# [eye(3) [0 x y; 0 x y; 0 x y]]
Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
(STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
-
+
jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]],dtype=object)
-
+
G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
if calc_n_LM(x) > 0:
print(Fx.shape)
return G, Fx,
-
-
.. code:: ipython3
@@ -442,68 +443,68 @@ reckoning and control functions are passed along here as well.
def calc_LM_Pos(x, z):
"""
Calculates the pose in the world coordinate frame of a landmark at the given measurement.
-
+
:param x: [x; y; theta]
:param z: [range; bearing]
:returns: [x; y] for given measurement
"""
zp = np.zeros((2, 1))
-
+
zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
#zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
#zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
-
+
return zp
-
-
+
+
def get_LM_Pos_from_state(x, ind):
"""
Returns the position of a given landmark
-
+
:param x: The state containing all landmark positions
:param ind: landmark id
:returns: The position of the landmark
"""
lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]
-
+
return lm
-
-
+
+
def search_correspond_LM_ID(xAug, PAug, zi):
"""
Landmark association with Mahalanobis distance.
-
- If this landmark is at least M_DIST_TH units away from all known landmarks,
+
+ If this landmark is at least M_DIST_TH units away from all known landmarks,
it is a NEW landmark.
-
+
:param xAug: The estimated state
:param PAug: The estimated covariance
:param zi: the read measurements of specific landmark
:returns: landmark id
"""
-
+
nLM = calc_n_LM(xAug)
-
+
mdist = []
-
+
for i in range(nLM):
lm = get_LM_Pos_from_state(xAug, i)
y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
mdist.append(y.T @ np.linalg.inv(S) @ y)
-
+
mdist.append(M_DIST_TH) # new landmark
-
+
minid = mdist.index(min(mdist))
-
+
return minid
-
+
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
return u
-
+
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
@@ -511,53 +512,53 @@ reckoning and control functions are passed along here as well.
def main():
print(" start!!")
-
+
time = 0.0
-
+
# RFID positions [x, y]
RFID = np.array([[10.0, -2.0],
[15.0, 10.0],
[3.0, 15.0],
[-5.0, 20.0]])
-
+
# State Vector [x y yaw v]'
xEst = np.zeros((STATE_SIZE, 1))
xTrue = np.zeros((STATE_SIZE, 1))
PEst = np.eye(STATE_SIZE)
-
+
xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
-
+
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
-
+
while SIM_TIME >= time:
time += DT
u = calc_input()
-
+
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
-
+
xEst, PEst = ekf_slam(xEst, PEst, ud, z)
-
+
x_state = xEst[0:STATE_SIZE]
-
+
# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
-
+
if show_animation: # pragma: no cover
plt.cla()
-
+
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(xEst[0], xEst[1], ".r")
-
+
# plot landmark
for i in range(calc_n_LM(xEst)):
plt.plot(xEst[STATE_SIZE + i * 2],
xEst[STATE_SIZE + i * 2 + 1], "xg")
-
+
plt.plot(hxTrue[0, :],
hxTrue[1, :], "-b")
plt.plot(hxDR[0, :],
@@ -583,9 +584,7 @@ reckoning and control functions are passed along here as well.
.. image:: ekf_slam_1_0.png
-References:
+Reference
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- `PROBABILISTIC ROBOTICS `_
-
-
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst
similarity index 97%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst
index 491320512b..15963aff79 100644
--- a/docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst
+++ b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst
@@ -165,7 +165,7 @@ different data sources into a single optimization problem.
6 215.8405 -0.000000
-.. figure:: graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/Graph_SLAM_optimization.gif
.. code:: ipython3
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc.rst b/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc.rst
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc.rst
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png b/docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png
rename to docs/modules/4_slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png
diff --git a/docs/modules/slam/graph_slam/graphSLAM_formulation.rst b/docs/modules/4_slam/graph_slam/graphSLAM_formulation.rst
similarity index 100%
rename from docs/modules/slam/graph_slam/graphSLAM_formulation.rst
rename to docs/modules/4_slam/graph_slam/graphSLAM_formulation.rst
diff --git a/docs/modules/slam/graph_slam/graph_slam_main.rst b/docs/modules/4_slam/graph_slam/graph_slam_main.rst
similarity index 84%
rename from docs/modules/slam/graph_slam/graph_slam_main.rst
rename to docs/modules/4_slam/graph_slam/graph_slam_main.rst
index 987eed385c..2ef17e4179 100644
--- a/docs/modules/slam/graph_slam/graph_slam_main.rst
+++ b/docs/modules/4_slam/graph_slam/graph_slam_main.rst
@@ -13,11 +13,17 @@ The black stars are landmarks for graph edge generation.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/animation.gif
+Code Link
+~~~~~~~~~~~~
+
+.. autofunction:: SLAM.GraphBasedSLAM.graph_based_slam.graph_based_slam
+
+
.. include:: graphSLAM_doc.rst
.. include:: graphSLAM_formulation.rst
.. include:: graphSLAM_SE2_example.rst
-References:
+Reference
~~~~~~~~~~~
- `A Tutorial on Graph-Based SLAM `_
diff --git a/docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst b/docs/modules/4_slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
similarity index 78%
rename from docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
rename to docs/modules/4_slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
index a30b1fc99b..772fe62889 100644
--- a/docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
+++ b/docs/modules/4_slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst
@@ -10,7 +10,13 @@ points to points.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif
+Code Link
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. autofunction:: SLAM.ICPMatching.icp_matching.icp_matching
+
+
References
-~~~~~~~~~~
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- `Introduction to Mobile Robotics: Iterative Closest Point Algorithm `_
diff --git a/docs/modules/4_slam/slam_main.rst b/docs/modules/4_slam/slam_main.rst
new file mode 100644
index 0000000000..98211986c2
--- /dev/null
+++ b/docs/modules/4_slam/slam_main.rst
@@ -0,0 +1,18 @@
+.. _`SLAM`:
+
+SLAM
+====
+
+Simultaneous Localization and Mapping(SLAM) examples
+Simultaneous Localization and Mapping (SLAM) is an ability to estimate the pose of a robot and the map of the environment at the same time. The SLAM problem is hard to
+solve, because a map is needed for localization and localization is needed for mapping. In this way, SLAM is often said to be similar to a ‘chicken-and-egg’ problem. Popular SLAM solution methods include the extended Kalman filter, particle filter, and Fast SLAM algorithm[31]. Fig.4 shows SLAM simulation results using extended Kalman filter and results using FastSLAM2.0[31].
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ iterative_closest_point_matching/iterative_closest_point_matching
+ ekf_slam/ekf_slam
+ FastSLAM1/FastSLAM1
+ FastSLAM2/FastSLAM2
+ graph_slam/graph_slam
diff --git a/docs/modules/path_planning/bezier_path/Figure_1.png b/docs/modules/5_path_planning/bezier_path/Figure_1.png
similarity index 100%
rename from docs/modules/path_planning/bezier_path/Figure_1.png
rename to docs/modules/5_path_planning/bezier_path/Figure_1.png
diff --git a/docs/modules/path_planning/bezier_path/Figure_2.png b/docs/modules/5_path_planning/bezier_path/Figure_2.png
similarity index 100%
rename from docs/modules/path_planning/bezier_path/Figure_2.png
rename to docs/modules/5_path_planning/bezier_path/Figure_2.png
diff --git a/docs/modules/path_planning/bezier_path/bezier_path_main.rst b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
similarity index 58%
rename from docs/modules/path_planning/bezier_path/bezier_path_main.rst
rename to docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
index fbba6a4496..19fb89a1b1 100644
--- a/docs/modules/path_planning/bezier_path/bezier_path_main.rst
+++ b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst
@@ -13,8 +13,15 @@ You can get different Beizer course:
.. image:: Figure_2.png
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.BezierPath.bezier_path.calc_4points_bezier_path
+
+
+Reference
+~~~~~~~~~~~~~~~
- `Continuous Curvature Path Generation Based on Bezier Curves for
Autonomous
- Vehicles `__
+ Vehicles `__
diff --git a/docs/modules/path_planning/bspline_path/approx_and_curvature.png b/docs/modules/5_path_planning/bspline_path/approx_and_curvature.png
similarity index 100%
rename from docs/modules/path_planning/bspline_path/approx_and_curvature.png
rename to docs/modules/5_path_planning/bspline_path/approx_and_curvature.png
diff --git a/docs/modules/path_planning/bspline_path/approximation1.png b/docs/modules/5_path_planning/bspline_path/approximation1.png
similarity index 100%
rename from docs/modules/path_planning/bspline_path/approximation1.png
rename to docs/modules/5_path_planning/bspline_path/approximation1.png
diff --git a/docs/modules/path_planning/bspline_path/basis_functions.png b/docs/modules/5_path_planning/bspline_path/basis_functions.png
similarity index 100%
rename from docs/modules/path_planning/bspline_path/basis_functions.png
rename to docs/modules/5_path_planning/bspline_path/basis_functions.png
diff --git a/docs/modules/path_planning/bspline_path/bspline_path_main.rst b/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst
similarity index 99%
rename from docs/modules/path_planning/bspline_path/bspline_path_main.rst
rename to docs/modules/5_path_planning/bspline_path/bspline_path_main.rst
index e734352e34..00e5ef2fdb 100644
--- a/docs/modules/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/path_planning/bspline_path/bspline_path_planning.png b/docs/modules/5_path_planning/bspline_path/bspline_path_planning.png
similarity index 100%
rename from docs/modules/path_planning/bspline_path/bspline_path_planning.png
rename to docs/modules/5_path_planning/bspline_path/bspline_path_planning.png
diff --git a/docs/modules/path_planning/bspline_path/interp_and_curvature.png b/docs/modules/5_path_planning/bspline_path/interp_and_curvature.png
similarity index 100%
rename from docs/modules/path_planning/bspline_path/interp_and_curvature.png
rename to docs/modules/5_path_planning/bspline_path/interp_and_curvature.png
diff --git a/docs/modules/path_planning/bspline_path/interpolation1.png b/docs/modules/5_path_planning/bspline_path/interpolation1.png
similarity index 100%
rename from docs/modules/path_planning/bspline_path/interpolation1.png
rename to docs/modules/5_path_planning/bspline_path/interpolation1.png
diff --git a/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst
new file mode 100644
index 0000000000..e1cd2fe353
--- /dev/null
+++ b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst
@@ -0,0 +1,16 @@
+Bug planner
+-----------
+
+This is a 2D planning with Bug algorithm.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BugPlanner/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.BugPlanning.bug.main
+
+Reference
+~~~~~~~~~~~~
+
+- `ECE452 Bug Algorithms `_
diff --git a/docs/modules/5_path_planning/catmull_rom_spline/blending_functions.png b/docs/modules/5_path_planning/catmull_rom_spline/blending_functions.png
new file mode 100644
index 0000000000..928946df46
Binary files /dev/null and b/docs/modules/5_path_planning/catmull_rom_spline/blending_functions.png differ
diff --git a/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_path_planning.png b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_path_planning.png
new file mode 100644
index 0000000000..1d0ff001e6
Binary files /dev/null and b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_path_planning.png differ
diff --git a/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst
new file mode 100644
index 0000000000..fa2a2ff72b
--- /dev/null
+++ b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst
@@ -0,0 +1,103 @@
+Catmull-Rom Spline Planning
+----------------------------
+
+.. image:: catmull_rom_path_planning.png
+
+This is a Catmull-Rom spline path planning routine.
+
+If you provide waypoints, the Catmull-Rom spline generates a smooth path that always passes through the control points,
+exhibits local control, and maintains 𝐶1 continuity.
+
+
+Catmull-Rom Spline Fundamentals
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Catmull-Rom splines are a type of cubic spline that passes through a given set of points, known as control points.
+
+They are defined by the following equation for calculating a point on the spline:
+
+:math:`P(t) = 0.5 \times \left( 2P_1 + (-P_0 + P_2)t + (2P_0 - 5P_1 + 4P_2 - P_3)t^2 + (-P_0 + 3P_1 - 3P_2 + P_3)t^3 \right)`
+
+Where:
+
+* :math:`P(t)` is the point on the spline at parameter :math:`t`.
+* :math:`P_0, P_1, P_2, P_3` are the control points surrounding the parameter :math:`t`.
+
+Types of Catmull-Rom Splines
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+There are different types of Catmull-Rom splines based on the choice of the **tau** parameter, which influences how the curve
+behaves in relation to the control points:
+
+1. **Uniform Catmull-Rom Spline**:
+ The standard implementation where the parameterization is uniform. Each segment of the spline is treated equally,
+ regardless of the distances between control points.
+
+
+2. **Chordal Catmull-Rom Spline**:
+ This spline type takes into account the distance between control points. The parameterization is based on the actual distance
+ along the spline, ensuring smoother transitions. The equation can be modified to include the chord length :math:`L_i` between
+ points :math:`P_i` and :math:`P_{i+1}`:
+
+ .. math::
+ \tau_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
+
+3. **Centripetal Catmull-Rom Spline**:
+ This variation improves upon the chordal spline by using the square root of the distance to determine the parameterization,
+ which avoids oscillations and creates a more natural curve. The parameter :math:`t_i` is adjusted using the following relation:
+
+ .. math::
+ t_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
+
+
+Blending Functions
+~~~~~~~~~~~~~~~~~~~~~
+
+In Catmull-Rom spline interpolation, blending functions are used to calculate the influence of each control point on the spline at a
+given parameter :math:`t`. The blending functions ensure that the spline is smooth and passes through the control points while
+maintaining continuity. The four blending functions used in Catmull-Rom splines are defined as follows:
+
+1. **Blending Function 1**:
+
+ .. math::
+ b_1(t) = -t + 2t^2 - t^3
+
+2. **Blending Function 2**:
+
+ .. math::
+ b_2(t) = 2 - 5t^2 + 3t^3
+
+3. **Blending Function 3**:
+
+ .. math::
+ b_3(t) = t + 4t^2 - 3t^3
+
+4. **Blending Function 4**:
+
+ .. math::
+ b_4(t) = -t^2 + t^3
+
+The blending functions are combined in the spline equation to create a smooth curve that reflects the influence of each control point.
+
+The following figure illustrates the blending functions over the interval :math:`[0, 1]`:
+
+.. image:: blending_functions.png
+
+Catmull-Rom Spline API
+~~~~~~~~~~~~~~~~~~~~~~~
+
+This section provides an overview of the functions used for Catmull-Rom spline path planning.
+
+Code Link
+++++++++++
+
+.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_point
+
+.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_spline
+
+
+References
+~~~~~~~~~~~~~~
+
+- `Catmull-Rom Spline - Wikipedia `__
+- `Catmull-Rom Splines `__
\ No newline at end of file
diff --git a/docs/modules/path_planning/clothoid_path/clothoid_path_main.rst b/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst
similarity index 95%
rename from docs/modules/path_planning/clothoid_path/clothoid_path_main.rst
rename to docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst
index 1e8cee694a..16d0ec03c1 100644
--- a/docs/modules/path_planning/clothoid_path/clothoid_path_main.rst
+++ b/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst
@@ -73,6 +73,11 @@ The final clothoid path can be calculated with the path parameters and Fresnel i
&y(s)=y_{0}+\int_{0}^{s} \sin \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau
\end{aligned}
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.ClothoidPath.clothoid_path_planner.generate_clothoid_path
+
References
~~~~~~~~~~
diff --git a/docs/modules/path_planning/coverage_path/coverage_path_main.rst b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst
similarity index 73%
rename from docs/modules/path_planning/coverage_path/coverage_path_main.rst
rename to docs/modules/5_path_planning/coverage_path/coverage_path_main.rst
index 828342a294..eaa876c80b 100644
--- a/docs/modules/path_planning/coverage_path/coverage_path_main.rst
+++ b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst
@@ -8,6 +8,11 @@ This is a 2D grid based sweep coverage path planner simulation:
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/GridBasedSweepCPP/animation.gif
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.GridBasedSweepCPP.grid_based_sweep_coverage_path_planner.planning
+
Spiral Spanning Tree
~~~~~~~~~~~~~~~~~~~~
@@ -17,6 +22,14 @@ This is a 2D grid based spiral spanning tree coverage path planner simulation:
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation2.gif
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation3.gif
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.SpiralSpanningTreeCPP.spiral_spanning_tree_coverage_path_planner.main
+
+Reference
++++++++++++++
+
- `Spiral-STC: An On-Line Coverage Algorithm of Grid Environments by a Mobile Robot `_
@@ -29,6 +42,14 @@ This is a 2D grid based wavefront coverage path planner simulation:
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation2.gif
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation3.gif
-- `Planning paths of complete coverage of an unstructured environment by a mobile robot `_
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.WavefrontCPP.wavefront_coverage_path_planner.wavefront
+
+Reference
++++++++++++++
+
+- `Planning paths of complete coverage of an unstructured environment by a mobile robot `_
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_1d.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_1d.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_1d.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_1d.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_2d_curvature.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_curvature.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_2d_curvature.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_curvature.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_2d_path.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_path.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_2d_path.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_path.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_2d_yaw.png b/docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_yaw.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_2d_yaw.png
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_2d_yaw.png
diff --git a/docs/modules/path_planning/cubic_spline/cubic_spline_main.rst b/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst
similarity index 99%
rename from docs/modules/path_planning/cubic_spline/cubic_spline_main.rst
rename to docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst
index 33471f7c85..a110217a2e 100644
--- a/docs/modules/path_planning/cubic_spline/cubic_spline_main.rst
+++ b/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst
@@ -171,8 +171,8 @@ the second derivative by:
These equations can be calculated by differentiating the cubic polynomial.
-API
-===
+Code Link
+==========
This is the 1D cubic spline class API:
@@ -199,8 +199,8 @@ Curvature of each point can be also calculated analytically by:
:math:`\kappa=\frac{y^{\prime \prime} x^{\prime}-x^{\prime \prime} y^{\prime}}{\left(x^{\prime2}+y^{\prime2}\right)^{\frac{2}{3}}}`
-API
-===
+Code Link
+==========
.. autoclass:: PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline2D
:members:
diff --git a/docs/modules/path_planning/cubic_spline/spline.png b/docs/modules/5_path_planning/cubic_spline/spline.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/spline.png
rename to docs/modules/5_path_planning/cubic_spline/spline.png
diff --git a/docs/modules/path_planning/cubic_spline/spline_continuity.png b/docs/modules/5_path_planning/cubic_spline/spline_continuity.png
similarity index 100%
rename from docs/modules/path_planning/cubic_spline/spline_continuity.png
rename to docs/modules/5_path_planning/cubic_spline/spline_continuity.png
diff --git a/docs/modules/path_planning/dubins_path/RLR.jpg b/docs/modules/5_path_planning/dubins_path/RLR.jpg
similarity index 100%
rename from docs/modules/path_planning/dubins_path/RLR.jpg
rename to docs/modules/5_path_planning/dubins_path/RLR.jpg
diff --git a/docs/modules/path_planning/dubins_path/RSR.jpg b/docs/modules/5_path_planning/dubins_path/RSR.jpg
similarity index 100%
rename from docs/modules/path_planning/dubins_path/RSR.jpg
rename to docs/modules/5_path_planning/dubins_path/RSR.jpg
diff --git a/docs/modules/path_planning/dubins_path/dubins_path.jpg b/docs/modules/5_path_planning/dubins_path/dubins_path.jpg
similarity index 100%
rename from docs/modules/path_planning/dubins_path/dubins_path.jpg
rename to docs/modules/5_path_planning/dubins_path/dubins_path.jpg
diff --git a/docs/modules/path_planning/dubins_path/dubins_path_main.rst b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst
similarity index 97%
rename from docs/modules/path_planning/dubins_path/dubins_path_main.rst
rename to docs/modules/5_path_planning/dubins_path/dubins_path_main.rst
index 516638d83d..5a3d14464b 100644
--- a/docs/modules/path_planning/dubins_path/dubins_path_main.rst
+++ b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst
@@ -62,7 +62,7 @@ You can generate a path from these information and the maximum curvature informa
A path type which has minimum course length among 6 types is selected,
and then a path is constructed based on the selected type and its distances.
-API
+Code Link
~~~~~~~~~~~~~~~~~~~~
.. autofunction:: PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path
@@ -72,5 +72,5 @@ Reference
~~~~~~~~~~~~~~~~~~~~
- `On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents `__
- `Dubins path - Wikipedia `__
-- `15.3.1 Dubins Curves `__
+- `15.3.1 Dubins Curves `__
- `A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths `__
diff --git a/docs/modules/path_planning/dynamic_window_approach/dynamic_window_approach_main.rst b/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
similarity index 74%
rename from docs/modules/path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
rename to docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
index d645838597..ac5322df94 100644
--- a/docs/modules/path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
+++ b/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst
@@ -5,7 +5,17 @@ Dynamic Window Approach
This is a 2D navigation sample code with Dynamic Window Approach.
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif
+
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.DynamicWindowApproach.dynamic_window_approach.dwa_control
+
+
+Reference
+~~~~~~~~~~~~
+
- `The Dynamic Window Approach to Collision
Avoidance `__
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif
diff --git a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
new file mode 100644
index 0000000000..d0109d4ec3
--- /dev/null
+++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst
@@ -0,0 +1,79 @@
+Elastic Bands
+-------------
+
+This is a path planning with Elastic Bands.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ElasticBands/animation.gif
+
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.ElasticBands.elastic_bands.ElasticBands
+
+
+Core Concept
+~~~~~~~~~~~~
+- **Elastic Band**: A dynamically deformable collision-free path initialized by a global planner.
+- **Objective**:
+
+ * Shorten and smooth the path.
+ * Maximize obstacle clearance.
+ * Maintain global path connectivity.
+
+Bubble Representation
+~~~~~~~~~~~~~~~~~~~~~~~~
+- **Definition**: A local free-space region around configuration :math:`b`:
+
+ .. math::
+ B(b) = \{ q: \|q - b\| < \rho(b) \},
+
+ where :math:`\rho(b)` is the radius of the bubble.
+
+
+Force-Based Deformation
+~~~~~~~~~~~~~~~~~~~~~~~
+The elastic band deforms under artificial forces:
+
+Internal Contraction Force
+++++++++++++++++++++++++++
+- **Purpose**: Reduces path slack and length.
+- **Formula**: For node :math:`b_i`:
+
+ .. math::
+ f_c(b_i) = k_c \left( \frac{b_{i-1} - b_i}{\|b_{i-1} - b_i\|} + \frac{b_{i+1} - b_i}{\|b_{i+1} - b_i\|} \right)
+
+ where :math:`k_c` is the contraction gain.
+
+External Repulsion Force
++++++++++++++++++++++++++
+- **Purpose**: Pushes the path away from obstacles.
+- **Formula**: For node :math:`b_i`:
+
+ .. math::
+ f_r(b_i) = \begin{cases}
+ k_r (\rho_0 - \rho(b_i)) \nabla \rho(b_i) & \text{if } \rho(b_i) < \rho_0, \\
+ 0 & \text{otherwise}.
+ \end{cases}
+
+ where :math:`k_r` is the repulsion gain, :math:`\rho_0` is the maximum distance for applying repulsion force, and :math:`\nabla \rho(b_i)` is approximated via finite differences:
+
+ .. math::
+ \frac{\partial \rho}{\partial x} \approx \frac{\rho(b_i + h) - \rho(b_i - h)}{2h}.
+
+Dynamic Path Maintenance
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+1. **Node Update**:
+
+ .. math::
+ b_i^{\text{new}} = b_i^{\text{old}} + \alpha (f_c + f_r),
+
+ where :math:`\alpha` is a step-size parameter, which often proportional to :math:`\rho(b_i^{\text{old}})`
+
+2. **Overlap Enforcement**:
+- Insert new nodes if adjacent nodes are too far apart
+- Remove redundant nodes if adjacent nodes are too close
+
+References
++++++++++++++
+
+- `Elastic Bands: Connecting Path Planning and Control `__
diff --git a/docs/modules/path_planning/eta3_spline/eta3_spline_main.rst b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst
similarity index 71%
rename from docs/modules/path_planning/eta3_spline/eta3_spline_main.rst
rename to docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst
index ffc3cc6451..9ee343e8a7 100644
--- a/docs/modules/path_planning/eta3_spline/eta3_spline_main.rst
+++ b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst
@@ -7,7 +7,14 @@ Eta^3 Spline path planning
This is a path planning with Eta^3 spline.
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autoclass:: PathPlanning.Eta3SplineTrajectory.eta3_spline_trajectory.Eta3SplineTrajectory
+
+
+Reference
+~~~~~~~~~~~~~~~
- `\\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile
Robots `__
diff --git a/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst
new file mode 100644
index 0000000000..0f84d381ea
--- /dev/null
+++ b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst
@@ -0,0 +1,52 @@
+Optimal Trajectory in a Frenet Frame
+------------------------------------
+
+This is optimal trajectory generation in a Frenet Frame.
+
+The cyan line is the target course and black crosses are obstacles.
+
+The red line is predicted path.
+
+Code Link
+~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.FrenetOptimalTrajectory.frenet_optimal_trajectory.main
+
+
+High Speed and Velocity Keeping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/high_speed_and_velocity_keeping_frenet_path.gif
+
+This scenario shows how the trajectory is maintained at high speeds while keeping a consistent velocity.
+
+High Speed and Merging and Stopping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/high_speed_and_merging_and_stopping_frenet_path.gif
+
+This scenario demonstrates the trajectory planning at high speeds with merging and stopping behaviors.
+
+Low Speed and Velocity Keeping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/low_speed_and_velocity_keeping_frenet_path.gif
+
+This scenario demonstrates how the trajectory is managed at low speeds while maintaining a steady velocity.
+
+Low Speed and Merging and Stopping Scenario
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/low_speed_and_merging_and_stopping_frenet_path.gif
+
+This scenario illustrates the trajectory planning at low speeds with merging and stopping behaviors.
+
+Reference
+
+- `Optimal Trajectory Generation for Dynamic Street Scenarios in a
+ Frenet
+ Frame `__
+
+- `Optimal trajectory generation for dynamic street scenarios in a
+ Frenet Frame `__
+
diff --git a/docs/modules/path_planning/grid_base_search/grid_base_search_main.rst b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst
similarity index 75%
rename from docs/modules/path_planning/grid_base_search/grid_base_search_main.rst
rename to docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst
index 1644ed00cc..c4aa6882aa 100644
--- a/docs/modules/path_planning/grid_base_search/grid_base_search_main.rst
+++ b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst
@@ -10,6 +10,12 @@ This is a 2D grid based path planning with Breadth first search algorithm.
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.BreadthFirstSearch.breadth_first_search.BreadthFirstSearchPlanner
+
+
Depth First Search
~~~~~~~~~~~~~~~~~~~~
@@ -19,6 +25,12 @@ This is a 2D grid based path planning with Depth first search algorithm.
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.DepthFirstSearch.depth_first_search.DepthFirstSearchPlanner
+
+
.. _dijkstra:
Dijkstra algorithm
@@ -30,6 +42,12 @@ This is a 2D grid based shortest path planning with Dijkstra's algorithm.
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.Dijkstra.dijkstra.DijkstraPlanner
+
+
.. _a*-algorithm:
A\* algorithm
@@ -43,6 +61,12 @@ In the animation, cyan points are searched nodes.
Its heuristic is 2D Euclid distance.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.AStar.a_star.AStarPlanner
+
+
Bidirectional A\* algorithm
~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -52,6 +76,12 @@ This is a 2D grid based shortest path planning with bidirectional A star algorit
In the animation, cyan points are searched nodes.
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.BidirectionalAStar.bidirectional_a_star.BidirectionalAStarPlanner
+
+
.. _D*-algorithm:
D\* algorithm
@@ -63,7 +93,14 @@ This is a 2D grid based shortest path planning with D star algorithm.
The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm.
-Ref:
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.DStar.dstar.Dstar
+
+
+Reference
+++++++++++++
- `D* search Wikipedia `__
@@ -74,7 +111,13 @@ This is a 2D grid based path planning and replanning with D star lite algorithm.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStarLite/animation.gif
-Ref:
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.DStarLite.d_star_lite.DStarLite
+
+Reference
+++++++++++++
- `Improved Fast Replanning for Robot Navigation in Unknown Terrain `_
@@ -88,7 +131,14 @@ This is a 2D grid based path planning with Potential Field algorithm.
In the animation, the blue heat map shows potential value on each grid.
-Ref:
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.PotentialFieldPlanning.potential_field_planning.potential_field_planning
+
+
+Reference
+++++++++++++
- `Robotic Motion Planning:Potential
Functions `__
diff --git a/docs/modules/path_planning/hybridastar/hybridastar_main.rst b/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst
similarity index 66%
rename from docs/modules/path_planning/hybridastar/hybridastar_main.rst
rename to docs/modules/5_path_planning/hybridastar/hybridastar_main.rst
index a9876fa3d4..36f340e0c2 100644
--- a/docs/modules/path_planning/hybridastar/hybridastar_main.rst
+++ b/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst
@@ -4,3 +4,8 @@ Hybrid a star
This is a simple vehicle model based hybrid A\* path planner.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/HybridAStar/animation.gif
+
+Code Link
++++++++++++++
+
+.. autofunction:: PathPlanning.HybridAStar.hybrid_a_star.hybrid_a_star_planning
diff --git a/docs/modules/path_planning/lqr_path/lqr_path_main.rst b/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst
similarity index 73%
rename from docs/modules/path_planning/lqr_path/lqr_path_main.rst
rename to docs/modules/5_path_planning/lqr_path/lqr_path_main.rst
index 788442ff63..1eb1e4d840 100644
--- a/docs/modules/path_planning/lqr_path/lqr_path_main.rst
+++ b/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst
@@ -4,3 +4,8 @@ LQR based path planning
A sample code using LQR based path planning for double integrator model.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true
+
+Code Link
++++++++++++++
+
+.. autoclass:: PathPlanning.LQRPlanner.lqr_planner.LQRPlanner
diff --git a/docs/modules/path_planning/model_predictive_trajectory_generator/lookup_table.png b/docs/modules/5_path_planning/model_predictive_trajectory_generator/lookup_table.png
similarity index 100%
rename from docs/modules/path_planning/model_predictive_trajectory_generator/lookup_table.png
rename to docs/modules/5_path_planning/model_predictive_trajectory_generator/lookup_table.png
diff --git a/docs/modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
similarity index 70%
rename from docs/modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
rename to docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
index 0fc997898e..76472a6792 100644
--- a/docs/modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
+++ b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst
@@ -6,6 +6,12 @@ generator.
This algorithm is used for state lattice planner.
+Code Link
+~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.ModelPredictiveTrajectoryGenerator.trajectory_generator.optimize_trajectory
+
+
Path optimization sample
~~~~~~~~~~~~~~~~~~~~~~~~
@@ -16,7 +22,8 @@ Lookup table generation sample
.. image:: lookup_table.png
-Ref:
+Reference
+~~~~~~~~~~~~
- `Optimal rough terrain trajectory generation for wheeled mobile
- robots `__
+ robots `__
diff --git a/docs/modules/path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst
similarity index 57%
rename from docs/modules/path_planning/path_planning_main.rst
rename to docs/modules/5_path_planning/path_planning_main.rst
index a3237f16f1..0c84a19c22 100644
--- a/docs/modules/path_planning/path_planning_main.rst
+++ b/docs/modules/5_path_planning/path_planning_main.rst
@@ -1,8 +1,10 @@
-.. _path_planning:
+.. _`Path Planning`:
Path Planning
=============
+Path planning is the ability of a robot to search feasible and efficient path to the goal. The path has to satisfy some constraints based on the robot’s motion model and obstacle positions, and optimize some objective functions such as time to goal and distance to obstacle. In path planning, dynamic programming based approaches and sampling based approaches are widely used[22]. Fig.5 shows simulation results of potential field path planning and LQRRRT* path planning[27].
+
.. toctree::
:maxdepth: 2
:caption: Contents
@@ -10,6 +12,7 @@ Path Planning
dynamic_window_approach/dynamic_window_approach
bugplanner/bugplanner
grid_base_search/grid_base_search
+ time_based_grid_search/time_based_grid_search
model_predictive_trajectory_generator/model_predictive_trajectory_generator
state_lattice_planner/state_lattice_planner
prm_planner/prm_planner
@@ -18,6 +21,7 @@ Path Planning
rrt/rrt
cubic_spline/cubic_spline
bspline_path/bspline_path
+ catmull_rom_spline/catmull_rom_spline
clothoid_path/clothoid_path
eta3_spline/eta3_spline
bezier_path/bezier_path
@@ -28,3 +32,4 @@ Path Planning
hybridastar/hybridastar
frenet_frame_path/frenet_frame_path
coverage_path/coverage_path
+ elastic_bands/elastic_bands
\ No newline at end of file
diff --git a/docs/modules/path_planning/prm_planner/prm_planner_main.rst b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
similarity index 79%
rename from docs/modules/path_planning/prm_planner/prm_planner_main.rst
rename to docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
index 0628719176..d58d1e2633 100644
--- a/docs/modules/path_planning/prm_planner/prm_planner_main.rst
+++ b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst
@@ -13,7 +13,14 @@ Cyan crosses means searched points with Dijkstra method,
The red line is the final path of PRM.
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.ProbabilisticRoadMap.probabilistic_road_map.prm_planning
+
+
+Reference
+~~~~~~~~~~~
- `Probabilistic roadmap -
Wikipedia `__
diff --git a/docs/modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
similarity index 94%
rename from docs/modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
rename to docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
index feec345bae..c7bc3fb55c 100644
--- a/docs/modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
+++ b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst
@@ -9,6 +9,11 @@ Motion planning with quintic polynomials.
It can calculate 2D path, velocity, and acceleration profile based on
quintic polynomials.
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathPlanning.QuinticPolynomialsPlanner.quintic_polynomials_planner.quintic_polynomials_planner
+
Quintic polynomials for one dimensional robot motion
@@ -97,10 +102,10 @@ Each velocity and acceleration boundary condition can be calculated with each or
:math:`v_{xe}=v_ecos(\theta_e), v_{ye}=v_esin(\theta_e)`
-References:
+Reference
~~~~~~~~~~~
- `Local Path Planning And Motion Control For Agv In
- Positioning `__
+ Positioning `__
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LR_L.png b/docs/modules/5_path_planning/reeds_shepp_path/LR_L.png
new file mode 100644
index 0000000000..1e64da57f2
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LR_L.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LR_LR.png b/docs/modules/5_path_planning/reeds_shepp_path/LR_LR.png
new file mode 100644
index 0000000000..e2c9883d8e
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LR_LR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSL.png b/docs/modules/5_path_planning/reeds_shepp_path/LSL.png
new file mode 100644
index 0000000000..6785ad3f8c
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSL.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSL90xR.png b/docs/modules/5_path_planning/reeds_shepp_path/LSL90xR.png
new file mode 100644
index 0000000000..54e892ba46
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSL90xR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSR.png b/docs/modules/5_path_planning/reeds_shepp_path/LSR.png
new file mode 100644
index 0000000000..8acc0de69f
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/LSR90_L.png b/docs/modules/5_path_planning/reeds_shepp_path/LSR90_L.png
new file mode 100644
index 0000000000..58d381010d
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/LSR90_L.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL.png
new file mode 100644
index 0000000000..c305c0be6e
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL90_R.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL90_R.png
new file mode 100644
index 0000000000..f2b38329da
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SL90_R.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R90SR.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SR.png
new file mode 100644
index 0000000000..4323a9fe3b
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R90SR.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_RL.png b/docs/modules/5_path_planning/reeds_shepp_path/L_RL.png
new file mode 100644
index 0000000000..ad58b8ffea
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_RL.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_RL_R.png b/docs/modules/5_path_planning/reeds_shepp_path/L_RL_R.png
new file mode 100644
index 0000000000..db4aaf6af3
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_RL_R.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/L_R_L.png b/docs/modules/5_path_planning/reeds_shepp_path/L_R_L.png
new file mode 100644
index 0000000000..0d3082aeaf
Binary files /dev/null and b/docs/modules/5_path_planning/reeds_shepp_path/L_R_L.png differ
diff --git a/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
new file mode 100644
index 0000000000..4dd54d7c97
--- /dev/null
+++ b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
@@ -0,0 +1,399 @@
+Reeds Shepp planning
+--------------------
+
+A sample code with Reeds Shepp path planning.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true
+
+Code Link
+==============
+
+.. autofunction:: PathPlanning.ReedsSheppPath.reeds_shepp_path_planning.reeds_shepp_path_planning
+
+
+Mathematical Description of Individual Path Types
+=================================================
+Here is an overview of mathematical derivations of formulae for individual path types.
+
+In all the derivations below, radius of curvature of the vehicle is assumed to be of unit length and start pose is considered to be at origin. (*In code we are removing the offset due to start position and normalising the lengths before passing the values to these functions.*)
+
+Also, (t, u, v) respresent the measure of each motion requried. Thus, in case of a turning maneuver, they represent the angle inscribed at the centre of turning circle and in case of straight maneuver, they represent the distance to be travelled.
+
+1. **Left-Straight-Left**
+
+.. image:: LSL.png
+
+We can deduce the following facts using geometry.
+
+- AGHC is a rectangle.
+- :math:`∠LAC = ∠BAG = t`
+- :math:`t + v = φ`
+- :math:`C(x - sin(φ), y + cos(φ))`
+- :math:`A(0, 1)`
+- :math:`u, t = polar(vector)`
+
+Hence, we have:
+
+- :math:`u, t = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`v = φ - t`
+
+
+2. **Left-Straight-Right**
+
+.. image:: LSR.png
+
+With followng notations:
+
+- :math:`∠MBD = t1`
+- :math:`∠BDF = θ`
+- :math:`BC = u1`
+
+We can deduce the following facts using geometry.
+
+- D is mid-point of BC and FG.
+- :math:`t - v = φ`
+- :math:`C(x + sin(φ), y - cos(φ))`
+- :math:`A(0, 1)`
+- :math:`u1, t1 = polar(vector)`
+- :math:`\frac{u1^2}{4} = 1 + \frac{u^2}{4}`
+- :math:`BF = 1` [Radius Of Curvature]
+- :math:`FD = \frac{u}{2}`
+- :math:`θ = arctan(\frac{BF}{FD})`
+- :math:`t1 + θ = t`
+
+Hence, we have:
+
+- :math:`u1, t1 = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = \sqrt{u1^2 - 4}`
+- :math:`θ = arctan(\frac{2}{u})`
+- :math:`t = t1 + θ`
+- :math:`v = t - φ`
+
+3. **LeftxRightxLeft**
+
+.. image:: L_R_L.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + u + v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`A = arccos(\frac{BD/2}{CD})`
+- :math:`u = (π - 2*A)`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD + ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`A = arccos(\frac{u1/2}{2})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`u = (π - 2*A)`
+- :math:`v = (φ - t - u)`
+
+4. **LeftxRight-Left**
+
+.. image:: L_RL.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + u - v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`A = arccos(\frac{BD/2}{CD})`
+- :math:`u = (π - 2*A)`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD + ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`A = arccos(\frac{u1/2}{2})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`u = (π - 2*A)`
+- :math:`v = (-φ + t + u)`
+
+5. **Left-RightxLeft**
+
+.. image:: LR_L.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - u - v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BC = CD = 2` [2 * radius of curvature]
+- :math:`cos(2π - u) = \frac{BC^2 + CD^2 - BD^2}{2 * BC * CD}` [Cosine Rule]
+- :math:`\frac{sin(A)}{BC} = \frac{sin(u)}{u1}` [Sine Rule]
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD - ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(1 - \frac{u1^2}{8})`
+- :math:`A = arcsin(\frac{sin(u)}{u1}*2)`
+- :math:`t = \frac{π}{2} + θ - A`
+- :math:`v = (t - u - φ)`
+
+6. **Left-RightxLeft-Right**
+
+.. image:: LR_LR.png
+
+With followng notations:
+
+- :math:`∠CLG = ∠BCL = ∠CBG = ∠LGB = A = u` [BGCL is an isoceles trapezium]
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - 2u + v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BC = CL = LG = 2` [2 * radius of curvature]
+- :math:`CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)` [Cosine rule in LGC]
+- :math:`CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)` [Cosine rule in LGC]
+- From the previous two equations: :math:`A = arccos(\frac{u1 + 2}{4})`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG + ∠GBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = arccos(\frac{u1 + 2}{4})`
+- :math:`t = \frac{π}{2} + θ + u`
+- :math:`v = (φ - t + 2u)`
+
+7. **LeftxRight-LeftxRight**
+
+.. image:: L_RL_R.png
+
+With followng notations:
+
+- :math:`∠GBC = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BC = CL = LG = 2` [2 * radius of curvature]
+- :math:`CD = 1` [radius of curvature]
+- D is midpoint of BG
+- :math:`BD = \frac{u1}{2}`
+- :math:`cos(u) = \frac{BC^2 + CD^2 - BD^2}{2*BC*CD}` [Cosine rule in BCD]
+- :math:`sin(A) = CD*\frac{sin(u)}{BD}` [Sine rule in BCD]
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG + ∠GBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = arccos(\frac{20 - u1^2}{16})`
+- :math:`A = arcsin(2*\frac{sin(u)}{u1})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ)`
+
+
+8. **LeftxRight90-Straight-Left**
+
+.. image:: L_R90SL.png
+
+With followng notations:
+
+- :math:`∠FBM = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBF = θ`
+- :math:`BF = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + \frac{π}{2} - v = φ`
+- :math:`F(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BM = CB = 2` [2 * radius of curvature]
+- :math:`MD = CD = 1` [CGDM is a rectangle]
+- :math:`MC = GD = u` [CGDM is a rectangle]
+- :math:`MF = MD + DF = 2`
+- :math:`BM = \sqrt{BF^2 - MF^2}` [Pythagoras theorem on BFM]
+- :math:`tan(A) = \frac{MF}{BM}`
+- :math:`u = MC = BM - CB`
+- :math:`t = ∠ABK + ∠KBF + ∠FBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(\sqrt{u1^2 - 4} - 2)`
+- :math:`A = arctan(\frac{2}{\sqrt{u1^2 - 4}})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ + \frac{π}{2})`
+
+
+9. **Left-Straight-Right90xLeft**
+
+.. image:: LSR90_L.png
+
+With followng notations:
+
+- :math:`∠MBH = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBH = θ`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - \frac{π}{2} - v = φ`
+- :math:`H(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`GH = 2` [2 * radius of curvature]
+- :math:`CM = DG = 1` [CGDM is a rectangle]
+- :math:`CD = MG = u` [CGDM is a rectangle]
+- :math:`BM = BC + CM = 2`
+- :math:`MH = \sqrt{BH^2 - BM^2}` [Pythagoras theorem on BHM]
+- :math:`tan(A) = \frac{HM}{BM}`
+- :math:`u = MC = BM - CB`
+- :math:`t = ∠ABK + ∠KBH - ∠HBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(\sqrt{u1^2 - 4} - 2)`
+- :math:`A = arctan(\frac{2}{\sqrt{u1^2 - 4}})`
+- :math:`t = \frac{π}{2} + θ - A`
+- :math:`v = (t - φ - \frac{π}{2})`
+
+
+10. **LeftxRight90-Straight-Right**
+
+.. image:: L_R90SR.png
+
+With followng notations:
+
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - \frac{π}{2} - v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`BD = 2` [2 * radius of curvature]
+- :math:`DG = EF = u` [DGFE is a rectangle]
+- :math:`DG = BG - BD = 2`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = u1 - 2`
+- :math:`t = \frac{π}{2} + θ`
+- :math:`v = (t - φ - \frac{π}{2})`
+
+
+11. **Left-Straight-Left90xRight**
+
+.. image:: LSL90xR.png
+
+With followng notations:
+
+- :math:`∠KBH = θ`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + \frac{π}{2} + v = φ`
+- :math:`H(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`GH = 2` [2 * radius of curvature]
+- :math:`DC = BG = u` [DGBC is a rectangle]
+- :math:`BG = BH - GH`
+- :math:`∠ABC= ∠KBH`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = u1 - 2`
+- :math:`t = θ`
+- :math:`v = (φ - t - \frac{π}{2})`
+
+
+12. **LeftxRight90-Straight-Left90xRight**
+
+.. image:: L_R90SL90_R.png
+
+With followng notations:
+
+- :math:`∠KBH = θ`
+- :math:`∠HBM = A`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - v = φ`
+- :math:`H(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector)`
+- :math:`GF = ED = 1` [radius of curvature]
+- :math:`BD = GH = 2` [2 * radius of curvature]
+- :math:`FN = GH = 2` [ENMD is a rectangle]
+- :math:`NH = GF = 1` [FNHG is a rectangle]
+- :math:`MN = ED = 1` [ENMD is a rectangle]
+- :math:`DO = EF = u` [DOFE is a rectangle]
+- :math:`MH = MN + NH = 2`
+- :math:`BM = \sqrt{BH^2 - MH^2}` [Pythagoras theorem on BHM]
+- :math:`DO = BM - BD - OM`
+- :math:`tan(A) = \frac{MH}{BM}`
+- :math:`∠ABC = ∠ABK + ∠KBH + ∠HBM`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = /sqrt{u1^2 - 4} - 4`
+- :math:`A = arctan(\frac{2}{u1^2 - 4})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ)`
+
+
+Reference
+=============
+
+- `15.3.2 Reeds-Shepp
+ Curves `__
+
+- `optimal paths for a car that goes both forwards and
+ backwards `__
+
+- `ghliu/pyReedsShepp: Implementation of Reeds Shepp
+ curve. `__
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_1.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_1.png
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_3.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_3.png
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_4.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_4.png
diff --git a/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_5.png
similarity index 100%
rename from docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png
rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_5.png
diff --git a/docs/modules/path_planning/rrt/figure_1.png b/docs/modules/5_path_planning/rrt/figure_1.png
similarity index 100%
rename from docs/modules/path_planning/rrt/figure_1.png
rename to docs/modules/5_path_planning/rrt/figure_1.png
diff --git a/docs/modules/path_planning/rrt/rrt_main.rst b/docs/modules/5_path_planning/rrt/rrt_main.rst
similarity index 74%
rename from docs/modules/path_planning/rrt/rrt_main.rst
rename to docs/modules/5_path_planning/rrt/rrt_main.rst
index 5a3a30dcff..1bd5497f4c 100644
--- a/docs/modules/path_planning/rrt/rrt_main.rst
+++ b/docs/modules/5_path_planning/rrt/rrt_main.rst
@@ -14,6 +14,12 @@ This is a simple path planning code with Rapidly-Exploring Random Trees
Black circles are obstacles, green line is a searched tree, red crosses
are start and goal positions.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRT.rrt.RRT
+
+
.. include:: rrt_star.rst
@@ -24,6 +30,12 @@ RRT with dubins path
Path planning for a car robot with RRT and dubins path planner.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTDubins.rrt_dubins.RRTDubins
+
+
.. _rrt*-with-dubins-path:
RRT\* with dubins path
@@ -33,6 +45,12 @@ RRT\* with dubins path
Path planning for a car robot with RRT\* and dubins path planner.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTStarDubins.rrt_star_dubins.RRTStarDubins
+
+
.. _rrt*-with-reeds-sheep-path:
RRT\* with reeds-sheep path
@@ -42,6 +60,12 @@ RRT\* with reeds-sheep path
Path planning for a car robot with RRT\* and reeds sheep path planner.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTStarReedsShepp.rrt_star_reeds_shepp.RRTStarReedsShepp
+
+
.. _informed-rrt*:
Informed RRT\*
@@ -53,11 +77,18 @@ This is a path planning code with Informed RRT*.
The cyan ellipse is the heuristic sampling domain of Informed RRT*.
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.InformedRRTStar.informed_rrt_star.InformedRRTStar
+
+
+Reference
+^^^^^^^^^^
- `Informed RRT\*: Optimal Sampling-based Path Planning Focused via
Direct Sampling of an Admissible Ellipsoidal
- Heuristic `__
+ Heuristic `__
.. _batch-informed-rrt*:
@@ -68,12 +99,20 @@ Batch Informed RRT\*
This is a path planning code with Batch Informed RRT*.
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.BatchInformedRRTStar.batch_informed_rrt_star.BITStar
+
+
+Reference
+^^^^^^^^^^^
- `Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the
Heuristically Guided Search of Implicit Random Geometric
Graphs `__
+
.. _closed-loop-rrt*:
Closed Loop RRT\*
@@ -87,17 +126,25 @@ In this code, pure-pursuit algorithm is used for steering control,
PID is used for speed control.
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.ClosedLoopRRTStar.closed_loop_rrt_star_car.ClosedLoopRRTStar
+
+
+Reference
+^^^^^^^^^^^^
- `Motion Planning in Complex Environments using Closed-loop
- Prediction `__
+ Prediction `__
- `Real-time Motion Planning with Applications to Autonomous Urban
- Driving `__
+ Driving `__
- `[1601.06326] Sampling-based Algorithms for Optimal Motion Planning
Using Closed-loop Prediction `__
+
.. _lqr-rrt*:
LQR-RRT\*
@@ -109,10 +156,17 @@ A double integrator motion model is used for LQR local planner.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif
-Ref:
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.LQRRRTStar.lqr_rrt_star.LQRRRTStar
+
+
+Reference
+~~~~~~~~~~~~~
- `LQR-RRT\*: Optimal Sampling-Based Motion Planning with Automatically
Derived Extension
- Heuristics `__
+ Heuristics `__
- `MahanFathi/LQR-RRTstar: LQR-RRT\* method is used for random motion planning of a simple pendulum in its phase plot `__
\ No newline at end of file
diff --git a/docs/modules/path_planning/rrt/rrt_star.rst b/docs/modules/5_path_planning/rrt/rrt_star.rst
similarity index 82%
rename from docs/modules/path_planning/rrt/rrt_star.rst
rename to docs/modules/5_path_planning/rrt/rrt_star.rst
index d36eaac70e..960b9976d5 100644
--- a/docs/modules/path_planning/rrt/rrt_star.rst
+++ b/docs/modules/5_path_planning/rrt/rrt_star.rst
@@ -7,6 +7,12 @@ This is a path planning code with RRT\*
Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.
+Code Link
+^^^^^^^^^^
+
+.. autoclass:: PathPlanning.RRTStar.rrt_star.RRTStar
+
+
Simulation
^^^^^^^^^^
@@ -16,6 +22,6 @@ Simulation
Ref
^^^
-- `Sampling-based Algorithms for Optimal Motion Planning `__
+- `Sampling-based Algorithms for Optimal Motion Planning `__
- `Incremental Sampling-based Algorithms for Optimal Motion Planning `__
diff --git a/docs/modules/path_planning/rrt/rrt_star/rrt_star_1_0.png b/docs/modules/5_path_planning/rrt/rrt_star/rrt_star_1_0.png
similarity index 100%
rename from docs/modules/path_planning/rrt/rrt_star/rrt_star_1_0.png
rename to docs/modules/5_path_planning/rrt/rrt_star/rrt_star_1_0.png
diff --git a/docs/modules/path_planning/rrt/rrt_star_reeds_shepp/figure_1.png b/docs/modules/5_path_planning/rrt/rrt_star_reeds_shepp/figure_1.png
similarity index 100%
rename from docs/modules/path_planning/rrt/rrt_star_reeds_shepp/figure_1.png
rename to docs/modules/5_path_planning/rrt/rrt_star_reeds_shepp/figure_1.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_1.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_1.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_1.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_1.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_2.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_2.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_2.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_2.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_3.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_3.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_3.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_3.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_4.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_4.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_4.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_4.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_5.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_5.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_5.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_5.png
diff --git a/docs/modules/path_planning/state_lattice_planner/Figure_6.png b/docs/modules/5_path_planning/state_lattice_planner/Figure_6.png
similarity index 100%
rename from docs/modules/path_planning/state_lattice_planner/Figure_6.png
rename to docs/modules/5_path_planning/state_lattice_planner/Figure_6.png
diff --git a/docs/modules/path_planning/state_lattice_planner/state_lattice_planner_main.rst b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst
similarity index 61%
rename from docs/modules/path_planning/state_lattice_planner/state_lattice_planner_main.rst
rename to docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst
index 3ef0c7eca2..9f8cc0c50f 100644
--- a/docs/modules/path_planning/state_lattice_planner/state_lattice_planner_main.rst
+++ b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst
@@ -12,22 +12,39 @@ Uniform polar sampling
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif
+Code Link
+^^^^^^^^^^^^^
+
+.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_uniform_polar_states
+
+
Biased polar sampling
~~~~~~~~~~~~~~~~~~~~~
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/BiasedPolarSampling.gif
+Code Link
+^^^^^^^^^^^^^
+.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_biased_polar_states
+
+
Lane sampling
~~~~~~~~~~~~~
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif
-Ref:
+Code Link
+^^^^^^^^^^^^^
+.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_lane_states
+
+
+Reference
+~~~~~~~~~~~~~~~
- `Optimal rough terrain trajectory generation for wheeled mobile
- robots `__
+ robots `__
- `State Space Sampling of Feasible Motions for High-Performance Mobile
Robot Navigation in Complex
- Environments `__
+ Environments `__
diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst
new file mode 100644
index 0000000000..a44dd20a97
--- /dev/null
+++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst
@@ -0,0 +1,108 @@
+Time based grid search
+----------------------
+
+Space-time astar
+~~~~~~~~~~~~~~~~~~~~~~
+
+This is an extension of A* algorithm that supports planning around dynamic obstacles.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar/path_animation.gif
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar/path_animation2.gif
+
+The key difference of this algorithm compared to vanilla A* is that the cost and heuristic are now time-based instead of distance-based.
+Using a time-based cost and heuristic ensures the path found is optimal in terms of time to reach the goal.
+
+The cost is the amount of time it takes to reach a given node, and the heuristic is the minimum amount of time it could take to reach the goal from that node, disregarding all obstacles.
+For a simple scenario where the robot can move 1 cell per time step and stop and go as it pleases, the heuristic for time is equivalent to the heuristic for distance.
+
+One optimization that was added in `this PR `__ was to add an expanded set to the algorithm. The algorithm will not expand nodes that are already in that set. This greatly reduces the number of node expansions needed to find a path, since no duplicates are expanded. It also helps to reduce the amount of memory the algorithm uses.
+
+Before::
+
+ Found path to goal after 204490 expansions
+ Planning took: 1.72464 seconds
+ Memory usage (RSS): 68.19 MB
+
+
+After::
+
+ Found path to goal after 2348 expansions
+ Planning took: 0.01550 seconds
+ Memory usage (RSS): 64.85 MB
+
+When starting at (1, 11) in the structured obstacle arrangement (second of the two gifs above).
+
+Code Link
+^^^^^^^^^^^^^
+.. autoclass:: PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar.SpaceTimeAStar
+
+
+Safe Interval Path Planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The safe interval path planning algorithm is described in this paper:
+
+`SIPP: Safe Interval Path Planning for Dynamic Environments `__
+
+It is faster than space-time A* because it pre-computes the intervals of time that are unoccupied in each cell. This allows it to reduce the number of successor node it generates by avoiding nodes within the same interval.
+
+**Comparison with SpaceTime A*:**
+
+Arrangement 1 starting at (1, 18)
+
+SafeInterval planner::
+
+ Found path to goal after 322 expansions
+ Planning took: 0.00730 seconds
+
+SpaceTime A*::
+
+ Found path to goal after 2717154 expansions
+ Planning took: 20.51330 seconds
+
+**Benchmarking the Safe Interval Path Planner:**
+
+250 random obstacles::
+
+ Found path to goal after 764 expansions
+ Planning took: 0.60596 seconds
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/SafeIntervalPathPlanner/path_animation.gif
+
+Arrangement 1 starting at (1, 18)::
+
+ Found path to goal after 322 expansions
+ Planning took: 0.00730 seconds
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/SafeIntervalPathPlanner/path_animation2.gif
+
+Code Link
+^^^^^^^^^^^^^
+.. autoclass:: PathPlanning.TimeBasedPathPlanning.SafeInterval.SafeIntervalPathPlanner
+
+Multi-Agent Path Planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Priority Based Planning
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The planner generates an order to plan in, and generates plans for the robots in that order. Each planned path is reserved in the grid, and all future plans must avoid that path. The robots are planned for in descending order of distance from start to goal.
+
+This is a sub-optimal algorithm because no replanning happens once paths are found. It does not guarantee the shortest path is found for any particular robot, nor that the final set of paths found contains the lowest possible amount of time or movement.
+
+Static Obstacles:
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner/priority_planner2.gif
+
+Dynamic Obstacles:
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner/priority_planner.gif
+
+
+References
+~~~~~~~~~~~
+
+- `Cooperative Pathfinding `__
+- `SIPP: Safe Interval Path Planning for Dynamic Environments `__
+- `Prioritized Planning Algorithms for Trajectory Coordination of Multiple Mobile Robots `__
\ No newline at end of file
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step0.png b/docs/modules/5_path_planning/visibility_road_map_planner/step0.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step0.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step0.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step1.png b/docs/modules/5_path_planning/visibility_road_map_planner/step1.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step1.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step1.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step2.png b/docs/modules/5_path_planning/visibility_road_map_planner/step2.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step2.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step2.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/step3.png b/docs/modules/5_path_planning/visibility_road_map_planner/step3.png
similarity index 100%
rename from docs/modules/path_planning/visibility_road_map_planner/step3.png
rename to docs/modules/5_path_planning/visibility_road_map_planner/step3.png
diff --git a/docs/modules/path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst b/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
similarity index 94%
rename from docs/modules/path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
rename to docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
index 3c9b7c008c..aac96d6e19 100644
--- a/docs/modules/path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
+++ b/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst
@@ -13,6 +13,11 @@ red crosses are visibility nodes, and blue lines area collision free visibility
The red line is the final path searched by dijkstra algorithm frm the visibility graphs.
+Code Link
+~~~~~~~~~~~~
+.. autoclass:: PathPlanning.VisibilityRoadMap.visibility_road_map.VisibilityRoadMap
+
+
Algorithms
~~~~~~~~~~
@@ -64,7 +69,7 @@ The red line is searched path in the figure:
You can find the details of Dijkstra algorithm in :ref:`dijkstra`.
References
-^^^^^^^^^^
+~~~~~~~~~~~~
- `Visibility graph - Wikipedia `_
diff --git a/docs/modules/path_planning/vrm_planner/vrm_planner_main.rst b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
similarity index 79%
rename from docs/modules/path_planning/vrm_planner/vrm_planner_main.rst
rename to docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
index 92e729ab29..a9a41aab74 100644
--- a/docs/modules/path_planning/vrm_planner/vrm_planner_main.rst
+++ b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst
@@ -11,7 +11,13 @@ Cyan crosses mean searched points with Dijkstra method,
The red line is the final path of Vornoi Road-Map.
-Ref:
+Code Link
+~~~~~~~~~~~~~~~
+.. autoclass:: PathPlanning.VoronoiRoadMap.voronoi_road_map.VoronoiRoadMapPlanner
+
+
+Reference
+~~~~~~~~~~~~
- `Robotic Motion Planning `__
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png
similarity index 100%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png
diff --git a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
similarity index 92%
rename from docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
rename to docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
index a1980ba17e..914a4555ff 100644
--- a/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
+++ b/docs/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst
@@ -17,6 +17,11 @@ Nonlinear Model Predictive Control with C-GMRES
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif
:alt: gif
+Code Link
+~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.cgmres_nmpc.cgmres_nmpc.NMPCControllerCGMRES
+
Mathematical Formulation
~~~~~~~~~~~~~~~~~~~~~~~~
@@ -60,7 +65,7 @@ Ref
- `Shunichi09/nonlinear_control: Implementing the nonlinear model
predictive control, sliding mode
- control `__
+ control `__
- `非線形モデル予測制御におけるCGMRES法をpythonで実装する -
Qiita `__
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
new file mode 100644
index 0000000000..b0ba9e0d45
--- /dev/null
+++ b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
@@ -0,0 +1,145 @@
+.. _linearquadratic-regulator-(lqr)-speed-and-steering-control:
+
+Linear–quadratic regulator (LQR) speed and steering control
+-----------------------------------------------------------
+
+Path tracking simulation with LQR speed and steering control.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif
+
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.lqr_speed_steer_control.lqr_speed_steer_control.lqr_speed_steering_control
+
+
+Overview
+~~~~~~~~
+
+The LQR (Linear Quadratic Regulator) speed and steering control model implemented in `lqr_speed_steer_control.py` provides a simulation
+for an autonomous vehicle to track:
+
+1. A desired speed by adjusting acceleration based on feedback from the current state and the desired speed.
+
+2. A desired trajectory by adjusting steering angle based on feedback from the current state and the desired trajectory.
+
+by only using one LQT controller.
+
+Vehicle motion Model
+~~~~~~~~~~~~~~~~~~~~~
+
+The below figure shows the geometric model of the vehicle used in this simulation:
+
+.. image:: lqr_steering_control_model.jpg
+ :width: 600px
+
+The `e`, :math:`{\theta}`, and :math:`\upsilon` represent the lateral error, orientation error, and velocity error, respectively, with respect to the desired trajectory and speed.
+And :math:`\dot{e}` and :math:`\dot{\theta}` represent the rates of change of these errors.
+
+The :math:`e_t` and :math:`\theta_t`, and :math:`\upsilon` are the updated values of `e`, :math:`\theta`, :math:`\upsilon` and at time `t`, respectively, and can be calculated using the following kinematic equations:
+
+.. math:: e_t = e_{t-1} + \dot{e}_{t-1} dt
+
+.. math:: \theta_t = \theta_{t-1} + \dot{\theta}_{t-1} dt
+
+.. math:: \upsilon_t = \upsilon_{t-1} + a_{t-1} dt
+
+Where `dt` is the time difference and :math:`a_t` is the acceleration at the time `t`.
+
+The change rate of the `e` can be calculated as:
+
+.. math:: \dot{e}_t = V \sin(\theta_{t-1})
+
+Where `V` is the current vehicle speed.
+
+If the :math:`\theta` is small,
+
+.. math:: \theta \approx 0
+
+the :math:`\sin(\theta)` can be approximated as :math:`\theta`:
+
+.. math:: \sin(\theta) = \theta
+
+So, the change rate of the `e` can be approximated as:
+
+.. math:: \dot{e}_t = V \theta_{t-1}
+
+The change rate of the :math:`\theta` can be calculated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \tan(\delta)
+
+where `L` is the wheelbase of the vehicle and :math:`\delta` is the steering angle.
+
+If the :math:`\delta` is small,
+
+.. math:: \delta \approx 0
+
+the :math:`\tan(\delta)` can be approximated as :math:`\delta`:
+
+.. math:: \tan(\delta) = \delta
+
+So, the change rate of the :math:`\theta` can be approximated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \delta
+
+The above equations can be used to update the state of the vehicle at each time step.
+
+Control Model
+~~~~~~~~~~~~~~
+
+To formulate the state-space representation of the vehicle dynamics as a linear model,
+the state vector `x` and control input vector `u` are defined as follows:
+
+.. math:: x_t = [e_t, \dot{e}_t, \theta_t, \dot{\theta}_t, \upsilon_t]^T
+
+.. math:: u_t = [\delta_t, a_t]^T
+
+The linear state transition equation can be represented as:
+
+.. math:: x_{t+1} = A x_t + B u_t
+
+where:
+
+:math:`\begin{equation*} A = \begin{bmatrix} 1 & dt & 0 & 0 & 0\\ 0 & 0 & v & 0 & 0\\ 0 & 0 & 1 & dt & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\\end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} B = \begin{bmatrix} 0 & 0\\ 0 & 0\\ 0 & 0\\ \frac{v}{L} & 0\\ 0 & dt \\ \end{bmatrix} \end{equation*}`
+
+LQR controller
+~~~~~~~~~~~~~~~
+
+The Linear Quadratic Regulator (LQR) controller is used to calculate the optimal control input `u` that minimizes the quadratic cost function:
+
+:math:`J = \sum_{t=0}^{N} (x_t^T Q x_t + u_t^T R u_t)`
+
+where `Q` and `R` are the weighting matrices for the state and control input, respectively.
+
+for the linear model:
+
+:math:`x_{t+1} = A x_t + B u_t`
+
+The optimal control input `u` can be calculated as:
+
+:math:`u_t = -K x_t`
+
+where `K` is the feedback gain matrix obtained by solving the Riccati equation.
+
+Simulation results
+~~~~~~~~~~~~~~~~~~~
+
+
+.. image:: x-y.png
+ :width: 600px
+
+.. image:: yaw.png
+ :width: 600px
+
+.. image:: speed.png
+ :width: 600px
+
+
+
+Reference
+~~~~~~~~~~~
+
+- `Towards fully autonomous driving: Systems and algorithms `__
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg
new file mode 100644
index 0000000000..83754d5bb0
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg differ
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/speed.png b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/speed.png
new file mode 100644
index 0000000000..ae99eb7ea3
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/speed.png differ
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/x-y.png b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/x-y.png
new file mode 100644
index 0000000000..bff3f1a786
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/x-y.png differ
diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/yaw.png b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/yaw.png
new file mode 100644
index 0000000000..7f3d9c1d10
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/yaw.png differ
diff --git a/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst
new file mode 100644
index 0000000000..fc8f2a33aa
--- /dev/null
+++ b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst
@@ -0,0 +1,125 @@
+.. _linearquadratic-regulator-(lqr)-steering-control:
+
+Linear–quadratic regulator (LQR) steering control
+-------------------------------------------------
+
+Path tracking simulation with LQR steering control and PID speed
+control.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.lqr_steer_control.lqr_steer_control.lqr_steering_control
+
+
+Overview
+~~~~~~~~
+
+The LQR (Linear Quadratic Regulator) steering control model implemented in lqr_steer_control.py provides a simulation
+for an autonomous vehicle to track a desired trajectory by adjusting steering angle based on feedback from the current state and the desired trajectory.
+This model utilizes a combination of PID speed control and LQR steering control to achieve smooth and accurate trajectory tracking.
+
+Vehicle motion Model
+~~~~~~~~~~~~~~~~~~~~~
+
+The below figure shows the geometric model of the vehicle used in this simulation:
+
+.. image:: lqr_steering_control_model.jpg
+ :width: 600px
+
+The `e` and :math:`\theta` represent the lateral error and orientation error, respectively, with respect to the desired trajectory.
+And :math:`\dot{e}` and :math:`\dot{\theta}` represent the rates of change of these errors.
+
+The :math:`e_t` and :math:`\theta_t` are the updated values of `e` and :math:`\theta` at time `t`, respectively, and can be calculated using the following kinematic equations:
+
+.. math:: e_t = e_{t-1} + \dot{e}_{t-1} dt
+
+.. math:: \theta_t = \theta_{t-1} + \dot{\theta}_{t-1} dt
+
+Where `dt` is the time difference.
+
+The change rate of the `e` can be calculated as:
+
+.. math:: \dot{e}_t = V \sin(\theta_{t-1})
+
+Where `V` is the current vehicle speed.
+
+If the :math:`\theta` is small,
+
+.. math:: \theta \approx 0
+
+the :math:`\sin(\theta)` can be approximated as :math:`\theta`:
+
+.. math:: \sin(\theta) = \theta
+
+So, the change rate of the `e` can be approximated as:
+
+.. math:: \dot{e}_t = V \theta_{t-1}
+
+The change rate of the :math:`\theta` can be calculated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \tan(\delta)
+
+where `L` is the wheelbase of the vehicle and :math:`\delta` is the steering angle.
+
+If the :math:`\delta` is small,
+
+.. math:: \delta \approx 0
+
+the :math:`\tan(\delta)` can be approximated as :math:`\delta`:
+
+.. math:: \tan(\delta) = \delta
+
+So, the change rate of the :math:`\theta` can be approximated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \delta
+
+The above equations can be used to update the state of the vehicle at each time step.
+
+Control Model
+~~~~~~~~~~~~~~
+
+To formulate the state-space representation of the vehicle dynamics as a linear model,
+the state vector `x` and control input vector `u` are defined as follows:
+
+.. math:: x_t = [e_t, \dot{e}_t, \theta_t, \dot{\theta}_t]^T
+
+.. math:: u_t = \delta_t
+
+The linear state transition equation can be represented as:
+
+.. math:: x_{t+1} = A x_t + B u_t
+
+where:
+
+:math:`\begin{equation*} A = \begin{bmatrix} 1 & dt & 0 & 0\\ 0 & 0 & v & 0\\ 0 & 0 & 1 & dt\\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} B = \begin{bmatrix} 0\\ 0\\ 0\\ \frac{v}{L} \\ \end{bmatrix} \end{equation*}`
+
+LQR controller
+~~~~~~~~~~~~~~~
+
+The Linear Quadratic Regulator (LQR) controller is used to calculate the optimal control input `u` that minimizes the quadratic cost function:
+
+:math:`J = \sum_{t=0}^{N} (x_t^T Q x_t + u_t^T R u_t)`
+
+where `Q` and `R` are the weighting matrices for the state and control input, respectively.
+
+for the linear model:
+
+:math:`x_{t+1} = A x_t + B u_t`
+
+The optimal control input `u` can be calculated as:
+
+:math:`u_t = -K x_t`
+
+where `K` is the feedback gain matrix obtained by solving the Riccati equation.
+
+Reference
+~~~~~~~~~~~
+- `ApolloAuto/apollo: An open autonomous driving platform `_
+
+- `Linear Quadratic Regulator (LQR) `_
+
diff --git a/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_model.jpg b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_model.jpg
new file mode 100644
index 0000000000..83754d5bb0
Binary files /dev/null and b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_model.jpg differ
diff --git a/docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst b/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
similarity index 92%
rename from docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
rename to docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
index 59a7166674..76a6819a46 100644
--- a/docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
+++ b/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst
@@ -5,13 +5,6 @@ Model predictive speed and steering control
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif?raw=true
:alt: Model predictive speed and steering control
- Model predictive speed and steering control
-
-code:
-
-`PythonRobotics/model_predictive_speed_and_steer_control.py at master ·
-AtsushiSakai/PythonRobotics `__
-
This is a path tracking simulation using model predictive control (MPC).
The MPC controller controls vehicle speed and steering base on
@@ -22,6 +15,12 @@ This code uses cvxpy as an optimization modeling tool.
- `Welcome to CVXPY 1.0 — CVXPY 1.0.6
documentation `__
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.model_predictive_speed_and_steer_control.model_predictive_speed_and_steer_control.iterative_linear_mpc_control
+
+
MPC modeling
~~~~~~~~~~~~
@@ -133,5 +132,5 @@ Reference
- `Vehicle Dynamics and Control \| Rajesh Rajamani \|
Springer `__
-- `MPC Course Material - MPC Lab @
- UC-Berkeley `__
+- `MPC Book - MPC Lab @
+ UC-Berkeley `__
diff --git a/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control_main.rst b/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst
similarity index 97%
rename from docs/modules/control/move_to_a_pose_control/move_to_a_pose_control_main.rst
rename to docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst
index 77ec682a30..19bb0e4c14 100644
--- a/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control_main.rst
+++ b/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst
@@ -3,7 +3,13 @@ Move to a Pose Control
In this section, we present the logic of PathFinderController that drives a car from a start pose (x, y, theta) to a goal pose. A simulation of moving to a pose control is presented below.
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/move_to_pose/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.move_to_pose.move_to_pose.move_to_pose
+
Position Control of non-Holonomic Systems
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
diff --git a/docs/modules/6_path_tracking/path_tracking_main.rst b/docs/modules/6_path_tracking/path_tracking_main.rst
new file mode 100644
index 0000000000..742cc807e6
--- /dev/null
+++ b/docs/modules/6_path_tracking/path_tracking_main.rst
@@ -0,0 +1,19 @@
+.. _`Path Tracking`:
+
+Path Tracking
+=============
+
+Path tracking is the ability of a robot to follow the reference path generated by a path planner while simultaneously stabilizing the robot. The path tracking controller may need to account for modeling error and other forms of uncertainty. In path tracking, feedback control techniques and optimization based control techniques are widely used[22]. Fig.6 shows simulations using rear wheel feedback steering control and PID speed control, and iterative linear model predictive path tracking control[27].
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents
+
+ pure_pursuit_tracking/pure_pursuit_tracking
+ stanley_control/stanley_control
+ rear_wheel_feedback_control/rear_wheel_feedback_control
+ lqr_steering_control/lqr_steering_control
+ lqr_speed_and_steering_control/lqr_speed_and_steering_control
+ model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control
+ cgmres_nmpc/cgmres_nmpc
+ move_to_a_pose/move_to_a_pose
diff --git a/docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
similarity index 80%
rename from docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
rename to docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
index 5c7bcef85f..ff6749bbbe 100644
--- a/docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
+++ b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
@@ -9,7 +9,13 @@ speed control.
The red line is a target course, the green cross means the target point
for pure pursuit control, the blue line is the tracking.
-References:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.pure_pursuit.pure_pursuit.pure_pursuit_steer_control
+
+
+Reference
~~~~~~~~~~~
- `A Survey of Motion Planning and Control Techniques for Self-driving
diff --git a/docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst b/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
similarity index 72%
rename from docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
rename to docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
index 70875fdc6c..56d0db63ad 100644
--- a/docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
+++ b/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst
@@ -6,7 +6,13 @@ PID speed control.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif
-References:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.rear_wheel_feedback_control.rear_wheel_feedback_control.rear_wheel_feedback_control
+
+
+Reference
~~~~~~~~~~~
- `A Survey of Motion Planning and Control Techniques for Self-driving
Urban Vehicles `__
diff --git a/docs/modules/path_tracking/stanley_control/stanley_control_main.rst b/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst
similarity index 82%
rename from docs/modules/path_tracking/stanley_control/stanley_control_main.rst
rename to docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst
index fe325b0102..3c491804f6 100644
--- a/docs/modules/path_tracking/stanley_control/stanley_control_main.rst
+++ b/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst
@@ -6,7 +6,13 @@ control.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif
-References:
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: PathTracking.stanley_control.stanley_control.stanley_control
+
+
+Reference
~~~~~~~~~~~
- `Stanley: The robot that won the DARPA grand
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png
diff --git a/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png b/docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png
similarity index 100%
rename from docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png
rename to docs/modules/7_arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png
diff --git a/docs/modules/arm_navigation/arm_navigation_main.rst b/docs/modules/7_arm_navigation/arm_navigation_main.rst
similarity index 88%
rename from docs/modules/arm_navigation/arm_navigation_main.rst
rename to docs/modules/7_arm_navigation/arm_navigation_main.rst
index bbbc872c58..7acd3ee7d3 100644
--- a/docs/modules/arm_navigation/arm_navigation_main.rst
+++ b/docs/modules/7_arm_navigation/arm_navigation_main.rst
@@ -1,4 +1,4 @@
-.. _arm_navigation:
+.. _`Arm Navigation`:
Arm Navigation
==============
diff --git a/docs/modules/arm_navigation/n_joint_arm_to_point_control_main.rst b/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst
similarity index 76%
rename from docs/modules/arm_navigation/n_joint_arm_to_point_control_main.rst
rename to docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst
index cc6279f681..56900acde1 100644
--- a/docs/modules/arm_navigation/n_joint_arm_to_point_control_main.rst
+++ b/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst
@@ -11,3 +11,10 @@ plotting area.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/n_joint_arm_to_point_control/animation.gif
In this simulation N = 10, however, you can change it.
+
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: ArmNavigation.n_joint_arm_to_point_control.n_joint_arm_to_point_control.main
+
diff --git a/docs/modules/arm_navigation/obstacle_avoidance_arm_navigation_main.rst b/docs/modules/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst
similarity index 52%
rename from docs/modules/arm_navigation/obstacle_avoidance_arm_navigation_main.rst
rename to docs/modules/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst
index 899a64a5cd..4433ab531b 100644
--- a/docs/modules/arm_navigation/obstacle_avoidance_arm_navigation_main.rst
+++ b/docs/modules/7_arm_navigation/obstacle_avoidance_arm_navigation_main.rst
@@ -3,4 +3,9 @@ Arm navigation with obstacle avoidance
Arm navigation with obstacle avoidance simulation.
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/arm_obstacle_navigation/animation.gif
\ No newline at end of file
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/arm_obstacle_navigation/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: ArmNavigation.arm_obstacle_navigation.arm_obstacle_navigation.main
diff --git a/docs/modules/arm_navigation/planar_two_link_ik_main.rst b/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst
similarity index 98%
rename from docs/modules/arm_navigation/planar_two_link_ik_main.rst
rename to docs/modules/7_arm_navigation/planar_two_link_ik_main.rst
index 2da2b0dc30..5b2712eb48 100644
--- a/docs/modules/arm_navigation/planar_two_link_ik_main.rst
+++ b/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst
@@ -11,6 +11,12 @@ This is a interactive simulation.
You can set the goal position of the end effector with left-click on the plotting area.
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: ArmNavigation.two_joint_arm_to_point_control.two_joint_arm_to_point_control.main
+
+
Inverse Kinematics for a Planar Two-Link Robotic Arm
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
diff --git a/docs/modules/aerial_navigation/aerial_navigation_main.rst b/docs/modules/8_aerial_navigation/aerial_navigation_main.rst
similarity index 89%
rename from docs/modules/aerial_navigation/aerial_navigation_main.rst
rename to docs/modules/8_aerial_navigation/aerial_navigation_main.rst
index b2ccb071af..7f76689770 100644
--- a/docs/modules/aerial_navigation/aerial_navigation_main.rst
+++ b/docs/modules/8_aerial_navigation/aerial_navigation_main.rst
@@ -1,4 +1,4 @@
-.. _aerial_navigation:
+.. _`Aerial Navigation`:
Aerial Navigation
=================
diff --git a/docs/modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst b/docs/modules/8_aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
similarity index 66%
rename from docs/modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
rename to docs/modules/8_aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
index c3bdc33cdc..1805bb3f6d 100644
--- a/docs/modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
+++ b/docs/modules/8_aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst
@@ -4,3 +4,8 @@ Drone 3d trajectory following
This is a 3d trajectory following simulation for a quadrotor.
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/drone_3d_trajectory_following/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: AerialNavigation.drone_3d_trajectory_following.drone_3d_trajectory_following.quad_sim
diff --git a/docs/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst b/docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
similarity index 96%
rename from docs/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
rename to docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
index 6c90d2b44e..4bf5117500 100644
--- a/docs/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
+++ b/docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst
@@ -1,6 +1,14 @@
Rocket powered landing
-----------------------------
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/rocket_powered_landing/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: AerialNavigation.rocket_powered_landing.rocket_powered_landing.main
+
+
Simulation
~~~~~~~~~~
diff --git a/docs/modules/bipedal/bipedal_main.rst b/docs/modules/9_bipedal/bipedal_main.rst
similarity index 88%
rename from docs/modules/bipedal/bipedal_main.rst
rename to docs/modules/9_bipedal/bipedal_main.rst
index fc5b933055..dc387dc4e8 100644
--- a/docs/modules/bipedal/bipedal_main.rst
+++ b/docs/modules/9_bipedal/bipedal_main.rst
@@ -1,4 +1,4 @@
-.. _bipedal:
+.. _`Bipedal`:
Bipedal
=================
diff --git a/docs/modules/bipedal/bipedal_planner/bipedal_planner_main.rst b/docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst
similarity index 67%
rename from docs/modules/bipedal/bipedal_planner/bipedal_planner_main.rst
rename to docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst
index 2ee5971e7a..6253715cc1 100644
--- a/docs/modules/bipedal/bipedal_planner/bipedal_planner_main.rst
+++ b/docs/modules/9_bipedal/bipedal_planner/bipedal_planner_main.rst
@@ -4,3 +4,8 @@ Bipedal Planner
Bipedal Walking with modifying designated footsteps
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Bipedal/bipedal_planner/animation.gif
+
+Code Link
+~~~~~~~~~~~~~~~
+
+.. autofunction:: Bipedal.bipedal_planner.bipedal_planner.BipedalPlanner
diff --git a/docs/modules/control/control_main.rst b/docs/modules/control/control_main.rst
deleted file mode 100644
index cee2aa9e8e..0000000000
--- a/docs/modules/control/control_main.rst
+++ /dev/null
@@ -1,12 +0,0 @@
-.. _control:
-
-Control
-=================
-
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- inverted_pendulum_control/inverted_pendulum_control
- move_to_a_pose_control/move_to_a_pose_control
-
diff --git a/docs/modules/introduction_main.rst b/docs/modules/introduction_main.rst
deleted file mode 100644
index f9fb487297..0000000000
--- a/docs/modules/introduction_main.rst
+++ /dev/null
@@ -1,42 +0,0 @@
-
-Introduction
-============
-
-TBD
-
-Definition Of Robotics
-----------------------
-
-TBD
-
-History Of Robotics
-----------------------
-
-TBD
-
-Application Of Robotics
-------------------------
-
-TBD
-
-Software for Robotics
-----------------------
-
-TBD
-
-Software for Robotics
-----------------------
-
-TBD
-
-Python for Robotics
-----------------------
-
-TBD
-
-Learning Robotics Algorithms
-----------------------------
-
-TBD
-
-
diff --git a/docs/modules/mapping/mapping_main.rst b/docs/modules/mapping/mapping_main.rst
deleted file mode 100644
index 1c02c75cb2..0000000000
--- a/docs/modules/mapping/mapping_main.rst
+++ /dev/null
@@ -1,16 +0,0 @@
-.. _mapping:
-
-Mapping
-=======
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- gaussian_grid_map/gaussian_grid_map
- ray_casting_grid_map/ray_casting_grid_map
- lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial
- point_cloud_sampling/point_cloud_sampling
- k_means_object_clustering/k_means_object_clustering
- circle_fitting/circle_fitting
- rectangle_fitting/rectangle_fitting
- normal_vector_estimation/normal_vector_estimation
diff --git a/docs/modules/mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst b/docs/modules/mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
deleted file mode 100644
index cc5a1a1c5b..0000000000
--- a/docs/modules/mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst
+++ /dev/null
@@ -1,6 +0,0 @@
-Ray casting grid map
---------------------
-
-This is a 2D ray casting grid mapping example.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/raycasting_grid_map/animation.gif
\ No newline at end of file
diff --git a/docs/modules/path_planning/bugplanner/bugplanner_main.rst b/docs/modules/path_planning/bugplanner/bugplanner_main.rst
deleted file mode 100644
index 72cc46709f..0000000000
--- a/docs/modules/path_planning/bugplanner/bugplanner_main.rst
+++ /dev/null
@@ -1,8 +0,0 @@
-Bug planner
------------
-
-This is a 2D planning with Bug algorithm.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BugPlanner/animation.gif
-
-- `ECE452 Bug Algorithms `_
diff --git a/docs/modules/path_planning/frenet_frame_path/frenet_frame_path_main.rst b/docs/modules/path_planning/frenet_frame_path/frenet_frame_path_main.rst
deleted file mode 100644
index e9d9041e0e..0000000000
--- a/docs/modules/path_planning/frenet_frame_path/frenet_frame_path_main.rst
+++ /dev/null
@@ -1,20 +0,0 @@
-Optimal Trajectory in a Frenet Frame
-------------------------------------
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif
-
-This is optimal trajectory generation in a Frenet Frame.
-
-The cyan line is the target course and black crosses are obstacles.
-
-The red line is predicted path.
-
-Ref:
-
-- `Optimal Trajectory Generation for Dynamic Street Scenarios in a
- Frenet
- Frame `__
-
-- `Optimal trajectory generation for dynamic street scenarios in a
- Frenet Frame `__
-
diff --git a/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst b/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
deleted file mode 100644
index 81e565888e..0000000000
--- a/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst
+++ /dev/null
@@ -1,17 +0,0 @@
-Reeds Shepp planning
---------------------
-
-A sample code with Reeds Shepp path planning.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true
-
-Ref:
-
-- `15.3.2 Reeds-Shepp
- Curves `__
-
-- `optimal paths for a car that goes both forwards and
- backwards `__
-
-- `ghliu/pyReedsShepp: Implementation of Reeds Shepp
- curve. `__
diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst b/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
deleted file mode 100644
index 68ea9c88b2..0000000000
--- a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst
+++ /dev/null
@@ -1,13 +0,0 @@
-.. _linearquadratic-regulator-(lqr)-speed-and-steering-control:
-
-Linear–quadratic regulator (LQR) speed and steering control
------------------------------------------------------------
-
-Path tracking simulation with LQR speed and steering control.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif
-
-References:
-~~~~~~~~~~~
-
-- `Towards fully autonomous driving: Systems and algorithms `__
diff --git a/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst b/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst
deleted file mode 100644
index bf6d6b5854..0000000000
--- a/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst
+++ /dev/null
@@ -1,14 +0,0 @@
-.. _linearquadratic-regulator-(lqr)-steering-control:
-
-Linear–quadratic regulator (LQR) steering control
--------------------------------------------------
-
-Path tracking simulation with LQR steering control and PID speed
-control.
-
-.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif
-
-References:
-~~~~~~~~~~~
-- `ApolloAuto/apollo: An open autonomous driving platform `_
-
diff --git a/docs/modules/path_tracking/path_tracking_main.rst b/docs/modules/path_tracking/path_tracking_main.rst
deleted file mode 100644
index 504ba08795..0000000000
--- a/docs/modules/path_tracking/path_tracking_main.rst
+++ /dev/null
@@ -1,16 +0,0 @@
-.. _path_tracking:
-
-Path Tracking
-=============
-
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- pure_pursuit_tracking/pure_pursuit_tracking
- stanley_control/stanley_control
- rear_wheel_feedback_control/rear_wheel_feedback_control
- lqr_steering_control/lqr_steering_control
- lqr_speed_and_steering_control/lqr_speed_and_steering_control
- model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control
- cgmres_nmpc/cgmres_nmpc
diff --git a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif
deleted file mode 100644
index 2fabeaafc9..0000000000
Binary files a/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif and /dev/null differ
diff --git a/docs/modules/slam/slam_main.rst b/docs/modules/slam/slam_main.rst
deleted file mode 100644
index 86befa6e35..0000000000
--- a/docs/modules/slam/slam_main.rst
+++ /dev/null
@@ -1,16 +0,0 @@
-.. _slam:
-
-SLAM
-====
-
-Simultaneous Localization and Mapping(SLAM) examples
-
-.. toctree::
- :maxdepth: 2
- :caption: Contents
-
- iterative_closest_point_matching/iterative_closest_point_matching
- ekf_slam/ekf_slam
- FastSLAM1/FastSLAM1
- FastSLAM2/FastSLAM2
- graph_slam/graph_slam
diff --git a/requirements/environment.yml b/requirements/environment.yml
index 13dfa29f66..023a3d75bf 100644
--- a/requirements/environment.yml
+++ b/requirements/environment.yml
@@ -2,7 +2,7 @@ name: python_robotics
channels:
- conda-forge
dependencies:
- - python=3.11
+ - python=3.13
- pip
- scipy
- numpy
diff --git a/requirements/requirements.txt b/requirements/requirements.txt
index 2fab6f69c8..050bd1e57c 100644
--- a/requirements/requirements.txt
+++ b/requirements/requirements.txt
@@ -1,8 +1,9 @@
-numpy == 1.25.0
-scipy == 1.11.1
-matplotlib == 3.7.1
-cvxpy == 1.3.2
-pytest == 7.4.0 # For unit test
-pytest-xdist == 3.3.1 # For unit test
-mypy == 1.4.1 # For unit test
-ruff == 0.0.276 # For unit test
+numpy == 2.2.4
+scipy == 1.15.2
+matplotlib == 3.10.1
+cvxpy == 1.7.3
+ecos == 2.0.14
+pytest == 8.4.2 # For unit test
+pytest-xdist == 3.8.0 # For unit test
+mypy == 1.18.2 # For unit test
+ruff == 0.13.2 # For unit test
diff --git a/ruff.toml b/ruff.toml
index 578864b33e..d76b715a06 100644
--- a/ruff.toml
+++ b/ruff.toml
@@ -5,8 +5,8 @@ ignore = ["E501", "E741", "E402"]
exclude = [
]
-# Assume Python 3.11
-target-version = "py311"
+# Assume Python 3.13
+target-version = "py313"
[per-file-ignores]
@@ -15,4 +15,4 @@ target-version = "py311"
max-complexity = 10
[pydocstyle]
-convention = "numpy"
\ No newline at end of file
+convention = "numpy"
diff --git a/runtests.sh b/runtests.sh
index a2a683e571..c4460c8aa7 100755
--- a/runtests.sh
+++ b/runtests.sh
@@ -1,8 +1,9 @@
#!/usr/bin/env bash
+cd "$(dirname "$0")" || exit 1
echo "Run test suites! "
# === pytest based test runner ===
# -Werror: warning as error
# --durations=0: show ranking of test durations
# -l (--showlocals); show local variables when test failed
-pytest -n auto tests -l -Werror --durations=0
+pytest tests -l -Werror --durations=0
diff --git a/tests/conftest.py b/tests/conftest.py
index 3485fe8150..b707b22d00 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -10,4 +10,4 @@
def run_this_test(file):
- pytest.main([os.path.abspath(file)])
+ pytest.main(args=["-W", "error", "-Werror", "--pythonwarnings=error", os.path.abspath(file)])
diff --git a/tests/test_batch_informed_rrt_star.py b/tests/test_batch_informed_rrt_star.py
index 3ad78bdb23..cf0a9827a8 100644
--- a/tests/test_batch_informed_rrt_star.py
+++ b/tests/test_batch_informed_rrt_star.py
@@ -1,7 +1,7 @@
import random
import conftest
-from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m
+from PathPlanning.BatchInformedRRTStar import batch_informed_rrt_star as m
def test_1():
diff --git a/tests/test_behavior_tree.py b/tests/test_behavior_tree.py
new file mode 100644
index 0000000000..0898690448
--- /dev/null
+++ b/tests/test_behavior_tree.py
@@ -0,0 +1,207 @@
+import pytest
+import conftest
+
+from MissionPlanning.BehaviorTree.behavior_tree import (
+ BehaviorTreeFactory,
+ Status,
+ ActionNode,
+)
+
+
+def test_sequence_node1():
+ xml_string = """
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+ bt.tick()
+ bt.tick()
+ assert bt.root.status == Status.FAILURE
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_sequence_node2():
+ xml_string = """
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick_while_running()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_selector_node1():
+ xml_string = """
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.FAILURE
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_selector_node2():
+ xml_string = """
+
+
+
+
+
+
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick_while_running()
+ assert bt.root.status == Status.FAILURE
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+
+
+def test_while_do_else_node():
+ xml_string = """
+
+
+
+
+
+ """
+
+ class CountNode(ActionNode):
+ def __init__(self, name, count_threshold):
+ super().__init__(name)
+ self.count = 0
+ self.count_threshold = count_threshold
+
+ def tick(self):
+ self.count += 1
+ if self.count >= self.count_threshold:
+ return Status.FAILURE
+ else:
+ return Status.SUCCESS
+
+ bt_factory = BehaviorTreeFactory()
+ bt_factory.register_node_builder(
+ "Count",
+ lambda node: CountNode(
+ node.attrib.get("name", CountNode.__name__),
+ int(node.attrib["count_threshold"]),
+ ),
+ )
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is Status.SUCCESS
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is Status.SUCCESS
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_node_children():
+ # ControlNode Must have children
+ xml_string = """
+
+
+ """
+ bt_factory = BehaviorTreeFactory()
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # DecoratorNode Must have child
+ xml_string = """
+
+
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # DecoratorNode Must have only one child
+ xml_string = """
+
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # ActionNode Must have no children
+ xml_string = """
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # WhileDoElse Must have exactly 2 or 3 children
+ xml_string = """
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+
+ xml_string = """
+
+
+
+
+
+
+ """
+ with pytest.raises(ValueError):
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_catmull_rom_spline.py b/tests/test_catmull_rom_spline.py
new file mode 100644
index 0000000000..41a73588c3
--- /dev/null
+++ b/tests/test_catmull_rom_spline.py
@@ -0,0 +1,16 @@
+import conftest
+from PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path import catmull_rom_spline
+
+def test_catmull_rom_spline():
+ way_points = [[0, 0], [1, 2], [2, 0], [3, 3]]
+ num_points = 100
+
+ spline_x, spline_y = catmull_rom_spline(way_points, num_points)
+
+ assert spline_x.size > 0, "Spline X coordinates should not be empty"
+ assert spline_y.size > 0, "Spline Y coordinates should not be empty"
+
+ assert spline_x.shape == spline_y.shape, "Spline X and Y coordinates should have the same shape"
+
+if __name__ == '__main__':
+ conftest.run_this_test(__file__)
diff --git a/tests/test_codestyle.py b/tests/test_codestyle.py
index a7d11d270f..55e558c184 100644
--- a/tests/test_codestyle.py
+++ b/tests/test_codestyle.py
@@ -54,7 +54,7 @@ def run_ruff(files, fix):
return 0, ""
args = ['--fix'] if fix else []
res = subprocess.run(
- ['ruff', f'--config={CONFIG}'] + args + files,
+ ['ruff', 'check', f'--config={CONFIG}'] + args + files,
stdout=subprocess.PIPE,
encoding='utf-8'
)
diff --git a/tests/test_distance_map.py b/tests/test_distance_map.py
new file mode 100644
index 0000000000..df6e394e2c
--- /dev/null
+++ b/tests/test_distance_map.py
@@ -0,0 +1,118 @@
+import conftest # noqa
+import numpy as np
+from Mapping.DistanceMap import distance_map as m
+
+
+def test_compute_sdf():
+ """Test the computation of Signed Distance Field (SDF)"""
+ # Create a simple obstacle map for testing
+ obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]])
+
+ sdf = m.compute_sdf(obstacles)
+
+ # Verify basic properties of SDF
+ assert sdf.shape == obstacles.shape, "SDF should have the same shape as input map"
+ assert np.all(np.isfinite(sdf)), "SDF should not contain infinite values"
+
+ # Verify SDF value is negative at obstacle position
+ assert sdf[1, 1] < 0, "SDF value should be negative at obstacle position"
+
+ # Verify SDF value is positive in free space
+ assert sdf[0, 0] > 0, "SDF value should be positive in free space"
+
+
+def test_compute_udf():
+ """Test the computation of Unsigned Distance Field (UDF)"""
+ # Create obstacle map for testing
+ obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]])
+
+ udf = m.compute_udf(obstacles)
+
+ # Verify basic properties of UDF
+ assert udf.shape == obstacles.shape, "UDF should have the same shape as input map"
+ assert np.all(np.isfinite(udf)), "UDF should not contain infinite values"
+ assert np.all(udf >= 0), "All UDF values should be non-negative"
+
+ # Verify UDF value is 0 at obstacle position
+ assert np.abs(udf[1, 1]) < 1e-10, "UDF value should be 0 at obstacle position"
+
+ # Verify UDF value is 1 for adjacent cells
+ assert np.abs(udf[0, 1] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+ assert np.abs(udf[1, 0] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+ assert np.abs(udf[1, 2] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+ assert np.abs(udf[2, 1] - 1.0) < 1e-10, (
+ "UDF value should be 1 for cells adjacent to obstacle"
+ )
+
+
+def test_dt():
+ """Test the computation of 1D distance transform"""
+ # Create test data
+ d = np.array([m.INF, 0, m.INF])
+ m.dt(d)
+
+ # Verify distance transform results
+ assert np.all(np.isfinite(d)), (
+ "Distance transform result should not contain infinite values"
+ )
+ assert d[1] == 0, "Distance at obstacle position should be 0"
+ assert d[0] == 1, "Distance at adjacent position should be 1"
+ assert d[2] == 1, "Distance at adjacent position should be 1"
+
+
+def test_compute_sdf_empty():
+ """Test SDF computation with empty map"""
+ # Test with empty map (no obstacles)
+ empty_map = np.zeros((5, 5))
+ sdf = m.compute_sdf(empty_map)
+
+ assert np.all(sdf > 0), "All SDF values should be positive for empty map"
+ assert sdf.shape == empty_map.shape, "Output shape should match input shape"
+
+
+def test_compute_sdf_full():
+ """Test SDF computation with fully occupied map"""
+ # Test with fully occupied map
+ full_map = np.ones((5, 5))
+ sdf = m.compute_sdf(full_map)
+
+ assert np.all(sdf < 0), "All SDF values should be negative for fully occupied map"
+ assert sdf.shape == full_map.shape, "Output shape should match input shape"
+
+
+def test_compute_udf_invalid_input():
+ """Test UDF computation with invalid input values"""
+ # Test with invalid values (not 0 or 1)
+ invalid_map = np.array([[0, 2, 0], [0, -1, 0], [0, 0.5, 0]])
+
+ try:
+ m.compute_udf(invalid_map)
+ assert False, "Should raise ValueError for invalid input values"
+ except ValueError:
+ pass
+
+
+def test_compute_udf_empty():
+ """Test UDF computation with empty map"""
+ # Test with empty map
+ empty_map = np.zeros((5, 5))
+ udf = m.compute_udf(empty_map)
+
+ assert np.all(udf > 0), "All UDF values should be positive for empty map"
+ assert np.all(np.isfinite(udf)), "UDF should not contain infinite values"
+
+
+def test_main():
+ """Test the execution of main function"""
+ m.ENABLE_PLOT = False
+ m.main()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_elastic_bands.py b/tests/test_elastic_bands.py
new file mode 100644
index 0000000000..ad4e13af1a
--- /dev/null
+++ b/tests/test_elastic_bands.py
@@ -0,0 +1,23 @@
+import conftest
+import numpy as np
+from PathPlanning.ElasticBands.elastic_bands import ElasticBands
+
+
+def test_1():
+ path = np.load("PathPlanning/ElasticBands/path.npy")
+ obstacles_points = np.load("PathPlanning/ElasticBands/obstacles.npy")
+ obstacles = np.zeros((500, 500))
+ for x, y in obstacles_points:
+ size = 30 # Side length of the square
+ half_size = size // 2
+ x_start = max(0, x - half_size)
+ x_end = min(obstacles.shape[0], x + half_size)
+ y_start = max(0, y - half_size)
+ y_end = min(obstacles.shape[1], y + half_size)
+ obstacles[x_start:x_end, y_start:y_end] = 1
+ elastic_bands = ElasticBands(path, obstacles)
+ elastic_bands.update_bubbles()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_frenet_optimal_trajectory.py b/tests/test_frenet_optimal_trajectory.py
index 72bb7a8341..b8761ff4a6 100644
--- a/tests/test_frenet_optimal_trajectory.py
+++ b/tests/test_frenet_optimal_trajectory.py
@@ -1,12 +1,48 @@
import conftest
from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m
+from PathPlanning.FrenetOptimalTrajectory.frenet_optimal_trajectory import (
+ LateralMovement,
+ LongitudinalMovement,
+)
-def test1():
+def default_scenario_test():
m.show_animation = False
m.SIM_LOOP = 5
m.main()
-if __name__ == '__main__':
+def high_speed_and_merging_and_stopping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.HIGH_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.MERGING_AND_STOPPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+def high_speed_and_velocity_keeping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.HIGH_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.VELOCITY_KEEPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+def low_speed_and_velocity_keeping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.LOW_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.VELOCITY_KEEPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+def low_speed_and_merging_and_stopping_scenario_test():
+ m.show_animation = False
+ m.LATERAL_MOVEMENT = LateralMovement.LOW_SPEED
+ m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.MERGING_AND_STOPPING
+ m.SIM_LOOP = 5
+ m.main()
+
+
+if __name__ == "__main__":
conftest.run_this_test(__file__)
diff --git a/tests/test_grid_map_lib.py b/tests/test_grid_map_lib.py
index 92ca67e297..670b357ad3 100644
--- a/tests/test_grid_map_lib.py
+++ b/tests/test_grid_map_lib.py
@@ -25,5 +25,16 @@ def test_polygon_set():
1.0, inside=False)
+def test_xy_and_grid_index_conversion():
+ grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
+
+ for x_ind in range(grid_map.width):
+ for y_ind in range(grid_map.height):
+ grid_ind = grid_map.calc_grid_index_from_xy_index(x_ind, y_ind)
+ x_ind_2, y_ind_2 = grid_map.calc_xy_index_from_grid_index(grid_ind)
+ assert x_ind == x_ind_2
+ assert y_ind == y_ind_2
+
+
if __name__ == '__main__':
conftest.run_this_test(__file__)
diff --git a/tests/test_inverted_pendulum_lqr_control.py b/tests/test_inverted_pendulum_lqr_control.py
index cbbabb93b1..62afda71c3 100644
--- a/tests/test_inverted_pendulum_lqr_control.py
+++ b/tests/test_inverted_pendulum_lqr_control.py
@@ -1,5 +1,5 @@
import conftest
-from Control.inverted_pendulum import inverted_pendulum_lqr_control as m
+from InvertedPendulum import inverted_pendulum_lqr_control as m
def test_1():
diff --git a/tests/test_inverted_pendulum_mpc_control.py b/tests/test_inverted_pendulum_mpc_control.py
index 800aefd7d5..94859c2e0a 100644
--- a/tests/test_inverted_pendulum_mpc_control.py
+++ b/tests/test_inverted_pendulum_mpc_control.py
@@ -1,6 +1,6 @@
import conftest
-from Control.inverted_pendulum import inverted_pendulum_mpc_control as m
+from InvertedPendulum import inverted_pendulum_mpc_control as m
def test1():
diff --git a/tests/test_iterative_closest_point.py b/tests/test_iterative_closest_point.py
index 3e726b5649..cdfa89cc55 100644
--- a/tests/test_iterative_closest_point.py
+++ b/tests/test_iterative_closest_point.py
@@ -1,5 +1,5 @@
import conftest
-from SLAM.iterative_closest_point import iterative_closest_point as m
+from SLAM.ICPMatching import icp_matching as m
def test_1():
diff --git a/tests/test_move_to_pose.py b/tests/test_move_to_pose.py
index 8bc11a8d24..e06d801555 100644
--- a/tests/test_move_to_pose.py
+++ b/tests/test_move_to_pose.py
@@ -1,13 +1,81 @@
+import itertools
+import numpy as np
import conftest # Add root path to sys.path
-from Control.move_to_pose import move_to_pose as m
+from PathTracking.move_to_pose import move_to_pose as m
-def test_1():
+def test_random():
m.show_animation = False
m.main()
-def test_2():
+def test_stability():
+ """
+ This unit test tests the move_to_pose.py program for stability
+ """
+ m.show_animation = False
+ x_start = 5
+ y_start = 5
+ theta_start = 0
+ x_goal = 1
+ y_goal = 4
+ theta_goal = 0
+ _, _, v_traj, w_traj = m.move_to_pose(
+ x_start, y_start, theta_start, x_goal, y_goal, theta_goal
+ )
+
+ def v_is_change(current, previous):
+ return abs(current - previous) > m.MAX_LINEAR_SPEED
+
+ def w_is_change(current, previous):
+ return abs(current - previous) > m.MAX_ANGULAR_SPEED
+
+ # Check if the speed is changing too much
+ window_size = 10
+ count_threshold = 4
+ v_change = [v_is_change(v_traj[i], v_traj[i - 1]) for i in range(1, len(v_traj))]
+ w_change = [w_is_change(w_traj[i], w_traj[i - 1]) for i in range(1, len(w_traj))]
+ for i in range(len(v_change) - window_size + 1):
+ v_window = v_change[i : i + window_size]
+ w_window = w_change[i : i + window_size]
+
+ v_unstable = sum(v_window) > count_threshold
+ w_unstable = sum(w_window) > count_threshold
+
+ assert not v_unstable, (
+ f"v_unstable in window [{i}, {i + window_size}], unstable count: {sum(v_window)}"
+ )
+ assert not w_unstable, (
+ f"w_unstable in window [{i}, {i + window_size}], unstable count: {sum(w_window)}"
+ )
+
+
+def test_reach_goal():
+ """
+ This unit test tests the move_to_pose.py program for reaching the goal
+ """
+ m.show_animation = False
+ x_start = 5
+ y_start = 5
+ theta_start_list = [0, np.pi / 2, np.pi, 3 * np.pi / 2]
+ x_goal_list = [0, 5, 10]
+ y_goal_list = [0, 5, 10]
+ theta_goal = 0
+ for theta_start, x_goal, y_goal in itertools.product(
+ theta_start_list, x_goal_list, y_goal_list
+ ):
+ x_traj, y_traj, _, _ = m.move_to_pose(
+ x_start, y_start, theta_start, x_goal, y_goal, theta_goal
+ )
+ x_diff = x_goal - x_traj[-1]
+ y_diff = y_goal - y_traj[-1]
+ rho = np.hypot(x_diff, y_diff)
+ assert rho < 0.001, (
+ f"start:[{x_start}, {y_start}, {theta_start}], goal:[{x_goal}, {y_goal}, {theta_goal}], rho: {rho} is too large"
+ )
+
+
+def test_max_speed():
"""
This unit test tests the move_to_pose.py program for a MAX_LINEAR_SPEED and
MAX_ANGULAR_SPEED
@@ -18,5 +86,5 @@ def test_2():
m.main()
-if __name__ == '__main__':
+if __name__ == "__main__":
conftest.run_this_test(__file__)
diff --git a/tests/test_move_to_pose_robot.py b/tests/test_move_to_pose_robot.py
index a93b44d198..7a82f98556 100644
--- a/tests/test_move_to_pose_robot.py
+++ b/tests/test_move_to_pose_robot.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from Control.move_to_pose import move_to_pose as m
+from PathTracking.move_to_pose import move_to_pose as m
def test_1():
diff --git a/tests/test_mypy_type_check.py b/tests/test_mypy_type_check.py
index 07afb40afd..6b933c1011 100644
--- a/tests/test_mypy_type_check.py
+++ b/tests/test_mypy_type_check.py
@@ -7,12 +7,12 @@
"AerialNavigation",
"ArmNavigation",
"Bipedal",
- "Control",
"Localization",
"Mapping",
"PathPlanning",
"PathTracking",
"SLAM",
+ "InvertedPendulum"
]
diff --git a/tests/test_priority_based_planner.py b/tests/test_priority_based_planner.py
new file mode 100644
index 0000000000..e2ff602d88
--- /dev/null
+++ b/tests/test_priority_based_planner.py
@@ -0,0 +1,39 @@
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ NodePath,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal
+from PathPlanning.TimeBasedPathPlanning import PriorityBasedPlanner as m
+from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
+import numpy as np
+import conftest
+
+
+def test_1():
+ grid_side_length = 21
+
+ start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+ obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
+
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ num_obstacles=250,
+ obstacle_avoid_points=obstacle_avoid_points,
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ m.show_animation = False
+
+ start_and_goals: list[StartAndGoal]
+ paths: list[NodePath]
+ start_and_goals, paths = m.PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, False)
+
+ # All paths should start at the specified position and reach the goal
+ for i, start_and_goal in enumerate(start_and_goals):
+ assert paths[i].path[0].position == start_and_goal.start
+ assert paths[i].path[-1].position == start_and_goal.goal
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_pure_pursuit.py b/tests/test_pure_pursuit.py
index 0e0b83bf6c..89c1829514 100644
--- a/tests/test_pure_pursuit.py
+++ b/tests/test_pure_pursuit.py
@@ -6,6 +6,10 @@ def test1():
m.show_animation = False
m.main()
+def test_backward():
+ m.show_animation = False
+ m.is_reverse_mode = True
+ m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)
diff --git a/tests/test_raycasting_grid_map.py b/tests/test_ray_casting_grid_map.py
similarity index 71%
rename from tests/test_raycasting_grid_map.py
rename to tests/test_ray_casting_grid_map.py
index f08ae9277e..2d192c9310 100644
--- a/tests/test_raycasting_grid_map.py
+++ b/tests/test_ray_casting_grid_map.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from Mapping.raycasting_grid_map import raycasting_grid_map as m
+from Mapping.ray_casting_grid_map import ray_casting_grid_map as m
def test1():
diff --git a/tests/test_rear_wheel_feedback.py b/tests/test_rear_wheel_feedback.py
index 895eb188b3..eccde52358 100644
--- a/tests/test_rear_wheel_feedback.py
+++ b/tests/test_rear_wheel_feedback.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from PathTracking.rear_wheel_feedback import rear_wheel_feedback as m
+from PathTracking.rear_wheel_feedback_control import rear_wheel_feedback_control as m
def test1():
diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py
index 20d4916f96..11157aa57a 100644
--- a/tests/test_rrt_star_reeds_shepp.py
+++ b/tests/test_rrt_star_reeds_shepp.py
@@ -32,16 +32,14 @@ def test2():
# + 0.00000000000001 for acceptable errors arising from the planning process
assert m.math.dist(path[i][0:2], path[i+1][0:2]) < step_size + 0.00000000000001
-def test3():
+def test_too_big_step_size():
step_size = 20
rrt_star_reeds_shepp = m.RRTStarReedsShepp(start, goal,
obstacleList, [-2.0, 15.0],
max_iter=100, step_size=step_size)
rrt_star_reeds_shepp.set_random_seed(seed=8)
path = rrt_star_reeds_shepp.planning(animation=False)
- for i in range(len(path)-1):
- # + 0.00000000000001 for acceptable errors arising from the planning process
- assert m.math.dist(path[i][0:2], path[i+1][0:2]) < step_size + 0.00000000000001
+ assert path is None
if __name__ == '__main__':
diff --git a/tests/test_rrt_with_pathsmoothing_radius.py b/tests/test_rrt_with_pathsmoothing_radius.py
new file mode 100644
index 0000000000..a1159255b5
--- /dev/null
+++ b/tests/test_rrt_with_pathsmoothing_radius.py
@@ -0,0 +1,48 @@
+import conftest
+import math
+
+from PathPlanning.RRT import rrt_with_pathsmoothing as rrt_module
+
+def test_smoothed_path_safety():
+ # Define test environment
+ obstacle_list = [
+ (5, 5, 1.0),
+ (3, 6, 2.0),
+ (3, 8, 2.0),
+ (3, 10, 2.0),
+ (7, 5, 2.0),
+ (9, 5, 2.0)
+ ]
+ robot_radius = 0.5
+
+ # Disable animation for testing
+ rrt_module.show_animation = False
+
+ # Create RRT planner
+ rrt = rrt_module.RRT(
+ start=[0, 0],
+ goal=[6, 10],
+ rand_area=[-2, 15],
+ obstacle_list=obstacle_list,
+ robot_radius=robot_radius
+ )
+
+ # Run RRT
+ path = rrt.planning(animation=False)
+
+ # Smooth the path
+ smoothed = rrt_module.path_smoothing(path, max_iter=1000,
+ obstacle_list=obstacle_list,
+ robot_radius=robot_radius)
+
+ # Check if all points on the smoothed path are safely distant from obstacles
+ for x, y in smoothed:
+ for ox, oy, obs_radius in obstacle_list:
+ d = math.hypot(x - ox, y - oy)
+ min_safe_dist = obs_radius + robot_radius
+ assert d > min_safe_dist, \
+ f"Point ({x:.2f}, {y:.2f}) too close to obstacle at ({ox}, {oy})"
+
+
+if __name__ == '__main__':
+ conftest.run_this_test(__file__)
diff --git a/tests/test_safe_interval_path_planner.py b/tests/test_safe_interval_path_planner.py
new file mode 100644
index 0000000000..bbcd4e427c
--- /dev/null
+++ b/tests/test_safe_interval_path_planner.py
@@ -0,0 +1,31 @@
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning import SafeInterval as m
+import numpy as np
+import conftest
+
+
+def test_1():
+ start = Position(1, 11)
+ goal = Position(19, 19)
+ grid_side_length = 21
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ m.show_animation = False
+ path = m.SafeIntervalPathPlanner.plan(grid, start, goal)
+
+ # path should have 31 entries
+ assert len(path.path) == 31
+
+ # path should end at the goal
+ assert path.path[-1].position == goal
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py
new file mode 100644
index 0000000000..1194c61d2e
--- /dev/null
+++ b/tests/test_space_time_astar.py
@@ -0,0 +1,32 @@
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+ Grid,
+ ObstacleArrangement,
+ Position,
+)
+from PathPlanning.TimeBasedPathPlanning import SpaceTimeAStar as m
+import numpy as np
+import conftest
+
+
+def test_1():
+ start = Position(1, 11)
+ goal = Position(19, 19)
+ grid_side_length = 21
+ grid = Grid(
+ np.array([grid_side_length, grid_side_length]),
+ obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+ )
+
+ m.show_animation = False
+ path = m.SpaceTimeAStar.plan(grid, start, goal)
+
+ # path should have 28 entries
+ assert len(path.path) == 31
+
+ # path should end at the goal
+ assert path.path[-1].position == goal
+
+ assert path.expanded_node_count < 1000
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/tests/test_stanley_controller.py b/tests/test_stanley_controller.py
index a1d8073764..bd590cb51a 100644
--- a/tests/test_stanley_controller.py
+++ b/tests/test_stanley_controller.py
@@ -1,5 +1,5 @@
import conftest # Add root path to sys.path
-from PathTracking.stanley_controller import stanley_controller as m
+from PathTracking.stanley_control import stanley_control as m
def test1():
diff --git a/tests/test_state_machine.py b/tests/test_state_machine.py
new file mode 100644
index 0000000000..e36a8120fd
--- /dev/null
+++ b/tests/test_state_machine.py
@@ -0,0 +1,51 @@
+import conftest
+
+from MissionPlanning.StateMachine.state_machine import StateMachine
+
+
+def test_transition():
+ sm = StateMachine("state_machine")
+ sm.add_transition(src_state="idle", event="start", dst_state="running")
+ sm.set_current_state("idle")
+ sm.process("start")
+ assert sm.get_current_state().name == "running"
+
+
+def test_guard():
+ class Model:
+ def can_start(self):
+ return False
+
+ sm = StateMachine("state_machine", Model())
+ sm.add_transition(
+ src_state="idle", event="start", dst_state="running", guard="can_start"
+ )
+ sm.set_current_state("idle")
+ sm.process("start")
+ assert sm.get_current_state().name == "idle"
+
+
+def test_action():
+ class Model:
+ def on_start(self):
+ self.start_called = True
+
+ model = Model()
+ sm = StateMachine("state_machine", model)
+ sm.add_transition(
+ src_state="idle", event="start", dst_state="running", action="on_start"
+ )
+ sm.set_current_state("idle")
+ sm.process("start")
+ assert model.start_called
+
+
+def test_plantuml():
+ sm = StateMachine("state_machine")
+ sm.add_transition(src_state="idle", event="start", dst_state="running")
+ sm.set_current_state("idle")
+ assert sm.generate_plantuml()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
diff --git a/users_comments.md b/users_comments.md
index 66e8b4022e..c814e74c4b 100644
--- a/users_comments.md
+++ b/users_comments.md
@@ -6,24 +6,18 @@ This is an electric wheelchair control demo by [Katsushun89](https://github.com/
[WHILL Model CR](https://whill.jp/model-cr) is the control target, [M5Stack](https://m5stack.com/) is used for the controller, and [toio](https://toio.io/) is used for the control input device.
-[Move to a Pose Control — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/control/move_to_a_pose_control/move_to_a_pose_control.html) is used for its control algorithm ([code link](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/move_to_pose/move_to_pose.py)).
+[Move to a Pose Control — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/move_to_a_pose/move_to_a_pose.html) is used for its control algorithm ([code link](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/move_to_pose/move_to_pose.py)).
-Ref:
+Reference:
- [toioと同じように動くWHILL Model CR (in Japanese)](https://qiita.com/KatsuShun89/items/68fde7544ecfe36096d2)
# Educational users
-This is a list of users who are using PythonRobotics for education.
-
-If you found other users, please make an issue to let me know.
-
-- [CSCI/ARTI 4530/6530: Introduction to Robotics (Fall 2018), University of Georgia ](http://cobweb.cs.uga.edu/~ramviyas/csci_x530.html)
-
-- [CIT Modules & Programmes \- COMP9073 \- Automation with Python](https://courses.cit.ie/index.cfm/page/module/moduleId/14416)
+If you found users who are using PythonRobotics for education, please make an issue to let me know.
# Stargazers location map
@@ -107,7 +101,7 @@ Thank you!
---
-Dear AtsushiSakai, Thanks a lot for the wonderful collection of projects.It was really useful. I really appreciate your time in maintaing and building this repository.It will be really great if I get a chance to meet you in person sometime.I got really inspired looking at your work. All the very best for all your future endeavors! Thanks once again,
+Dear AtsushiSakai, Thanks a lot for the wonderful collection of projects.It was really useful. I really appreciate your time in maintaining and building this repository.It will be really great if I get a chance to meet you in person sometime.I got really inspired looking at your work. All the very best for all your future endeavors! Thanks once again,
---Ezhil Bharathi
@@ -169,7 +163,7 @@ so kind of you can you make videos of it.
---
-Dear AtshshiSakai, You are very wise and smart that you created this library for students wanting to learn Probabilistic Robotics and actually perform robot control. Hats off to you and your work. I am also reading your book titled : Python Robotics
+Dear AtsushiSakai, You are very wise and smart that you created this library for students wanting to learn Probabilistic Robotics and actually perform robot control. Hats off to you and your work. I am also reading your book titled : Python Robotics
--Himanshu
@@ -269,7 +263,7 @@ Hey Atsushi We are working on a multiagent simulation game and this project
---
-Thanks a lot for this amazing set of very useful librarires!
+Thanks a lot for this amazing set of very useful libraries!
--Razvan Viorel Mihai
@@ -281,7 +275,7 @@ Dear Atsushi Sakai,
This is probably one of the best open-source roboti
---
-hanks frnd, for ur contibusion
+Thanks friend, for your contribution
—--
@@ -386,14 +380,14 @@ Dear Atsushi Sakai, Thank you so much for creating PythonRobotics and docume
1. B. Blaga, M. Deac, R. W. Y. Al-doori, M. Negru and R. Dǎnescu, "Miniature Autonomous Vehicle Development on Raspberry Pi," 2018 IEEE 14th International Conference on Intelligent Computer Communication and Processing (ICCP), Cluj-Napoca, Romania, 2018, pp. 229-236.
doi: 10.1109/ICCP.2018.8516589
keywords: {Automobiles;Task analysis;Autonomous vehicles;Path planning;Global Positioning System;Cameras;miniature autonomous vehicle;path planning;navigation;parking assist;lane detection and tracking;traffic sign recognition},
-URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8516589&isnumber=8516425
+URL: https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8516589&isnumber=8516425
2. Peggy (Yuchun) Wang and Caitlin Hogan, "Path Planning with Dynamic Obstacle Avoidance for a Jumping-Enabled Robot", AA228/CS238 class report, Department of Computer Science, Stanford University, URL: https://web.stanford.edu/class/aa228/reports/2018/final113.pdf
-3. Welburn, E, Hakim Khalili, H, Gupta, A, Watson, S & Carrasco, J 2019, A Navigational System for Quadcopter Remote Inspection of Offshore Substations. in The Fifteenth International Conference on Autonomic and Autonomous Systems 2019. URL:https://www.research.manchester.ac.uk/portal/files/107169964/ICAS19_A_Navigational_System_for_Quadcopter_Remote_Inspection_of_Offshore_Substations.pdf
+3. Welburn, E, Hakim Khalili, H, Gupta, A, Watson, S & Carrasco, J 2019, A Navigational System for Quadcopter Remote Inspection of Offshore Substations. in The Fifteenth International Conference on Autonomic and Autonomous Systems 2019. URL:https://research.manchester.ac.uk/portal/files/107169964/ICAS19_A_Navigational_System_for_Quadcopter_Remote_Inspection_of_Offshore_Substations.pdf
4. E. Horváth, C. Hajdu, C. Radu and Á. Ballagi, "Range Sensor-based Occupancy Grid Mapping with Signatures," 2019 20th International Carpathian Control Conference (ICCC), Krakow-Wieliczka, Poland, 2019, pp. 1-5.
-URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8765684&isnumber=8765679
+URL: https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8765684&isnumber=8765679
5. Josie Hughes, Masaru Shimizu, and Arnoud Visser, "A Review of Robot Rescue Simulation Platforms for Robotics Education"
URL: https://2019.robocup.org/downloads/program/HughesEtAl2019.pdf
@@ -408,7 +402,7 @@ URL: https://arxiv.org/abs/1910.01557
URL: https://pdfs.semanticscholar.org/5c06/f3cb9542a51e1bf1a32523c1bc7fea6cecc5.pdf
9. Brijen Thananjeyan, et al. "ABC-LMPC: Safe Sample-Based Learning MPC for Stochastic Nonlinear Dynamical Systems with Adjustable Boundary Conditions"
-URL: https://arxiv.org/pdf/2003.01410.pdf
+URL: https://arxiv.org/pdf/2003.01410
# Others
diff --git a/utils/plot.py b/utils/plot.py
index d4bbe29ffd..eb5aa7ad04 100644
--- a/utils/plot.py
+++ b/utils/plot.py
@@ -9,6 +9,69 @@
from mpl_toolkits.mplot3d.proj3d import proj_transform
from mpl_toolkits.mplot3d import Axes3D
+from utils.angle import rot_mat_2d
+
+
+def plot_covariance_ellipse(x, y, cov, chi2=3.0, color="-r", ax=None):
+ """
+ This function plots an ellipse that represents a covariance matrix. The ellipse is centered at (x, y) and its shape, size and rotation are determined by the covariance matrix.
+
+ Parameters:
+ x : (float) The x-coordinate of the center of the ellipse.
+ y : (float) The y-coordinate of the center of the ellipse.
+ cov : (numpy.ndarray) A 2x2 covariance matrix that determines the shape, size, and rotation of the ellipse.
+ chi2 : (float, optional) A scalar value that scales the ellipse size. This value is typically set based on chi-squared distribution quantiles to achieve certain confidence levels (e.g., 3.0 corresponds to ~95% confidence for a 2D Gaussian). Defaults to 3.0.
+ color : (str, optional) The color and line style of the ellipse plot, following matplotlib conventions. Defaults to "-r" (a red solid line).
+ ax : (matplotlib.axes.Axes, optional) The Axes object to draw the ellipse on. If None (default), a new figure and axes are created.
+
+ Returns:
+ None. This function plots the covariance ellipse on the specified axes.
+ """
+ eig_val, eig_vec = np.linalg.eig(cov)
+
+ if eig_val[0] >= eig_val[1]:
+ big_ind = 0
+ small_ind = 1
+ else:
+ big_ind = 1
+ small_ind = 0
+ a = math.sqrt(chi2 * eig_val[big_ind])
+ b = math.sqrt(chi2 * eig_val[small_ind])
+ angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
+ plot_ellipse(x, y, a, b, angle, color=color, ax=ax)
+
+
+def plot_ellipse(x, y, a, b, angle, color="-r", ax=None, **kwargs):
+ """
+ This function plots an ellipse based on the given parameters.
+
+ Parameters
+ ----------
+ x : (float) The x-coordinate of the center of the ellipse.
+ y : (float) The y-coordinate of the center of the ellipse.
+ a : (float) The length of the semi-major axis of the ellipse.
+ b : (float) The length of the semi-minor axis of the ellipse.
+ angle : (float) The rotation angle of the ellipse, in radians.
+ color : (str, optional) The color and line style of the ellipse plot, following matplotlib conventions. Defaults to "-r" (a red solid line).
+ ax : (matplotlib.axes.Axes, optional) The Axes object to draw the ellipse on. If None (default), a new figure and axes are created.
+ **kwargs: Additional keyword arguments to pass to plt.plot or ax.plot.
+
+ Returns
+ ---------
+ None. This function plots the ellipse based on the specified parameters.
+ """
+
+ t = np.arange(0, 2 * math.pi + 0.1, 0.1)
+ px = [a * math.cos(it) for it in t]
+ py = [b * math.sin(it) for it in t]
+ fx = rot_mat_2d(angle) @ (np.array([px, py]))
+ px = np.array(fx[0, :] + x).flatten()
+ py = np.array(fx[1, :] + y).flatten()
+ if ax is None:
+ plt.plot(px, py, color, **kwargs)
+ else:
+ ax.plot(px, py, color, **kwargs)
+
def plot_arrow(x, y, yaw, arrow_length=1.0,
origin_point_plot_style="xr",
@@ -132,7 +195,6 @@ def plot_3d_vector_arrow(ax, p1, p2):
)
-
def plot_triangle(p1, p2, p3, ax):
ax.add_collection3d(art3d.Poly3DCollection([[p1, p2, p3]], color='b'))
@@ -163,3 +225,10 @@ def set_equal_3d_axis(ax, x_lims, y_lims, z_lims):
ax.set_xlim(mid_x - max_range, mid_x + max_range)
ax.set_ylim(mid_y - max_range, mid_y + max_range)
ax.set_zlim(mid_z - max_range, mid_z + max_range)
+
+
+if __name__ == '__main__':
+ plot_ellipse(0, 0, 1, 2, np.deg2rad(15))
+ plt.axis('equal')
+ plt.show()
+
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
## Particle filter localization
@@ -179,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/)
@@ -200,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/)
@@ -249,7 +256,7 @@ It can calculate a rotation matrix, and a translation vector between points and
-Ref:
+Reference
- [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
@@ -268,7 +275,7 @@ Black points are landmarks, blue crosses are estimated landmark positions by Fas
-Ref:
+Reference
- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
@@ -314,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*)
@@ -328,7 +335,7 @@ The animation shows a robot finding its path and rerouting to avoid obstacles as
Refs:
-- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pd)
+- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf)
- [Improved Fast Replanning for Robot Navigation in Unknown Terrain](http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf)
### Potential Field algorithm
@@ -339,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)
@@ -355,11 +362,11 @@ This script is a path planning code with state lattice planning.
This code uses the model predictive trajectory generator to solve boundary problem.
-Ref:
+Reference
-- [Optimal rough terrain trajectory generation for wheeled mobile robots](http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
+- [Optimal rough terrain trajectory generation for wheeled mobile robots](https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
-- [State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments](http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
+- [State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments](https://www.cs.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
### Biased polar sampling
@@ -383,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)
@@ -399,15 +406,15 @@ This is a path planning code with RRT\*
Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.
-Ref:
+Reference
- [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416)
-- [Sampling-based Algorithms for Optimal Motion Planning](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf)
+- [Sampling-based Algorithms for Optimal Motion Planning](https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf&doi=bddbc99f97173430aa49a0ada53ab5bade5902fa)
### RRT\* with reeds-shepp path
-)
+
Path planning for a car robot with RRT\* and reeds shepp path planner.
@@ -419,9 +426,9 @@ A double integrator motion model is used for LQR local planner.
-Ref:
+Reference
-- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](http://lis.csail.mit.edu/pubs/perez-icra12.pdf)
+- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](https://lis.csail.mit.edu/pubs/perez-icra12.pdf)
- [MahanFathi/LQR\-RRTstar: LQR\-RRT\* method is used for random motion planning of a simple pendulum in its phase plot](https://github.com/MahanFathi/LQR-RRTstar)
@@ -434,9 +441,9 @@ Motion planning with quintic polynomials.
It can calculate a 2D path, velocity, and acceleration profile based on quintic polynomials.
-Ref:
+Reference
-- [Local Path Planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
+- [Local Path Planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)
## Reeds Shepp planning
@@ -444,7 +451,7 @@ A sample code with Reeds Shepp path planning.
-Ref:
+Reference
- [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html)
@@ -470,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)
@@ -483,9 +490,9 @@ Ref:
This is a simulation of moving to a pose control
-
+
-Ref:
+Reference
- [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
@@ -496,7 +503,7 @@ Path tracking simulation with Stanley steering control and PID speed control.
-Ref:
+Reference
- [Stanley: The robot that won the DARPA grand challenge](http://robots.stanford.edu/papers/thrun.stanley05.pdf)
@@ -510,7 +517,7 @@ Path tracking simulation with rear wheel feedback steering control and PID speed
-Ref:
+Reference
- [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
@@ -521,9 +528,9 @@ Path tracking simulation with LQR speed and steering control.
-Ref:
+Reference
-- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](http://ieeexplore.ieee.org/document/5940562/)
+- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](https://ieeexplore.ieee.org/document/5940562/)
## Model predictive speed and steering control
@@ -532,9 +539,9 @@ Path tracking simulation with iterative linear model predictive speed and steeri
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)
- [Real\-time Model Predictive Control \(MPC\), ACADO, Python \| Work\-is\-Playing](http://grauonline.de/wordpress/?page_id=3244)
@@ -544,9 +551,9 @@ A motion planning and path tracking simulation with NMPC of C-GMRES
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/cgmres_nmpc/cgmres_nmpc.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc.html)
# Arm Navigation
@@ -584,9 +591,9 @@ This is a 3d trajectory generation simulation for a rocket powered landing.
-Ref:
+Reference
-- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
# Bipedal
@@ -614,7 +621,7 @@ This is a list of user's comment and references:[users\_comments](https://github
Any contribution is welcome!!
-Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/how_to_contribute.html)
+Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html)
# Citing
@@ -630,7 +637,7 @@ If you or your company would like to support this project, please consider:
- [Become a backer or sponsor on Patreon](https://www.patreon.com/myenigma)
-- [One-time donation via PayPal](https://www.paypal.me/myenigmapay/)
+- [One-time donation via PayPal](https://www.paypal.com/paypalme/myenigmapay/)
If you would like to support us in some other way, please contact with creating an issue.
@@ -640,7 +647,7 @@ If you would like to support us in some other way, please contact with creating
They are providing a free license of their IDEs for this OSS development.
-### [1Password](https://github.com/1Password/1password-teams-open-source)
+### [1Password](https://github.com/1Password/for-open-source)
They are providing a free license of their 1Password team license for this OSS project.
diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index bb648ce9d9..5c4417d7c3 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/SLAM/EKFSLAM/ekf_slam.py
@@ -8,6 +8,7 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2
@@ -28,10 +29,9 @@
def ekf_slam(xEst, PEst, u, z):
# Predict
- S = STATE_SIZE
- G, Fx = jacob_motion(xEst[0:S], u)
- xEst[0:S] = motion_model(xEst[0:S], u)
- PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
+ G, Fx = jacob_motion(xEst, u)
+ xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
+ PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
initP = np.eye(2)
# Update
@@ -119,7 +119,7 @@ def jacob_motion(x, u):
[0.0, 0.0, DT * u[0, 0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]], dtype=float)
- G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
+ G = np.eye(len(x)) + Fx.T @ jF @ Fx
return G, Fx,
@@ -192,7 +192,7 @@ def jacob_h(q, delta, x, i):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py
index 7b89f52df4..98f8a66417 100644
--- a/SLAM/FastSLAM1/fast_slam1.py
+++ b/SLAM/FastSLAM1/fast_slam1.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -71,19 +72,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
- # print(xEst)
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -234,28 +234,27 @@ def resampling(particles):
pw = np.array(pw)
n_eff = 1.0 / (pw @ pw.T) # Effective particle number
- # print(n_eff)
if n_eff < NTH: # resampling
w_cum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -274,34 +273,34 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, rfid):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
for i in range(len(rfid[:, 0])):
- dx = rfid[i, 0] - xTrue[0, 0]
- dy = rfid[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_with_noize = angle + np.random.randn() * Q_sim[
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_with_noize = angle + np.random.randn() * Q_SIM[
1, 1] ** 0.5 # add noise
zi = np.array([dn, pi_2_pi(angle_with_noize), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -321,7 +320,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -330,7 +329,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -339,17 +338,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -357,18 +356,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam1(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -376,16 +375,16 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event', lambda event:
[exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py
index aa77aa956a..d4cf0d84dd 100644
--- a/SLAM/FastSLAM2/fast_slam2.py
+++ b/SLAM/FastSLAM2/fast_slam2.py
@@ -10,14 +10,15 @@
import matplotlib.pyplot as plt
import numpy as np
+from utils.angle import angle_mod
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
# Simulation parameter
-Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
-R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
+Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
+R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01
DT = 0.1 # time tick [s]
@@ -34,16 +35,16 @@
class Particle:
- def __init__(self, N_LM):
+ def __init__(self, n_landmark):
self.w = 1.0 / N_PARTICLE
self.x = 0.0
self.y = 0.0
self.yaw = 0.0
self.P = np.eye(3)
# landmark x-y positions
- self.lm = np.zeros((N_LM, LM_SIZE))
+ self.lm = np.zeros((n_landmark, LM_SIZE))
# landmark position covariance
- self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))
+ self.lmP = np.zeros((n_landmark * LM_SIZE, LM_SIZE))
def fast_slam2(particles, u, z):
@@ -72,18 +73,18 @@ def normalize_weight(particles):
def calc_final_state(particles):
- xEst = np.zeros((STATE_SIZE, 1))
+ x_est = np.zeros((STATE_SIZE, 1))
particles = normalize_weight(particles)
for i in range(N_PARTICLE):
- xEst[0, 0] += particles[i].w * particles[i].x
- xEst[1, 0] += particles[i].w * particles[i].y
- xEst[2, 0] += particles[i].w * particles[i].yaw
+ x_est[0, 0] += particles[i].w * particles[i].x
+ x_est[1, 0] += particles[i].w * particles[i].y
+ x_est[2, 0] += particles[i].w * particles[i].yaw
- xEst[2, 0] = pi_2_pi(xEst[2, 0])
+ x_est[2, 0] = pi_2_pi(x_est[2, 0])
- return xEst
+ return x_est
def predict_particles(particles, u):
@@ -265,21 +266,21 @@ def resampling(particles):
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
- inds = []
- ind = 0
+ indexes = []
+ index = 0
for ip in range(N_PARTICLE):
- while (ind < w_cum.shape[0] - 1) \
- and (resample_id[ip] > w_cum[ind]):
- ind += 1
- inds.append(ind)
+ while (index < w_cum.shape[0] - 1) \
+ and (resample_id[ip] > w_cum[index]):
+ index += 1
+ indexes.append(index)
tmp_particles = particles[:]
- for i in range(len(inds)):
- particles[i].x = tmp_particles[inds[i]].x
- particles[i].y = tmp_particles[inds[i]].y
- particles[i].yaw = tmp_particles[inds[i]].yaw
- particles[i].lm = tmp_particles[inds[i]].lm[:, :]
- particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
+ for i in range(len(indexes)):
+ particles[i].x = tmp_particles[indexes[i]].x
+ particles[i].y = tmp_particles[indexes[i]].y
+ particles[i].yaw = tmp_particles[indexes[i]].yaw
+ particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
+ particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE
return particles
@@ -298,35 +299,35 @@ def calc_input(time):
return u
-def observation(xTrue, xd, u, RFID):
+def observation(x_true, xd, u, rfid):
# calc true state
- xTrue = motion_model(xTrue, u)
+ x_true = motion_model(x_true, u)
# add noise to range observation
z = np.zeros((3, 0))
- for i in range(len(RFID[:, 0])):
+ for i in range(len(rfid[:, 0])):
- dx = RFID[i, 0] - xTrue[0, 0]
- dy = RFID[i, 1] - xTrue[1, 0]
+ dx = rfid[i, 0] - x_true[0, 0]
+ dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
- angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
+ angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
- dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
- angle_noise = np.random.randn() * Q_sim[1, 1] ** 0.5
+ dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
+ angle_noise = np.random.randn() * Q_SIM[1, 1] ** 0.5
angle_with_noise = angle + angle_noise # add noise
zi = np.array([dn, pi_2_pi(angle_with_noise), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
- ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
- ud2 = u[1, 0] + np.random.randn() * R_sim[
+ ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
+ ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
- return xTrue, z, xd, ud
+ return x_true, z, xd, ud
def motion_model(x, u):
@@ -346,7 +347,7 @@ def motion_model(x, u):
def pi_2_pi(angle):
- return (angle + math.pi) % (2 * math.pi) - math.pi
+ return angle_mod(angle)
def main():
@@ -355,7 +356,7 @@ def main():
time = 0.0
# RFID positions [x, y]
- RFID = np.array([[10.0, -2.0],
+ rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
@@ -364,17 +365,17 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
- n_landmark = RFID.shape[0]
+ n_landmark = rfid.shape[0]
# State Vector [x y yaw v]'
- xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
- xTrue = np.zeros((STATE_SIZE, 1)) # True state
- xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
+ x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
+ x_true = np.zeros((STATE_SIZE, 1)) # True state
+ x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
- hxEst = xEst
- hxTrue = xTrue
- hxDR = xTrue
+ hist_x_est = x_est
+ hist_x_true = x_true
+ hist_x_dr = x_dr
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
@@ -382,18 +383,18 @@ def main():
time += DT
u = calc_input(time)
- xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+ x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)
particles = fast_slam2(particles, ud, z)
- xEst = calc_final_state(particles)
+ x_est = calc_final_state(particles)
- x_state = xEst[0: STATE_SIZE]
+ x_state = x_est[0: STATE_SIZE]
# store data history
- hxEst = np.hstack((hxEst, x_state))
- hxDR = np.hstack((hxDR, xDR))
- hxTrue = np.hstack((hxTrue, xTrue))
+ hist_x_est = np.hstack((hist_x_est, x_state))
+ hist_x_dr = np.hstack((hist_x_dr, x_dr))
+ hist_x_true = np.hstack((hist_x_true, x_true))
if show_animation: # pragma: no cover
plt.cla()
@@ -401,21 +402,21 @@ def main():
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
- plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+ plt.plot(rfid[:, 0], rfid[:, 1], "*k")
for iz in range(len(z[:, 0])):
landmark_id = int(z[2, iz])
- plt.plot([xEst[0][0], RFID[landmark_id, 0]], [
- xEst[1][0], RFID[landmark_id, 1]], "-k")
+ plt.plot([x_est[0][0], rfid[landmark_id, 0]], [
+ x_est[1][0], rfid[landmark_id, 1]], "-k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
- plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
- plt.plot(hxDR[0, :], hxDR[1, :], "-k")
- plt.plot(hxEst[0, :], hxEst[1, :], "-r")
- plt.plot(xEst[0], xEst[1], "xk")
+ plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
+ plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
+ plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
+ plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
diff --git a/SLAM/GraphBasedSLAM/data/README.rst b/SLAM/GraphBasedSLAM/data/README.rst
index c875cce73f..15bc5b6c03 100644
--- a/SLAM/GraphBasedSLAM/data/README.rst
+++ b/SLAM/GraphBasedSLAM/data/README.rst
@@ -3,4 +3,4 @@ Acknowledgments and References
Thanks to Luca Larlone for allowing inclusion of the `Intel dataset