diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 91f6dfa822..3bcc499e6a 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,23 +1,5 @@ -# Contributing to Python +# Contributing -:+1::tada: First of off, thanks very much for taking the time to contribute! :tada::+1: +:+1::tada: First of all, thank you very much for taking the time to contribute! :tada::+1: -The following is a set of guidelines for contributing to PythonRobotics. - -These are mostly guidelines, not rules. - -Use your best judgment, and feel free to propose changes to this document in a pull request. - -# Before contributing - -## Taking a look at the paper. - -Please check this paper to understand the philosophy of this project. - -- [\[1808\.10703\] PythonRobotics: a Python code collection of robotics algorithms](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib)) - -## Check your Python version. - -We only accept a PR for Python 3.8.x or higher. - -We will not accept a PR for Python 2.x. +Please check this document for contribution: [How to contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html) diff --git a/Control/inverted_pendulum/inverted_pendulum_lqr_control.py b/InvertedPendulum/inverted_pendulum_lqr_control.py similarity index 100% rename from Control/inverted_pendulum/inverted_pendulum_lqr_control.py rename to InvertedPendulum/inverted_pendulum_lqr_control.py diff --git a/Control/inverted_pendulum/inverted_pendulum_mpc_control.py b/InvertedPendulum/inverted_pendulum_mpc_control.py similarity index 100% rename from Control/inverted_pendulum/inverted_pendulum_mpc_control.py rename to InvertedPendulum/inverted_pendulum_mpc_control.py diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py new file mode 100644 index 0000000000..0dcc7380c5 --- /dev/null +++ b/Mapping/DistanceMap/distance_map.py @@ -0,0 +1,202 @@ +""" +Distance Map + +author: Wang Zheng (@Aglargil) + +Ref: + +- [Distance Map] +(https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf) +""" + +import numpy as np +import matplotlib.pyplot as plt +import scipy + +INF = 1e20 +ENABLE_PLOT = True + + +def compute_sdf_scipy(obstacles): + """ + Compute the signed distance field (SDF) from a boolean field using scipy. + This function has the same functionality as compute_sdf. + However, by using scipy.ndimage.distance_transform_edt, it can compute much faster. + + Example: 500×500 map + • compute_sdf: 3 sec + • compute_sdf_scipy: 0.05 sec + + Parameters + ---------- + obstacles : array_like + A 2D boolean array where '1' represents obstacles and '0' represents free space. + + Returns + ------- + array_like + A 2D array representing the signed distance field, where positive values indicate distance + to the nearest obstacle, and negative values indicate distance to the nearest free space. + """ + # distance_transform_edt use '0' as obstacles, so we need to convert the obstacles to '0' + a = scipy.ndimage.distance_transform_edt(obstacles == 0) + b = scipy.ndimage.distance_transform_edt(obstacles == 1) + return a - b + + +def compute_udf_scipy(obstacles): + """ + Compute the unsigned distance field (UDF) from a boolean field using scipy. + This function has the same functionality as compute_udf. + However, by using scipy.ndimage.distance_transform_edt, it can compute much faster. + + Example: 500×500 map + • compute_udf: 1.5 sec + • compute_udf_scipy: 0.02 sec + + Parameters + ---------- + obstacles : array_like + A 2D boolean array where '1' represents obstacles and '0' represents free space. + + Returns + ------- + array_like + A 2D array of distances from the nearest obstacle, with the same dimensions as `bool_field`. + """ + return scipy.ndimage.distance_transform_edt(obstacles == 0) + + +def compute_sdf(obstacles): + """ + Compute the signed distance field (SDF) from a boolean field. + + Parameters + ---------- + obstacles : array_like + A 2D boolean array where '1' represents obstacles and '0' represents free space. + + Returns + ------- + array_like + A 2D array representing the signed distance field, where positive values indicate distance + to the nearest obstacle, and negative values indicate distance to the nearest free space. + """ + a = compute_udf(obstacles) + b = compute_udf(obstacles == 0) + return a - b + + +def compute_udf(obstacles): + """ + Compute the unsigned distance field (UDF) from a boolean field. + + Parameters + ---------- + obstacles : array_like + A 2D boolean array where '1' represents obstacles and '0' represents free space. + + Returns + ------- + array_like + A 2D array of distances from the nearest obstacle, with the same dimensions as `bool_field`. + """ + edt = obstacles.copy() + if not np.all(np.isin(edt, [0, 1])): + raise ValueError("Input array should only contain 0 and 1") + edt = np.where(edt == 0, INF, edt) + edt = np.where(edt == 1, 0, edt) + for row in range(len(edt)): + dt(edt[row]) + edt = edt.T + for row in range(len(edt)): + dt(edt[row]) + edt = edt.T + return np.sqrt(edt) + + +def dt(d): + """ + Compute 1D distance transform under the squared Euclidean distance + + Parameters + ---------- + d : array_like + Input array containing the distances. + + Returns: + -------- + d : array_like + The transformed array with computed distances. + """ + v = np.zeros(len(d) + 1) + z = np.zeros(len(d) + 1) + k = 0 + v[0] = 0 + z[0] = -INF + z[1] = INF + for q in range(1, len(d)): + s = ((d[q] + q * q) - (d[int(v[k])] + v[k] * v[k])) / (2 * q - 2 * v[k]) + while s <= z[k]: + k = k - 1 + s = ((d[q] + q * q) - (d[int(v[k])] + v[k] * v[k])) / (2 * q - 2 * v[k]) + k = k + 1 + v[k] = q + z[k] = s + z[k + 1] = INF + k = 0 + for q in range(len(d)): + while z[k + 1] < q: + k = k + 1 + dx = q - v[k] + d[q] = dx * dx + d[int(v[k])] + + +def main(): + obstacles = np.array( + [ + [1, 0, 0, 0, 0], + [0, 1, 1, 1, 0], + [0, 1, 1, 1, 0], + [0, 0, 1, 1, 0], + [0, 0, 1, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 1, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, 0, 0], + ] + ) + + # Compute the signed distance field + sdf = compute_sdf(obstacles) + udf = compute_udf(obstacles) + + if ENABLE_PLOT: + _, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(15, 5)) + + obstacles_plot = ax1.imshow(obstacles, cmap="binary") + ax1.set_title("Obstacles") + ax1.set_xlabel("x") + ax1.set_ylabel("y") + plt.colorbar(obstacles_plot, ax=ax1) + + udf_plot = ax2.imshow(udf, cmap="viridis") + ax2.set_title("Unsigned Distance Field") + ax2.set_xlabel("x") + ax2.set_ylabel("y") + plt.colorbar(udf_plot, ax=ax2) + + sdf_plot = ax3.imshow(sdf, cmap="RdBu") + ax3.set_title("Signed Distance Field") + ax3.set_xlabel("x") + ax3.set_ylabel("y") + plt.colorbar(sdf_plot, ax=ax3) + + plt.tight_layout() + plt.show() + + +if __name__ == "__main__": + main() diff --git a/MissionPlanning/StateMachine/robot_behavior_case.py b/MissionPlanning/StateMachine/robot_behavior_case.py new file mode 100644 index 0000000000..03ee60ae9f --- /dev/null +++ b/MissionPlanning/StateMachine/robot_behavior_case.py @@ -0,0 +1,111 @@ +""" +A case study of robot behavior using state machine + +author: Wang Zheng (@Aglargil) +""" + +from state_machine import StateMachine + + +class Robot: + def __init__(self): + self.battery = 100 + self.task_progress = 0 + + # Initialize state machine + self.machine = StateMachine("robot_sm", self) + + # Add state transition rules + self.machine.add_transition( + src_state="patrolling", + event="detect_task", + dst_state="executing_task", + guard=None, + action=None, + ) + + self.machine.add_transition( + src_state="executing_task", + event="task_complete", + dst_state="patrolling", + guard=None, + action="reset_task", + ) + + self.machine.add_transition( + src_state="executing_task", + event="low_battery", + dst_state="returning_to_base", + guard="is_battery_low", + ) + + self.machine.add_transition( + src_state="returning_to_base", + event="reach_base", + dst_state="charging", + guard=None, + action=None, + ) + + self.machine.add_transition( + src_state="charging", + event="charge_complete", + dst_state="patrolling", + guard=None, + action="battery_full", + ) + + # Set initial state + self.machine.set_current_state("patrolling") + + def is_battery_low(self): + """Battery level check condition""" + return self.battery < 30 + + def reset_task(self): + """Reset task progress""" + self.task_progress = 0 + print("[Action] Task progress has been reset") + + # Modify state entry callback naming convention (add state_ prefix) + def on_enter_executing_task(self): + print("\n------ Start Executing Task ------") + print(f"Current battery: {self.battery}%") + while self.machine.get_current_state().name == "executing_task": + self.task_progress += 10 + self.battery -= 25 + print( + f"Task progress: {self.task_progress}%, Remaining battery: {self.battery}%" + ) + + if self.task_progress >= 100: + self.machine.process("task_complete") + break + elif self.is_battery_low(): + self.machine.process("low_battery") + break + + def on_enter_returning_to_base(self): + print("\nLow battery, returning to charging station...") + self.machine.process("reach_base") + + def on_enter_charging(self): + print("\n------ Charging ------") + self.battery = 100 + print("Charging complete!") + self.machine.process("charge_complete") + + +# Keep the test section structure the same, only modify the trigger method +if __name__ == "__main__": + robot = Robot() + print(robot.machine.generate_plantuml()) + + print(f"Initial state: {robot.machine.get_current_state().name}") + print("------------") + + # Trigger task detection event + robot.machine.process("detect_task") + + print("\n------------") + print(f"Final state: {robot.machine.get_current_state().name}") diff --git a/MissionPlanning/StateMachine/state_machine.py b/MissionPlanning/StateMachine/state_machine.py new file mode 100644 index 0000000000..de72f0f451 --- /dev/null +++ b/MissionPlanning/StateMachine/state_machine.py @@ -0,0 +1,294 @@ +""" +State Machine + +author: Wang Zheng (@Aglargil) + +Ref: + +- [State Machine] +(https://en.wikipedia.org/wiki/Finite-state_machine) +""" + +import string +from urllib.request import urlopen, Request +from base64 import b64encode +from zlib import compress +from io import BytesIO +from collections.abc import Callable +from matplotlib.image import imread +from matplotlib import pyplot as plt + + +def deflate_and_encode(plantuml_text): + """ + zlib compress the plantuml text and encode it for the plantuml server. + + Ref: https://plantuml.com/en/text-encoding + """ + plantuml_alphabet = ( + string.digits + string.ascii_uppercase + string.ascii_lowercase + "-_" + ) + base64_alphabet = ( + string.ascii_uppercase + string.ascii_lowercase + string.digits + "+/" + ) + b64_to_plantuml = bytes.maketrans( + base64_alphabet.encode("utf-8"), plantuml_alphabet.encode("utf-8") + ) + zlibbed_str = compress(plantuml_text.encode("utf-8")) + compressed_string = zlibbed_str[2:-4] + return b64encode(compressed_string).translate(b64_to_plantuml).decode("utf-8") + + +class State: + def __init__(self, name, on_enter=None, on_exit=None): + self.name = name + self.on_enter = on_enter + self.on_exit = on_exit + + def enter(self): + print(f"entering <{self.name}>") + if self.on_enter: + self.on_enter() + + def exit(self): + print(f"exiting <{self.name}>") + if self.on_exit: + self.on_exit() + + +class StateMachine: + def __init__(self, name: str, model=object): + """Initialize the state machine. + + Args: + name (str): Name of the state machine. + model (object, optional): Model object used to automatically look up callback functions + for states and transitions: + State callbacks: Automatically searches for 'on_enter_' 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/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py new file mode 100644 index 0000000000..785f822d14 --- /dev/null +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -0,0 +1,300 @@ +""" +Elastic Bands + +author: Wang Zheng (@Aglargil) + +Ref: + +- [Elastic Bands: Connecting Path Planning and Control] +(http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf) +""" + +import numpy as np +import sys +import pathlib +import matplotlib.pyplot as plt +from matplotlib.patches import Circle + +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) + +from Mapping.DistanceMap.distance_map import compute_sdf_scipy + +# Elastic Bands Params +MAX_BUBBLE_RADIUS = 100 +MIN_BUBBLE_RADIUS = 10 +RHO0 = 20.0 # Maximum distance for applying repulsive force +KC = 0.05 # Contraction force gain +KR = -0.1 # Repulsive force gain +LAMBDA = 0.7 # Overlap constraint factor +STEP_SIZE = 3.0 # Step size for calculating gradient + +# Visualization Params +ENABLE_PLOT = True +# ENABLE_INTERACTIVE is True allows user to add obstacles by left clicking +# and add path points by right clicking and start planning by middle clicking +ENABLE_INTERACTIVE = False +# ENABLE_SAVE_DATA is True allows saving the path and obstacles which added +# by user in interactive mode to file +ENABLE_SAVE_DATA = False +MAX_ITER = 50 + + +class Bubble: + def __init__(self, position, radius): + self.pos = np.array(position) # Bubble center coordinates [x, y] + self.radius = radius # Safety distance radius ρ(b) + if self.radius > MAX_BUBBLE_RADIUS: + self.radius = MAX_BUBBLE_RADIUS + if self.radius < MIN_BUBBLE_RADIUS: + self.radius = MIN_BUBBLE_RADIUS + + +class ElasticBands: + def __init__( + self, + initial_path, + obstacles, + rho0=RHO0, + kc=KC, + kr=KR, + lambda_=LAMBDA, + step_size=STEP_SIZE, + ): + self.distance_map = compute_sdf_scipy(obstacles) + self.bubbles = [ + Bubble(p, self.compute_rho(p)) for p in initial_path + ] # Initialize bubble chain + self.kc = kc # Contraction force gain + self.kr = kr # Repulsive force gain + self.rho0 = rho0 # Maximum distance for applying repulsive force + self.lambda_ = lambda_ # Overlap constraint factor + self.step_size = step_size # Step size for calculating gradient + self._maintain_overlap() + + def compute_rho(self, position): + """Compute the distance field value at the position""" + return self.distance_map[int(position[0]), int(position[1])] + + def contraction_force(self, i): + """Calculate internal contraction force for the i-th bubble""" + if i == 0 or i == len(self.bubbles) - 1: + return np.zeros(2) + + prev = self.bubbles[i - 1].pos + next_ = self.bubbles[i + 1].pos + current = self.bubbles[i].pos + + # f_c = kc * ( (prev-current)/|prev-current| + (next-current)/|next-current| ) + dir_prev = (prev - current) / (np.linalg.norm(prev - current) + 1e-6) + dir_next = (next_ - current) / (np.linalg.norm(next_ - current) + 1e-6) + return self.kc * (dir_prev + dir_next) + + def repulsive_force(self, i): + """Calculate external repulsive force for the i-th bubble""" + h = self.step_size # Step size + b = self.bubbles[i].pos + rho = self.bubbles[i].radius + + if rho >= self.rho0: + return np.zeros(2) + + # Finite difference approximation of the gradient ∂ρ/∂b + dx = np.array([h, 0]) + dy = np.array([0, h]) + grad_x = (self.compute_rho(b - dx) - self.compute_rho(b + dx)) / (2 * h) + grad_y = (self.compute_rho(b - dy) - self.compute_rho(b + dy)) / (2 * h) + grad = np.array([grad_x, grad_y]) + + return self.kr * (self.rho0 - rho) * grad + + def update_bubbles(self): + """Update bubble positions""" + new_bubbles = [] + for i in range(len(self.bubbles)): + if i == 0 or i == len(self.bubbles) - 1: + new_bubbles.append(self.bubbles[i]) # Fixed start and end points + continue + + f_total = self.contraction_force(i) + self.repulsive_force(i) + v = self.bubbles[i - 1].pos - self.bubbles[i + 1].pos + + # Remove tangential component + f_star = f_total - f_total * v * v / (np.linalg.norm(v) ** 2 + 1e-6) + + alpha = self.bubbles[i].radius # Adaptive step size + new_pos = self.bubbles[i].pos + alpha * f_star + new_pos = np.clip(new_pos, 0, 499) + new_radius = self.compute_rho(new_pos) + + # Update bubble and maintain overlap constraint + new_bubble = Bubble(new_pos, new_radius) + new_bubbles.append(new_bubble) + + self.bubbles = new_bubbles + self._maintain_overlap() + + def _maintain_overlap(self): + """Maintain bubble chain continuity (simplified insertion/deletion mechanism)""" + # Insert bubbles + i = 0 + while i < len(self.bubbles) - 1: + bi, bj = self.bubbles[i], self.bubbles[i + 1] + dist = np.linalg.norm(bi.pos - bj.pos) + if dist > self.lambda_ * (bi.radius + bj.radius): + new_pos = (bi.pos + bj.pos) / 2 + rho = self.compute_rho( + new_pos + ) # Calculate new radius using environment model + self.bubbles.insert(i + 1, Bubble(new_pos, rho)) + i += 2 # Skip the processed region + else: + i += 1 + + # Delete redundant bubbles + i = 1 + while i < len(self.bubbles) - 1: + prev = self.bubbles[i - 1] + next_ = self.bubbles[i + 1] + dist = np.linalg.norm(prev.pos - next_.pos) + if dist <= self.lambda_ * (prev.radius + next_.radius): + del self.bubbles[i] # Delete if redundant + else: + i += 1 + + +class ElasticBandsVisualizer: + def __init__(self): + self.obstacles = np.zeros((500, 500)) + self.obstacles_points = [] + self.path_points = [] + self.elastic_band = None + self.running = True + + if ENABLE_PLOT: + self.fig, self.ax = plt.subplots(figsize=(8, 8)) + self.fig.canvas.mpl_connect("close_event", self.on_close) + self.ax.set_xlim(0, 500) + self.ax.set_ylim(0, 500) + + if ENABLE_INTERACTIVE: + self.path_points = [] # Add a list to store path points + # Connect mouse events + self.fig.canvas.mpl_connect("button_press_event", self.on_click) + else: + self.path_points = np.load(pathlib.Path(__file__).parent / "path.npy") + self.obstacles_points = np.load( + pathlib.Path(__file__).parent / "obstacles.npy" + ) + for x, y in self.obstacles_points: + self.add_obstacle(x, y) + self.plan_path() + + self.plot_background() + + def on_close(self, event): + """Handle window close event""" + self.running = False + plt.close("all") # Close all figure windows + + def plot_background(self): + """Plot the background grid""" + if not ENABLE_PLOT or not self.running: + return + + self.ax.cla() + self.ax.set_xlim(0, 500) + self.ax.set_ylim(0, 500) + self.ax.grid(True) + + if ENABLE_INTERACTIVE: + self.ax.set_title( + "Elastic Bands Path Planning\n" + "Left click: Add obstacles\n" + "Right click: Add path points\n" + "Middle click: Start planning", + pad=20, + ) + else: + self.ax.set_title("Elastic Bands Path Planning", pad=20) + + if self.path_points: + self.ax.plot( + [p[0] for p in self.path_points], + [p[1] for p in self.path_points], + "yo", + markersize=8, + ) + + self.ax.imshow(self.obstacles.T, origin="lower", cmap="binary", alpha=0.8) + self.ax.plot([], [], color="black", label="obstacles") + if self.elastic_band is not None: + path = [b.pos.tolist() for b in self.elastic_band.bubbles] + path = np.array(path) + self.ax.plot(path[:, 0], path[:, 1], "b-", linewidth=2, label="path") + + for bubble in self.elastic_band.bubbles: + circle = Circle( + bubble.pos, bubble.radius, fill=False, color="g", alpha=0.3 + ) + self.ax.add_patch(circle) + self.ax.plot(bubble.pos[0], bubble.pos[1], "bo", markersize=10) + self.ax.plot([], [], color="green", label="bubbles") + + self.ax.legend(loc="upper right") + plt.draw() + plt.pause(0.01) + + def add_obstacle(self, x, y): + """Add an obstacle at the given coordinates""" + size = 30 # Side length of the square + half_size = size // 2 + x_start = max(0, x - half_size) + x_end = min(self.obstacles.shape[0], x + half_size) + y_start = max(0, y - half_size) + y_end = min(self.obstacles.shape[1], y + half_size) + self.obstacles[x_start:x_end, y_start:y_end] = 1 + + def on_click(self, event): + """Handle mouse click events""" + if event.inaxes != self.ax: + return + + x, y = int(event.xdata), int(event.ydata) + + if event.button == 1: # Left click to add obstacles + self.add_obstacle(x, y) + self.obstacles_points.append([x, y]) + + elif event.button == 3: # Right click to add path points + self.path_points.append([x, y]) + + elif event.button == 2: # Middle click to end path input and start planning + if len(self.path_points) >= 2: + if ENABLE_SAVE_DATA: + np.save( + pathlib.Path(__file__).parent / "path.npy", self.path_points + ) + np.save( + pathlib.Path(__file__).parent / "obstacles.npy", + self.obstacles_points, + ) + self.plan_path() + + self.plot_background() + + def plan_path(self): + """Plan the path""" + + initial_path = self.path_points + # Create an elastic band object and optimize + self.elastic_band = ElasticBands(initial_path, self.obstacles) + for _ in range(MAX_ITER): + self.elastic_band.update_bubbles() + self.path_points = [b.pos for b in self.elastic_band.bubbles] + self.plot_background() + + +if __name__ == "__main__": + _ = ElasticBandsVisualizer() + if ENABLE_PLOT: + plt.show(block=True) diff --git a/PathPlanning/ElasticBands/obstacles.npy b/PathPlanning/ElasticBands/obstacles.npy new file mode 100644 index 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/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py new file mode 100644 index 0000000000..7b0190d023 --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -0,0 +1,273 @@ +""" +This file implements a grid with a 3d reservation matrix with dimensions for x, y, and time. There +is also infrastructure to generate dynamic obstacles that move around the grid. The obstacles' paths +are stored in the reservation matrix on creation. +""" +import numpy as np +import matplotlib.pyplot as plt +from enum import Enum +from dataclasses import dataclass + +@dataclass(order=True) +class Position: + x: int + y: int + + def as_ndarray(self) -> np.ndarray: + return np.array([self.x, self.y]) + + def __add__(self, other): + if isinstance(other, Position): + return Position(self.x + other.x, self.y + other.y) + raise NotImplementedError( + f"Addition not supported for Position and {type(other)}" + ) + + def __sub__(self, other): + if isinstance(other, Position): + return Position(self.x - other.x, self.y - other.y) + raise NotImplementedError( + f"Subtraction not supported for Position and {type(other)}" + ) + + +class ObstacleArrangement(Enum): + # Random obstacle positions and movements + RANDOM = 0 + # Obstacles start in a line in y at center of grid and move side-to-side in x + ARRANGEMENT1 = 1 + + +class Grid: + # Set in constructor + grid_size: np.ndarray + reservation_matrix: np.ndarray + obstacle_paths: list[list[Position]] = [] + # Obstacles will never occupy these points. Useful to avoid impossible scenarios + obstacle_avoid_points: list[Position] = [] + + # Number of time steps in the simulation + time_limit: int + + # Logging control + verbose = False + + def __init__( + self, + grid_size: np.ndarray, + num_obstacles: int = 40, + obstacle_avoid_points: list[Position] = [], + obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM, + time_limit: int = 100, + ): + self.obstacle_avoid_points = obstacle_avoid_points + self.time_limit = time_limit + self.grid_size = grid_size + self.reservation_matrix = np.zeros((grid_size[0], grid_size[1], self.time_limit)) + + if num_obstacles > self.grid_size[0] * self.grid_size[1]: + raise Exception("Number of obstacles is greater than grid size!") + + if obstacle_arrangement == ObstacleArrangement.RANDOM: + self.obstacle_paths = self.generate_dynamic_obstacles(num_obstacles) + elif obstacle_arrangement == ObstacleArrangement.ARRANGEMENT1: + self.obstacle_paths = self.obstacle_arrangement_1(num_obstacles) + + for i, path in enumerate(self.obstacle_paths): + obs_idx = i + 1 # avoid using 0 - that indicates free space in the grid + for t, position in enumerate(path): + # Reserve old & new position at this time step + if t > 0: + self.reservation_matrix[path[t - 1].x, path[t - 1].y, t] = obs_idx + self.reservation_matrix[position.x, position.y, t] = obs_idx + + """ + Generate dynamic obstacles that move around the grid. Initial positions and movements are random + """ + def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]: + obstacle_paths = [] + for _ in (0, obs_count): + # Sample until a free starting space is found + initial_position = self.sample_random_position() + while not self.valid_obstacle_position(initial_position, 0): + initial_position = self.sample_random_position() + + positions = [initial_position] + if self.verbose: + print("Obstacle initial position: ", initial_position) + + # Encourage obstacles to mostly stay in place - too much movement leads to chaotic planning scenarios + # that are not fun to watch + weights = [0.05, 0.05, 0.05, 0.05, 0.8] + diffs = [ + Position(0, 1), + Position(0, -1), + Position(1, 0), + Position(-1, 0), + Position(0, 0), + ] + + for t in range(1, self.time_limit - 1): + sampled_indices = np.random.choice( + len(diffs), size=5, replace=False, p=weights + ) + rand_diffs = [diffs[i] for i in sampled_indices] + + valid_position = None + for diff in rand_diffs: + new_position = positions[-1] + diff + + if not self.valid_obstacle_position(new_position, t): + continue + + valid_position = new_position + break + + # Impossible situation for obstacle - stay in place + # -> this can happen if the oaths of other obstacles this one + if valid_position is None: + valid_position = positions[-1] + + positions.append(valid_position) + + obstacle_paths.append(positions) + + return obstacle_paths + + """ + Generate a line of obstacles in y at the center of the grid that move side-to-side in x + Bottom half start moving right, top half start moving left. If `obs_count` is less than the length of + the grid, only the first `obs_count` obstacles will be generated. + """ + def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]: + obstacle_paths = [] + half_grid_x = self.grid_size[0] // 2 + half_grid_y = self.grid_size[1] // 2 + + for y_idx in range(0, min(obs_count, self.grid_size[1])): + moving_right = y_idx < half_grid_y + position = Position(half_grid_x, y_idx) + path = [position] + + for t in range(1, self.time_limit - 1): + # sit in place every other time step + if t % 2 == 0: + path.append(position) + continue + + # first check if we should switch direction (at edge of grid) + if (moving_right and position.x == self.grid_size[0] - 1) or ( + not moving_right and position.x == 0 + ): + moving_right = not moving_right + # step in direction + position = Position( + position.x + (1 if moving_right else -1), position.y + ) + path.append(position) + + obstacle_paths.append(path) + + return obstacle_paths + + """ + Check if the given position is valid at time t + + input: + position (Position): (x, y) position + t (int): time step + + output: + bool: True if position/time combination is valid, False otherwise + """ + def valid_position(self, position: Position, t: int) -> bool: + # Check if new position is in grid + if not self.inside_grid_bounds(position): + return False + + # Check if new position is not occupied at time t + return self.reservation_matrix[position.x, position.y, t] == 0 + + """ + Returns True if the given position is valid at time t and is not in the set of obstacle_avoid_points + """ + def valid_obstacle_position(self, position: Position, t: int) -> bool: + return ( + self.valid_position(position, t) + and position not in self.obstacle_avoid_points + ) + + """ + Returns True if the given position is within the grid's boundaries + """ + def inside_grid_bounds(self, position: Position) -> bool: + return ( + position.x >= 0 + and position.x < self.grid_size[0] + and position.y >= 0 + and position.y < self.grid_size[1] + ) + + """ + Sample a random position that is within the grid's boundaries + + output: + Position: (x, y) position + """ + def sample_random_position(self) -> Position: + return Position( + np.random.randint(0, self.grid_size[0]), + np.random.randint(0, self.grid_size[1]), + ) + + """ + Returns a tuple of (x_positions, y_positions) of the obstacles at time t + """ + def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]: + x_positions = [] + y_positions = [] + for obs_path in self.obstacle_paths: + x_positions.append(obs_path[t].x) + y_positions.append(obs_path[t].y) + return (x_positions, y_positions) + + +show_animation = True + + +def main(): + grid = Grid( + np.array([11, 11]), + num_obstacles=10, + obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, + ) + + if not show_animation: + return + + fig = plt.figure(figsize=(8, 7)) + ax = fig.add_subplot( + autoscale_on=False, + xlim=(0, grid.grid_size[0] - 1), + ylim=(0, grid.grid_size[1] - 1), + ) + ax.set_aspect("equal") + ax.grid() + ax.set_xticks(np.arange(0, 11, 1)) + ax.set_yticks(np.arange(0, 11, 1)) + (obs_points,) = ax.plot([], [], "ro", ms=15) + + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect( + "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] + ) + + for i in range(0, grid.time_limit - 1): + obs_positions = grid.get_obstacle_positions_at_time(i) + obs_points.set_data(obs_positions[0], obs_positions[1]) + plt.pause(0.2) + plt.show() + + +if __name__ == "__main__": + main() diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py new file mode 100644 index 0000000000..a7aed41869 --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -0,0 +1,220 @@ +""" +Space-time A* Algorithm + This script demonstrates the Space-time A* algorithm for path planning in a grid world with moving obstacles. + This algorithm is different from normal 2D A* in one key way - the cost (often notated as g(n)) is + the number of time steps it took to get to a given node, instead of the number of cells it has + traversed. This ensures the path is time-optimal, while 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 +import matplotlib.pyplot as plt +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + ObstacleArrangement, + Position, +) +import heapq +from collections.abc import Generator +import random +from dataclasses import dataclass +from functools import total_ordering + + +# Seed randomness for reproducibility +RANDOM_SEED = 50 +random.seed(RANDOM_SEED) +np.random.seed(RANDOM_SEED) + +@dataclass() +# Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because +# this class needs to override the __lt__ and __eq__ methods to ignore parent_index. Parent +# index is just used to track the path found by the algorithm, and has no effect on the quality +# of a node. +@total_ordering +class Node: + position: Position + time: int + heuristic: int + parent_index: int + + """ + This is what is used to drive node expansion. The node with the lowest value is expanded next. + This comparison prioritizes the node with the lowest cost-to-come (self.time) + cost-to-go (self.heuristic) + """ + def __lt__(self, other: object): + if not isinstance(other, Node): + return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}") + return (self.time + self.heuristic) < (other.time + other.heuristic) + + def __eq__(self, other: object): + if not isinstance(other, Node): + return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}") + return self.position == other.position and self.time == other.time + + +class NodePath: + path: list[Node] + positions_at_time: dict[int, Position] = {} + + def __init__(self, path: list[Node]): + self.path = path + for node in path: + self.positions_at_time[node.time] = node.position + + """ + Get the position of the path at a given time + """ + def get_position(self, time: int) -> Position | None: + return self.positions_at_time.get(time) + + """ + Time stamp of the last node in the path + """ + def goal_reached_time(self) -> int: + return self.path[-1].time + + def __repr__(self): + repr_string = "" + for i, node in enumerate(self.path): + repr_string += f"{i}: {node}\n" + return repr_string + + +class SpaceTimeAStar: + grid: Grid + start: Position + goal: Position + + def __init__(self, grid: Grid, start: Position, goal: Position): + self.grid = grid + self.start = start + self.goal = goal + + def plan(self, verbose: bool = False) -> NodePath: + open_set: list[Node] = [] + heapq.heappush( + open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1) + ) + + expanded_set: list[Node] = [] + while open_set: + expanded_node: Node = heapq.heappop(open_set) + if verbose: + print("Expanded node:", expanded_node) + + if expanded_node.time + 1 >= self.grid.time_limit: + if verbose: + print(f"\tSkipping node that is past time limit: {expanded_node}") + continue + + if expanded_node.position == self.goal: + print(f"Found path to goal after {len(expanded_set)} expansions") + path = [] + path_walker: Node = expanded_node + while True: + path.append(path_walker) + if path_walker.parent_index == -1: + break + path_walker = expanded_set[path_walker.parent_index] + + # reverse path so it goes start -> goal + path.reverse() + return NodePath(path) + + expanded_idx = len(expanded_set) + expanded_set.append(expanded_node) + + for child in self.generate_successors(expanded_node, expanded_idx, verbose): + heapq.heappush(open_set, child) + + raise Exception("No path found") + + """ + Generate possible successors of the provided `parent_node` + """ + def generate_successors( + self, parent_node: Node, parent_node_idx: int, verbose: bool + ) -> Generator[Node, None, None]: + diffs = [ + Position(0, 0), + Position(1, 0), + Position(-1, 0), + Position(0, 1), + Position(0, -1), + ] + for diff in diffs: + new_pos = parent_node.position + diff + if self.grid.valid_position(new_pos, parent_node.time + 1): + new_node = Node( + new_pos, + parent_node.time + 1, + self.calculate_heuristic(new_pos), + parent_node_idx, + ) + if verbose: + print("\tNew successor node: ", new_node) + yield new_node + + def calculate_heuristic(self, position) -> int: + diff = self.goal - position + return abs(diff.x) + abs(diff.y) + + +show_animation = True +verbose = False + +def main(): + start = Position(1, 11) + goal = Position(19, 19) + grid_side_length = 21 + grid = Grid( + np.array([grid_side_length, grid_side_length]), + num_obstacles=40, + obstacle_avoid_points=[start, goal], + obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, + ) + + planner = SpaceTimeAStar(grid, start, goal) + path = planner.plan(verbose) + + if verbose: + print(f"Path: {path}") + + if not show_animation: + return + + fig = plt.figure(figsize=(10, 7)) + ax = fig.add_subplot( + autoscale_on=False, + xlim=(0, grid.grid_size[0] - 1), + ylim=(0, grid.grid_size[1] - 1), + ) + ax.set_aspect("equal") + ax.grid() + ax.set_xticks(np.arange(0, grid_side_length, 1)) + ax.set_yticks(np.arange(0, grid_side_length, 1)) + + (start_and_goal,) = ax.plot([], [], "mD", ms=15, label="Start and Goal") + start_and_goal.set_data([start.x, goal.x], [start.y, goal.y]) + (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles") + (path_points,) = ax.plot([], [], "bo", ms=10, label="Path Found") + ax.legend(bbox_to_anchor=(1.05, 1)) + + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect( + "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] + ) + + for i in range(0, path.goal_reached_time()): + obs_positions = grid.get_obstacle_positions_at_time(i) + obs_points.set_data(obs_positions[0], obs_positions[1]) + path_position = path.get_position(i) + path_points.set_data([path_position.x], [path_position.y]) + plt.pause(0.2) + plt.show() + + +if __name__ == "__main__": + main() diff --git a/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/Control/move_to_pose/__init__.py b/PathTracking/move_to_pose/__init__.py similarity index 100% rename from Control/move_to_pose/__init__.py rename to PathTracking/move_to_pose/__init__.py diff --git a/Control/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py similarity index 68% rename from Control/move_to_pose/move_to_pose.py rename to PathTracking/move_to_pose/move_to_pose.py index 279ba0625b..34736a2e21 100644 --- a/Control/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -5,6 +5,7 @@ Author: Daniel Ingram (daniel-s-ingram) Atsushi Sakai (@Atsushi_twi) Seied Muhammad Yazdian (@Muhammad-Yazdian) + Wang Zheng (@Aglargil) P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 @@ -12,8 +13,13 @@ import matplotlib.pyplot as plt import numpy as np -from random import random +import sys +import pathlib + +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from utils.angle import angle_mod +from random import random + class PathFinderController: """ @@ -70,14 +76,20 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal): # [-pi, pi] to prevent unstable behavior e.g. difference going # from 0 rad to 2*pi rad with slight turn + # Ref: The velocity v always has a constant sign which depends on the initial value of α. rho = np.hypot(x_diff, y_diff) - alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta) - beta = angle_mod(theta_goal - theta - alpha) v = self.Kp_rho * rho - w = self.Kp_alpha * alpha - self.Kp_beta * beta + alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta) + beta = angle_mod(theta_goal - theta - alpha) if alpha > np.pi / 2 or alpha < -np.pi / 2: + # recalculate alpha to make alpha in the range of [-pi/2, pi/2] + alpha = angle_mod(np.arctan2(-y_diff, -x_diff) - theta) + beta = angle_mod(theta_goal - theta - alpha) + w = self.Kp_alpha * alpha - self.Kp_beta * beta v = -v + else: + w = self.Kp_alpha * alpha - self.Kp_beta * beta return rho, v, w @@ -85,6 +97,7 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal): # simulation parameters controller = PathFinderController(9, 15, 3) dt = 0.01 +MAX_SIM_TIME = 5 # seconds, robot will stop moving when time exceeds this value # Robot specifications MAX_LINEAR_SPEED = 15 @@ -101,18 +114,19 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): x_diff = x_goal - x y_diff = y_goal - y - x_traj, y_traj = [], [] + x_traj, y_traj, v_traj, w_traj = [x], [y], [0], [0] rho = np.hypot(x_diff, y_diff) - while rho > 0.001: + t = 0 + while rho > 0.001 and t < MAX_SIM_TIME: + t += dt x_traj.append(x) y_traj.append(y) x_diff = x_goal - x y_diff = y_goal - y - rho, v, w = controller.calc_control_command( - x_diff, y_diff, theta, theta_goal) + rho, v, w = controller.calc_control_command(x_diff, y_diff, theta, theta_goal) if abs(v) > MAX_LINEAR_SPEED: v = np.sign(v) * MAX_LINEAR_SPEED @@ -120,18 +134,35 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): if abs(w) > MAX_ANGULAR_SPEED: w = np.sign(w) * MAX_ANGULAR_SPEED + v_traj.append(v) + w_traj.append(w) + theta = theta + w * dt x = x + v * np.cos(theta) * dt y = y + v * np.sin(theta) * dt if show_animation: # pragma: no cover plt.cla() - plt.arrow(x_start, y_start, np.cos(theta_start), - np.sin(theta_start), color='r', width=0.1) - plt.arrow(x_goal, y_goal, np.cos(theta_goal), - np.sin(theta_goal), color='g', width=0.1) + plt.arrow( + x_start, + y_start, + np.cos(theta_start), + np.sin(theta_start), + color="r", + width=0.1, + ) + plt.arrow( + x_goal, + y_goal, + np.cos(theta_goal), + np.sin(theta_goal), + color="g", + width=0.1, + ) plot_vehicle(x, y, theta, x_traj, y_traj) + return x_traj, y_traj, v_traj, w_traj + def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover # Corners of triangular vehicle when pointing to the right (0 radians) @@ -144,16 +175,16 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover p2 = np.matmul(T, p2_i) p3 = np.matmul(T, p3_i) - plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-') - plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-') - plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-') + plt.plot([p1[0], p2[0]], [p1[1], p2[1]], "k-") + plt.plot([p2[0], p3[0]], [p2[1], p3[1]], "k-") + plt.plot([p3[0], p1[0]], [p3[1], p1[1]], "k-") - plt.plot(x_traj, y_traj, 'b--') + plt.plot(x_traj, y_traj, "b--") # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( - 'key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] + ) plt.xlim(0, 20) plt.ylim(0, 20) @@ -162,15 +193,16 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover def transformation_matrix(x, y, theta): - return np.array([ - [np.cos(theta), -np.sin(theta), x], - [np.sin(theta), np.cos(theta), y], - [0, 0, 1] - ]) + return np.array( + [ + [np.cos(theta), -np.sin(theta), x], + [np.sin(theta), np.cos(theta), y], + [0, 0, 1], + ] + ) def main(): - for i in range(5): x_start = 20.0 * random() y_start = 20.0 * random() @@ -178,10 +210,14 @@ def main(): x_goal = 20 * random() y_goal = 20 * random() theta_goal = 2 * np.pi * random() - np.pi - print(f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n") - print(f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n") + print( + f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n" + ) + print( + f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n" + ) move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/Control/move_to_pose/move_to_pose_robot.py b/PathTracking/move_to_pose/move_to_pose_robot.py similarity index 100% rename from Control/move_to_pose/move_to_pose_robot.py rename to PathTracking/move_to_pose/move_to_pose_robot.py diff --git a/README.md b/README.md index cb1816c2b5..9e605435ce 100644 --- a/README.md +++ b/README.md @@ -89,6 +89,10 @@ See this documentation - [Getting Started — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/getting_started.html#what-is-pythonrobotics) +or this Youtube video: + +- [PythonRobotics project audio overview](https://www.youtube.com/watch?v=uMeRnNoJAfU) + or this paper for more details: - [\[1808\.10703\] PythonRobotics: a Python code collection of robotics algorithms](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib)) @@ -617,7 +621,7 @@ This is a list of user's comment and references:[users\_comments](https://github Any contribution is welcome!! -Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/how_to_contribute.html) +Please check this document:[How To Contribute — PythonRobotics documentation](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html) # Citing diff --git a/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/index_main.rst b/docs/index_main.rst index c1e8d22d32..65634f32e8 100644 --- a/docs/index_main.rst +++ b/docs/index_main.rst @@ -43,7 +43,8 @@ this graph shows GitHub star history of this project: modules/7_arm_navigation/arm_navigation modules/8_aerial_navigation/aerial_navigation modules/9_bipedal/bipedal - modules/10_control/control + modules/10_inverted_pendulum/inverted_pendulum + modules/13_mission_planning/mission_planning modules/11_utils/utils modules/12_appendix/appendix diff --git a/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst b/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst index 8c932b7263..2a7bd574f0 100644 --- a/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst +++ b/docs/modules/0_getting_started/1_what_is_python_robotics_main.rst @@ -110,6 +110,12 @@ the following additional libraries are required. For instructions on installing the above libraries, please refer to this section ":ref:`How to run sample codes`". +Audio overview of this project +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +For an audio overview of this project, please refer to this `YouTube video`_. + +.. _`YouTube video`: https://www.youtube.com/watch?v=uMeRnNoJAfU + Arxiv paper ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/docs/modules/0_getting_started/3_how_to_contribute_main.rst b/docs/modules/0_getting_started/3_how_to_contribute_main.rst index 6e5c1be8ee..1e61760649 100644 --- a/docs/modules/0_getting_started/3_how_to_contribute_main.rst +++ b/docs/modules/0_getting_started/3_how_to_contribute_main.rst @@ -9,10 +9,30 @@ There are several ways to contribute to this project as below: #. `Adding missed documentations for existing examples`_ #. `Supporting this project`_ +Before contributing +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Please check following items before contributing: + +Understanding this project +--------------------------- + +Please check this :ref:`What is PythonRobotics?` section and this paper +`PythonRobotics: a Python code collection of robotics algorithms`_ +to understand the philosophies of this project. + +.. _`PythonRobotics: a Python code collection of robotics algorithms`: https://arxiv.org/abs/1808.10703 + +Check your Python version. +--------------------------- + +We only accept a PR for Python 3.12.x or higher. + +We will not accept a PR for Python 2.x. .. _`Adding a new algorithm example`: -Adding a new algorithm example +1. Adding a new algorithm example ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This is a step by step manual to add a new algorithm example. @@ -112,8 +132,8 @@ Note that this is my hobby project; I appreciate your patience during the review .. _`Reporting and fixing a defect`: -Reporting and fixing a defect -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2. Reporting and fixing a defect +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Reporting and fixing a defect is also great contribution. @@ -136,8 +156,8 @@ This doc `submit a pull request`_ can be helpful to submit a pull request. .. _`Adding missed documentations for existing examples`: -Adding missed documentations for existing examples -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3. Adding missed documentations for existing examples +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Adding the missed documentations for existing examples is also great contribution. @@ -150,8 +170,8 @@ This doc `how to write doc`_ can be helpful to write documents. .. _`Supporting this project`: -Supporting this project -^^^^^^^^^^^^^^^^^^^^^^^^ +4. Supporting this project +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Supporting this project financially is also a great contribution!!. @@ -165,9 +185,9 @@ If you or your company would like to support this project, please consider: If you would like to support us in some other way, please contact with creating an issue. -Current Major Sponsors ------------------------ +Current Major Sponsors: +#. `GitHub`_ : They are providing a GitHub Copilot Pro license for this OSS development. #. `JetBrains`_ : They are providing a free license of their IDEs for this OSS development. #. `1Password`_ : They are providing a free license of their 1Password team license for this OSS project. @@ -183,6 +203,7 @@ Current Major Sponsors .. _`doc README`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/docs/README.md .. _`test_codestyle.py`: https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_codestyle.py .. _`JetBrains`: https://www.jetbrains.com/ +.. _`GitHub`: https://www.github.com/ .. _`Sponsor @AtsushiSakai on GitHub Sponsors`: https://github.com/sponsors/AtsushiSakai .. _`Become a backer or sponsor on Patreon`: https://www.patreon.com/myenigma .. _`One-time donation via PayPal`: https://www.paypal.com/paypalme/myenigmapay/ diff --git a/docs/modules/10_control/control_main.rst b/docs/modules/10_control/control_main.rst deleted file mode 100644 index cee2aa9e8e..0000000000 --- a/docs/modules/10_control/control_main.rst +++ /dev/null @@ -1,12 +0,0 @@ -.. _control: - -Control -================= - -.. toctree:: - :maxdepth: 2 - :caption: Contents - - inverted_pendulum_control/inverted_pendulum_control - move_to_a_pose_control/move_to_a_pose_control - diff --git a/docs/modules/10_control/inverted_pendulum_control/inverted-pendulum.png b/docs/modules/10_inverted_pendulum/inverted-pendulum.png similarity index 100% rename from docs/modules/10_control/inverted_pendulum_control/inverted-pendulum.png rename to docs/modules/10_inverted_pendulum/inverted-pendulum.png diff --git a/docs/modules/10_control/inverted_pendulum_control/inverted_pendulum_control_main.rst b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst similarity index 97% rename from docs/modules/10_control/inverted_pendulum_control/inverted_pendulum_control_main.rst rename to docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst index e41729fd61..048cbea9ac 100644 --- a/docs/modules/10_control/inverted_pendulum_control/inverted_pendulum_control_main.rst +++ b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst @@ -1,5 +1,7 @@ -Inverted Pendulum Control ------------------------------ +.. _`Inverted Pendulum`: + +Inverted Pendulum +------------------ An inverted pendulum on a cart consists of a mass :math:`m` at the top of a pole of length :math:`l` pivoted on a horizontally moving base as shown in the adjacent. diff --git a/docs/modules/11_utils/utils_main.rst b/docs/modules/11_utils/utils_main.rst index ff79a26205..95c982b077 100644 --- a/docs/modules/11_utils/utils_main.rst +++ b/docs/modules/11_utils/utils_main.rst @@ -1,4 +1,4 @@ -.. _utils: +.. _`utils`: Utilities ========== diff --git a/docs/modules/12_appendix/appendix_main.rst b/docs/modules/12_appendix/appendix_main.rst index cb1ac04066..d0b9eeea3a 100644 --- a/docs/modules/12_appendix/appendix_main.rst +++ b/docs/modules/12_appendix/appendix_main.rst @@ -1,4 +1,4 @@ -.. _appendix: +.. _`Appendix`: Appendix ============== @@ -10,4 +10,6 @@ Appendix steering_motion_model Kalmanfilter_basics Kalmanfilter_basics_2 + internal_sensors + external_sensors diff --git a/docs/modules/12_appendix/external_sensors_main.rst b/docs/modules/12_appendix/external_sensors_main.rst new file mode 100644 index 0000000000..3597418150 --- /dev/null +++ b/docs/modules/12_appendix/external_sensors_main.rst @@ -0,0 +1,62 @@ +.. _`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 +------------- + +TBD + + +Monocular Cameras +----------------- + +Monocular cameras utilize a single camera to recognize the external environment. Compared to other sensors, they can detect color and brightness information, making them primarily useful for object recognition. However, they face challenges in independently measuring distances to surrounding objects and may struggle in low-light or dark conditions. + +Requirements for Cameras and Image Processing in Robotics +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +While camera sensors are widely used in applications like surveillance, deploying them in robotics necessitates meeting specific requirements: + +1. High dynamic range to adapt to various lighting conditions +2. Wide measurement range +3. Capability for three-dimensional measurement through techniques like motion stereo +4. Real-time processing with high frame rates +5. Cost-effectiveness + +Stereo Cameras +-------------- + +Stereo cameras employ multiple cameras to measure distances to surrounding objects. By knowing the positions and orientations of each camera and analyzing the disparity in the images (parallax), the distance to a specific point (the object represented by a particular pixel) can be calculated. + +Characteristics of Stereo Cameras +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Advantages of stereo cameras include the ability to obtain high-precision and high-density distance information at close range, depending on factors like camera resolution and the distance between cameras (baseline). This makes them suitable for indoor robots that require precise shape recognition of nearby objects. Additionally, stereo cameras are relatively cost-effective compared to other sensors, leading to their use in consumer products like Subaru's EyeSight system. However, stereo cameras are less effective for long-distance measurements due to a decrease in accuracy proportional to the square of the distance. They are also susceptible to environmental factors such as lighting conditions. + +Ultrasonic Sensors +------------------ + +Ultrasonic sensors are commonly used in indoor robots and some automotive autonomous driving systems. Their features include affordability compared to laser or radar sensors, the ability to detect very close objects, and the capability to sense materials like glass, which may be challenging for lasers or cameras. However, they have limitations such as shorter maximum measurement distances and lower resolution and accuracy. + +References +---------- + +TBD 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..18f209098e --- /dev/null +++ b/docs/modules/12_appendix/internal_sensors_main.rst @@ -0,0 +1,34 @@ +.. _`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 +------------ + +Global Navigation Satellite System (GNSS) +------------------------------------------- + +Gyroscope +---------- + +Accelerometer +-------------- + +Magnetometer +-------------- + +Inertial Measurement Unit (IMU) +-------------------------------- + +Pressure Sensor +----------------- + +Temperature Sensor +-------------------- + + diff --git a/docs/modules/13_mission_planning/mission_planning_main.rst b/docs/modules/13_mission_planning/mission_planning_main.rst new file mode 100644 index 0000000000..385e62f68e --- /dev/null +++ b/docs/modules/13_mission_planning/mission_planning_main.rst @@ -0,0 +1,12 @@ +.. _`Mission Planning`: + +Mission Planning +================ + +Mission planning includes tools such as finite state machines and behavior trees used to describe robot behavior and high level task planning. + +.. toctree:: + :maxdepth: 2 + :caption: Contents + + state_machine/state_machine diff --git a/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png b/docs/modules/13_mission_planning/state_machine/robot_behavior_case.png new file mode 100644 index 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..abaece1b11 --- /dev/null +++ b/docs/modules/13_mission_planning/state_machine/state_machine_main.rst @@ -0,0 +1,74 @@ +State Machine +------------- + +A state machine is a model used to describe the transitions of an object between different states. It clearly shows how an object changes state based on events and may trigger corresponding actions. + +Core Concepts +~~~~~~~~~~~~~ + +- **State**: A distinct mode or condition of the system (e.g. "Idle", "Running"). Managed by State class with optional on_enter/on_exit callbacks +- **Event**: A trigger signal that may cause state transitions (e.g. "start", "stop") +- **Transition**: A state change path from source to destination state triggered by an event +- **Action**: An operation executed during transition (before entering new state) +- **Guard**: A precondition that must be satisfied to allow transition + +API +~~~ + +.. autoclass:: MissionPlanning.StateMachine.state_machine.StateMachine + :members: add_transition, process, register_state + :special-members: __init__ + +PlantUML Support +~~~~~~~~~~~~~~~~ + +The ``generate_plantuml()`` method creates diagrams showing: + +- Current state (marked with [*] arrow) +- All possible transitions +- Guard conditions in [brackets] +- Actions prefixed with / + +Example +~~~~~~~ + +state machine diagram: ++++++++++++++++++++++++ +.. image:: robot_behavior_case.png + +state transition table: ++++++++++++++++++++++++ +.. list-table:: State Transitions + :header-rows: 1 + :widths: 20 15 20 20 20 + + * - Source State + - Event + - Target State + - Guard + - Action + * - patrolling + - detect_task + - executing_task + - + - + * - executing_task + - task_complete + - patrolling + - + - reset_task + * - executing_task + - low_battery + - returning_to_base + - is_battery_low + - + * - returning_to_base + - reach_base + - charging + - + - + * - charging + - charge_complete + - patrolling + - + - \ No newline at end of file diff --git a/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst b/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst index 2f31834017..ca595301a6 100644 --- a/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst +++ b/docs/modules/1_introduction/1_definition_of_robotics/definition_of_robotics_main.rst @@ -1,7 +1,107 @@ Definition of Robotics ---------------------- -In recent years, autonomous navigation technologies have received huge -attention in many fields. -Such fields include, autonomous driving[22], drone flight navigation, -and other transportation systems. +This section explains the definition, history, key components, and applications of robotics. + +What is Robotics? +^^^^^^^^^^^^^^^^^^ + +Robot is a machine that can perform tasks automatically or semi-autonomously. +Robotics is the study of robots. + +The word “robot” comes from the Czech word “robota,” which means “forced labor” or “drudgery.” +It was first used in the 1920 science fiction play `R.U.R.`_ (Rossum’s Universal Robots) +by the Czech writer `Karel Čapek`_. +In the play, robots were artificial workers created to serve humans, but they eventually rebelled. + +Over time, “robot” came to refer to machines or automated systems that can perform tasks, +often with some level of intelligence or autonomy. + +Currently, 2 millions robots are working in the world, and the number is increasing every year. +In South Korea, where the adoption of robots is particularly rapid, +50 robots are operating per 1,000 people. + +.. _`R.U.R.`: https://thereader.mitpress.mit.edu/origin-word-robot-rur/ +.. _`Karel Čapek`: https://en.wikipedia.org/wiki/Karel_%C4%8Capek + +The History of Robots +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This timeline highlights key milestones in the history of robotics: + +Ancient and Early Concepts (Before 1500s) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The idea of **automated machines** has existed for thousands of years. +Ancient civilizations imagined mechanical beings: + +- **Ancient Greece (4th Century BC)** – Greek engineer `Hero of Alexandria`_ designed early **automata** (self-operating machines) powered by water or air. +- **Chinese and Arabic Automata (9th–13th Century)** – Inventors like `Ismail Al-Jazari`_ created intricate mechanical devices, including water clocks and automated moving peacocks driven by hydropower. + +.. _`Hero of Alexandria`: https://en.wikipedia.org/wiki/Hero_of_Alexandria +.. _`Ismail Al-Jazari`: https://en.wikipedia.org/wiki/Ismail_al-Jazari + +The Birth of Modern Robotics (1500s–1800s) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +- `Leonardo da Vinci’s Robot`_ (1495) – Designed a humanoid knight with mechanical movement. +- `Jacques de Vaucanson’s Digesting Duck`_ (1738) – Created robotic figures like a mechanical duck that could "eat" and "digest." +- `Industrial Revolution`_ (18th–19th Century) – Machines began replacing human labor in factories, setting the foundation for automation. + +.. _`Leonardo da Vinci’s Robot`: https://en.wikipedia.org/wiki/Leonardo%27s_robot +.. _`Jacques de Vaucanson’s Digesting Duck`: https://en.wikipedia.org/wiki/Jacques_de_Vaucanson +.. _`Industrial Revolution`: https://en.wikipedia.org/wiki/Industrial_Revolution + +The Rise of Industrial Robots (1900s–1950s) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +- **The Term “Robot” (1921)** – Czech writer `Karel Čapek`_ introduced the word *“robot”* in his play `R.U.R.`_ (Rossum’s Universal Robots). +- **Early Cybernetics (1940s–1950s)** – Scientists like `Norbert Wiener`_ developed theories of self-regulating machines, influencing modern robotics (Cybernetics). + +.. _`Norbert Wiener`: https://en.wikipedia.org/wiki/Norbert_Wiener + +The Birth of Modern Robotics (1950s–1980s) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +- **First Industrial Robot (1961)** – `Unimate`_, created by `George Devol`_ and `Joseph Engelberger`_, was the first programmable robot used in a factory. +- **Rise of AI & Autonomous Robots (1970s–1980s)** – Researchers developed mobile robots like `Shakey`_ (Stanford, 1966) and AI-based control systems. + +.. _`Unimate`: https://en.wikipedia.org/wiki/Unimate +.. _`George Devol`: https://en.wikipedia.org/wiki/George_Devol +.. _`Joseph Engelberger`: https://en.wikipedia.org/wiki/Joseph_Engelberger +.. _`Shakey`: https://en.wikipedia.org/wiki/Shakey_the_robot + +Advanced Robotics and AI Integration (1990s–Present) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +- **Industrial Robots** – Advanced robots like `Baxter`_ and `Amazon Robotics`_ revolutionized manufacturing and logistics. +- **Autonomous Vehicles** – Self-driving cars for robo taxi like `Waymo`_ and autonomous haulage system in mining like `AHS`_ became more advanced and bisiness-ready. +- **Exploration Robots** – Used for planetary exploration (e.g., NASA’s `Mars rovers`_). +- **Medical Robotics** – Robots like `da Vinci Surgical System`_ revolutionized healthcare. +- **Personal Robots** – Devices like `Roomba`_ (vacuum robot) and `Sophia`_ (AI humanoid) became popular. +- **Service Robots** - Assistive robots like serving robots in restaurants and hotels like `Bellabot`_. +- **Collaborative Robots (Drones)** – Collaborative robots like UAV (Unmanned Aerial Vehicle) in drone shows and delivery services. + +.. _`Baxter`: https://en.wikipedia.org/wiki/Baxter_(robot) +.. _`Amazon Robotics`: https://en.wikipedia.org/wiki/Amazon_Robotics +.. _`Mars rovers`: https://en.wikipedia.org/wiki/Mars_rover +.. _`Waymo`: https://waymo.com/ +.. _`AHS`: https://www.futurebridge.com/industry/perspectives-industrial-manufacturing/autonomous-haulage-systems-the-future-of-mining-operations/ +.. _`da Vinci Surgical System`: https://en.wikipedia.org/wiki/Da_Vinci_Surgical_System +.. _`Roomba`: https://en.wikipedia.org/wiki/Roomba +.. _`Sophia`: https://en.wikipedia.org/wiki/Sophia_(robot) +.. _`Bellabot`: https://www.pudurobotics.com/en + +Key Components of Robotics +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Robotics consists of several essential components: + +#. Sensors – Gather information from the environment (e.g., Cameras, LiDAR, GNSS, Gyro, Accelerometer, Wheel encoders). +#. Actuators – Enable movement and interaction with the world (e.g., Motors, Hydraulic systems). +#. Computers – Process sensor data and make decisions (e.g., Micro-controllers, CPUs, GPUs). +#. Power Supply – Provides energy to run the robot (e.g., Batteries, Solar power). +#. Software & Algorithms – Allow the robot to function and make intelligent decisions (e.g., ROS, Machine learning models, Localization, Mapping, Path planning, Control). + +This project, `PythonRobotics`, focuses on the software and algorithms part of robotics. +If you are interested in `Sensors` hardware, you can check :ref:`Internal Sensors for Robots` or :ref:`External Sensors for Robots`. diff --git a/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst b/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst 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/2_software_for_robotics/software_for_robotics_main.rst b/docs/modules/1_introduction/2_software_for_robotics/software_for_robotics_main.rst deleted file mode 100644 index 835441f85d..0000000000 --- a/docs/modules/1_introduction/2_software_for_robotics/software_for_robotics_main.rst +++ /dev/null @@ -1,7 +0,0 @@ -Software for Robotics ----------------------- - -Python for Robotics -~~~~~~~~~~~~~~~~~~~~~ - -TBD diff --git a/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst new file mode 100644 index 0000000000..0ed51e961b --- /dev/null +++ b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst @@ -0,0 +1,68 @@ +Technologies for Robotics +------------------------- + +The field of robotics needs wide areas of technologies such as mechanical engineering, +electrical engineering, computer science, and artificial intelligence (AI). +This project, `PythonRobotics`, only focus on computer science and artificial intelligence. + +The technologies for robotics are categorized as following 3 categories: + +#. `Autonomous Navigation`_ +#. `Manipulation`_ +#. `Robot type specific technologies`_ + +.. _`Autonomous Navigation`: + +Autonomous Navigation +^^^^^^^^^^^^^^^^^^^^^^^^ +Autonomous navigation is a capability that can move to a goal over long +periods of time without any external control by an operator. + +To achieve autonomous navigation, the robot needs to have the following technologies: +- It needs to know where it is (localization) +- Where it is safe (mapping) +- Where is is safe and where the robot is in the map (Simultaneous Localization and Mapping (SLAM)) +- Where and how to move (path planning) +- How to control its motion (path following). + +The autonomous system would not work correctly if any of these technologies is missing. + +In recent years, autonomous navigation technologies have received huge +attention in many fields. +For example, self-driving cars, drones, and autonomous mobile robots in indoor and outdoor environments. + +In this project, we provide many algorithms, sample codes, +and documentations for autonomous navigation. + +#. :ref:`Localization` +#. :ref:`Mapping` +#. :ref:`SLAM` +#. :ref:`Path planning` +#. :ref:`Path tracking` + + + +.. _`Manipulation`: + +Manipulation +^^^^^^^^^^^^^^^^^^^^^^^^ + +#. :ref:`Arm Navigation` + +.. _`Robot type specific technologies`: + +Robot type specific technologies +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +#. :ref:`Aerial Navigation` +#. :ref:`Bipedal` +#. :ref:`Inverted Pendulum` + + +Additional Information +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +#. :ref:`utils` +#. :ref:`Appendix` + + diff --git a/docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst b/docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst deleted file mode 100644 index 4dd1d1842f..0000000000 --- a/docs/modules/1_introduction/3_technology_for_robotics/technology_for_robotics_main.rst +++ /dev/null @@ -1,12 +0,0 @@ -Technology for Robotics -------------------------- - -An autonomous navigation system is a system that can move to a goal over long -periods of time without any external control by an operator. -The system requires a wide range of technologies: -- It needs to know where it is (localization) -- Where it is safe (mapping) -- Where and how to move (path planning) -- How to control its motion (path following). - -The autonomous system would not work correctly if any of these technologies is missing. diff --git a/docs/modules/1_introduction/introduction_main.rst b/docs/modules/1_introduction/introduction_main.rst index ec1f237545..1871dfc3b1 100644 --- a/docs/modules/1_introduction/introduction_main.rst +++ b/docs/modules/1_introduction/introduction_main.rst @@ -3,11 +3,16 @@ 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_software_for_robotics/software_for_robotics - 3_technology_for_robotics/technology_for_robotics + 2_python_for_robotics/python_for_robotics + 3_technologies_for_robotics/technologies_for_robotics diff --git a/docs/modules/2_localization/localization_main.rst b/docs/modules/2_localization/localization_main.rst index 22cbd094da..770a234b69 100644 --- a/docs/modules/2_localization/localization_main.rst +++ b/docs/modules/2_localization/localization_main.rst @@ -1,4 +1,4 @@ -.. _localization: +.. _`Localization`: Localization ============ diff --git a/docs/modules/3_mapping/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..0ef9e3022f --- /dev/null +++ b/docs/modules/3_mapping/distance_map/distance_map_main.rst @@ -0,0 +1,27 @@ +Distance Map +------------ + +This is an implementation of the Distance Map algorithm for path planning. + +The Distance Map algorithm computes the unsigned distance field (UDF) and signed distance field (SDF) from a boolean field representing obstacles. + +The UDF gives the distance from each point to the nearest obstacle. The SDF gives positive distances for points outside obstacles and negative distances for points inside obstacles. + +Example +~~~~~~~ + +The algorithm is demonstrated on a simple 2D grid with obstacles: + +.. image:: distance_map.png + +API +~~~ + +.. autofunction:: 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/3_mapping/mapping_main.rst b/docs/modules/3_mapping/mapping_main.rst index 28e18984d3..34a3744893 100644 --- a/docs/modules/3_mapping/mapping_main.rst +++ b/docs/modules/3_mapping/mapping_main.rst @@ -1,4 +1,4 @@ -.. _mapping: +.. _`Mapping`: Mapping ======= @@ -17,3 +17,4 @@ Mapping is the ability of a robot to understand its surroundings with external s circle_fitting/circle_fitting rectangle_fitting/rectangle_fitting normal_vector_estimation/normal_vector_estimation + distance_map/distance_map diff --git a/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst index 491320512b..15963aff79 100644 --- a/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst +++ b/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst @@ -165,7 +165,7 @@ different data sources into a single optimization problem. 6 215.8405 -0.000000 -.. figure:: graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/Graph_SLAM_optimization.gif .. code:: ipython3 diff --git a/docs/modules/4_slam/slam_main.rst b/docs/modules/4_slam/slam_main.rst index dec04f253a..98211986c2 100644 --- a/docs/modules/4_slam/slam_main.rst +++ b/docs/modules/4_slam/slam_main.rst @@ -1,4 +1,4 @@ -.. _slam: +.. _`SLAM`: SLAM ==== diff --git a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst new file mode 100644 index 0000000000..8a3e517105 --- /dev/null +++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst @@ -0,0 +1,74 @@ +Elastic Bands +------------- + +This is a path planning with Elastic Bands. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ElasticBands/animation.gif + + +Core Concept +~~~~~~~~~~~~ +- **Elastic Band**: A dynamically deformable collision-free path initialized by a global planner. +- **Objective**: + + * Shorten and smooth the path. + * Maximize obstacle clearance. + * Maintain global path connectivity. + +Bubble Representation +~~~~~~~~~~~~~~~~~~~~~~~~ +- **Definition**: A local free-space region around configuration :math:`b`: + + .. math:: + B(b) = \{ q: \|q - b\| < \rho(b) \}, + + where :math:`\rho(b)` is the radius of the bubble. + + +Force-Based Deformation +~~~~~~~~~~~~~~~~~~~~~~~ +The elastic band deforms under artificial forces: + +Internal Contraction Force +++++++++++++++++++++++++++ +- **Purpose**: Reduces path slack and length. +- **Formula**: For node :math:`b_i`: + + .. math:: + f_c(b_i) = k_c \left( \frac{b_{i-1} - b_i}{\|b_{i-1} - b_i\|} + \frac{b_{i+1} - b_i}{\|b_{i+1} - b_i\|} \right) + + where :math:`k_c` is the contraction gain. + +External Repulsion Force ++++++++++++++++++++++++++ +- **Purpose**: Pushes the path away from obstacles. +- **Formula**: For node :math:`b_i`: + + .. math:: + f_r(b_i) = \begin{cases} + k_r (\rho_0 - \rho(b_i)) \nabla \rho(b_i) & \text{if } \rho(b_i) < \rho_0, \\ + 0 & \text{otherwise}. + \end{cases} + + where :math:`k_r` is the repulsion gain, :math:`\rho_0` is the maximum distance for applying repulsion force, and :math:`\nabla \rho(b_i)` is approximated via finite differences: + + .. math:: + \frac{\partial \rho}{\partial x} \approx \frac{\rho(b_i + h) - \rho(b_i - h)}{2h}. + +Dynamic Path Maintenance +~~~~~~~~~~~~~~~~~~~~~~~~~~ +1. **Node Update**: + + .. math:: + b_i^{\text{new}} = b_i^{\text{old}} + \alpha (f_c + f_r), + + where :math:`\alpha` is a step-size parameter, which often proportional to :math:`\rho(b_i^{\text{old}})` + +2. **Overlap Enforcement**: +- Insert new nodes if adjacent nodes are too far apart +- Remove redundant nodes if adjacent nodes are too close + +References +~~~~~~~~~~~~~~~~~~~~~~~ + +- `Elastic Bands: Connecting Path Planning and Control `__ diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst index 4960330b3e..0c84a19c22 100644 --- a/docs/modules/5_path_planning/path_planning_main.rst +++ b/docs/modules/5_path_planning/path_planning_main.rst @@ -1,4 +1,4 @@ -.. _path_planning: +.. _`Path Planning`: Path Planning ============= @@ -12,6 +12,7 @@ Path planning is the ability of a robot to search feasible and efficient path to dynamic_window_approach/dynamic_window_approach bugplanner/bugplanner grid_base_search/grid_base_search + time_based_grid_search/time_based_grid_search model_predictive_trajectory_generator/model_predictive_trajectory_generator state_lattice_planner/state_lattice_planner prm_planner/prm_planner @@ -31,3 +32,4 @@ Path planning is the ability of a robot to search feasible and efficient path to hybridastar/hybridastar frenet_frame_path/frenet_frame_path coverage_path/coverage_path + elastic_bands/elastic_bands \ No newline at end of file diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst new file mode 100644 index 0000000000..0c26badec7 --- /dev/null +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -0,0 +1,22 @@ +Time based grid search +---------------------- + +Space-time astar +~~~~~~~~~~~~~~~~~~~~~~ + +This is an extension of A* algorithm that supports planning around dynamic obstacles. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar/path_animation.gif + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar/path_animation2.gif + +The key difference of this algorithm compared to vanilla A* is that the cost and heuristic are now time-based instead of distance-based. +Using a time-based cost and heuristic ensures the path found is optimal in terms of time to reach the goal. + +The cost is the amount of time it takes to reach a given node, and the heuristic is the minimum amount of time it could take to reach the goal from that node, disregarding all obstacles. +For a simple scenario where the robot can move 1 cell per time step and stop and go as it pleases, the heuristic for time is equivalent to the heuristic for distance. + +References: +~~~~~~~~~~~ + +- `Cooperative Pathfinding `__ diff --git a/docs/modules/10_control/move_to_a_pose_control/move_to_a_pose_control_main.rst b/docs/modules/6_path_tracking/move_to_a_pose_control/move_to_a_pose_control_main.rst similarity index 100% rename from docs/modules/10_control/move_to_a_pose_control/move_to_a_pose_control_main.rst rename to docs/modules/6_path_tracking/move_to_a_pose_control/move_to_a_pose_control_main.rst diff --git a/docs/modules/6_path_tracking/path_tracking_main.rst b/docs/modules/6_path_tracking/path_tracking_main.rst index d7a895b562..d98e324583 100644 --- a/docs/modules/6_path_tracking/path_tracking_main.rst +++ b/docs/modules/6_path_tracking/path_tracking_main.rst @@ -1,4 +1,4 @@ -.. _path_tracking: +.. _`Path Tracking`: Path Tracking ============= @@ -16,3 +16,4 @@ Path tracking is the ability of a robot to follow the reference path generated b lqr_speed_and_steering_control/lqr_speed_and_steering_control model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control cgmres_nmpc/cgmres_nmpc + move_to_a_pose_control/move_to_a_pose_control diff --git a/docs/modules/7_arm_navigation/arm_navigation_main.rst b/docs/modules/7_arm_navigation/arm_navigation_main.rst index bbbc872c58..7acd3ee7d3 100644 --- a/docs/modules/7_arm_navigation/arm_navigation_main.rst +++ b/docs/modules/7_arm_navigation/arm_navigation_main.rst @@ -1,4 +1,4 @@ -.. _arm_navigation: +.. _`Arm Navigation`: Arm Navigation ============== diff --git a/docs/modules/8_aerial_navigation/aerial_navigation_main.rst b/docs/modules/8_aerial_navigation/aerial_navigation_main.rst index b2ccb071af..7f76689770 100644 --- a/docs/modules/8_aerial_navigation/aerial_navigation_main.rst +++ b/docs/modules/8_aerial_navigation/aerial_navigation_main.rst @@ -1,4 +1,4 @@ -.. _aerial_navigation: +.. _`Aerial Navigation`: Aerial Navigation ================= diff --git a/docs/modules/9_bipedal/bipedal_main.rst b/docs/modules/9_bipedal/bipedal_main.rst index fc5b933055..dc387dc4e8 100644 --- a/docs/modules/9_bipedal/bipedal_main.rst +++ b/docs/modules/9_bipedal/bipedal_main.rst @@ -1,4 +1,4 @@ -.. _bipedal: +.. _`Bipedal`: Bipedal ================= diff --git a/requirements/requirements.txt b/requirements/requirements.txt index e8bed7b6d7..552bf8482b 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -1,8 +1,8 @@ -numpy == 2.2.2 -scipy == 1.15.1 -matplotlib == 3.10.0 +numpy == 2.2.3 +scipy == 1.15.2 +matplotlib == 3.10.1 cvxpy == 1.5.3 -pytest == 8.3.4 # For unit test +pytest == 8.3.5 # For unit test pytest-xdist == 3.6.1 # For unit test -mypy == 1.14.1 # For unit test -ruff == 0.9.3 # For unit test +mypy == 1.15.0 # For unit test +ruff == 0.9.9 # For unit test diff --git a/tests/test_distance_map.py b/tests/test_distance_map.py new file mode 100644 index 0000000000..df6e394e2c --- /dev/null +++ b/tests/test_distance_map.py @@ -0,0 +1,118 @@ +import conftest # noqa +import numpy as np +from Mapping.DistanceMap import distance_map as m + + +def test_compute_sdf(): + """Test the computation of Signed Distance Field (SDF)""" + # Create a simple obstacle map for testing + obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]]) + + sdf = m.compute_sdf(obstacles) + + # Verify basic properties of SDF + assert sdf.shape == obstacles.shape, "SDF should have the same shape as input map" + assert np.all(np.isfinite(sdf)), "SDF should not contain infinite values" + + # Verify SDF value is negative at obstacle position + assert sdf[1, 1] < 0, "SDF value should be negative at obstacle position" + + # Verify SDF value is positive in free space + assert sdf[0, 0] > 0, "SDF value should be positive in free space" + + +def test_compute_udf(): + """Test the computation of Unsigned Distance Field (UDF)""" + # Create obstacle map for testing + obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]]) + + udf = m.compute_udf(obstacles) + + # Verify basic properties of UDF + assert udf.shape == obstacles.shape, "UDF should have the same shape as input map" + assert np.all(np.isfinite(udf)), "UDF should not contain infinite values" + assert np.all(udf >= 0), "All UDF values should be non-negative" + + # Verify UDF value is 0 at obstacle position + assert np.abs(udf[1, 1]) < 1e-10, "UDF value should be 0 at obstacle position" + + # Verify UDF value is 1 for adjacent cells + assert np.abs(udf[0, 1] - 1.0) < 1e-10, ( + "UDF value should be 1 for cells adjacent to obstacle" + ) + assert np.abs(udf[1, 0] - 1.0) < 1e-10, ( + "UDF value should be 1 for cells adjacent to obstacle" + ) + assert np.abs(udf[1, 2] - 1.0) < 1e-10, ( + "UDF value should be 1 for cells adjacent to obstacle" + ) + assert np.abs(udf[2, 1] - 1.0) < 1e-10, ( + "UDF value should be 1 for cells adjacent to obstacle" + ) + + +def test_dt(): + """Test the computation of 1D distance transform""" + # Create test data + d = np.array([m.INF, 0, m.INF]) + m.dt(d) + + # Verify distance transform results + assert np.all(np.isfinite(d)), ( + "Distance transform result should not contain infinite values" + ) + assert d[1] == 0, "Distance at obstacle position should be 0" + assert d[0] == 1, "Distance at adjacent position should be 1" + assert d[2] == 1, "Distance at adjacent position should be 1" + + +def test_compute_sdf_empty(): + """Test SDF computation with empty map""" + # Test with empty map (no obstacles) + empty_map = np.zeros((5, 5)) + sdf = m.compute_sdf(empty_map) + + assert np.all(sdf > 0), "All SDF values should be positive for empty map" + assert sdf.shape == empty_map.shape, "Output shape should match input shape" + + +def test_compute_sdf_full(): + """Test SDF computation with fully occupied map""" + # Test with fully occupied map + full_map = np.ones((5, 5)) + sdf = m.compute_sdf(full_map) + + assert np.all(sdf < 0), "All SDF values should be negative for fully occupied map" + assert sdf.shape == full_map.shape, "Output shape should match input shape" + + +def test_compute_udf_invalid_input(): + """Test UDF computation with invalid input values""" + # Test with invalid values (not 0 or 1) + invalid_map = np.array([[0, 2, 0], [0, -1, 0], [0, 0.5, 0]]) + + try: + m.compute_udf(invalid_map) + assert False, "Should raise ValueError for invalid input values" + except ValueError: + pass + + +def test_compute_udf_empty(): + """Test UDF computation with empty map""" + # Test with empty map + empty_map = np.zeros((5, 5)) + udf = m.compute_udf(empty_map) + + assert np.all(udf > 0), "All UDF values should be positive for empty map" + assert np.all(np.isfinite(udf)), "UDF should not contain infinite values" + + +def test_main(): + """Test the execution of main function""" + m.ENABLE_PLOT = False + m.main() + + +if __name__ == "__main__": + conftest.run_this_test(__file__) diff --git a/tests/test_elastic_bands.py b/tests/test_elastic_bands.py new file mode 100644 index 0000000000..ad4e13af1a --- /dev/null +++ b/tests/test_elastic_bands.py @@ -0,0 +1,23 @@ +import conftest +import numpy as np +from PathPlanning.ElasticBands.elastic_bands import ElasticBands + + +def test_1(): + path = np.load("PathPlanning/ElasticBands/path.npy") + obstacles_points = np.load("PathPlanning/ElasticBands/obstacles.npy") + obstacles = np.zeros((500, 500)) + for x, y in obstacles_points: + size = 30 # Side length of the square + half_size = size // 2 + x_start = max(0, x - half_size) + x_end = min(obstacles.shape[0], x + half_size) + y_start = max(0, y - half_size) + y_end = min(obstacles.shape[1], y + half_size) + obstacles[x_start:x_end, y_start:y_end] = 1 + elastic_bands = ElasticBands(path, obstacles) + elastic_bands.update_bubbles() + + +if __name__ == "__main__": + conftest.run_this_test(__file__) diff --git a/tests/test_inverted_pendulum_lqr_control.py b/tests/test_inverted_pendulum_lqr_control.py index cbbabb93b1..62afda71c3 100644 --- a/tests/test_inverted_pendulum_lqr_control.py +++ b/tests/test_inverted_pendulum_lqr_control.py @@ -1,5 +1,5 @@ import conftest -from Control.inverted_pendulum import inverted_pendulum_lqr_control as m +from InvertedPendulum import inverted_pendulum_lqr_control as m def test_1(): diff --git a/tests/test_inverted_pendulum_mpc_control.py b/tests/test_inverted_pendulum_mpc_control.py index 800aefd7d5..94859c2e0a 100644 --- a/tests/test_inverted_pendulum_mpc_control.py +++ b/tests/test_inverted_pendulum_mpc_control.py @@ -1,6 +1,6 @@ import conftest -from Control.inverted_pendulum import inverted_pendulum_mpc_control as m +from InvertedPendulum import inverted_pendulum_mpc_control as m def test1(): diff --git a/tests/test_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_space_time_astar.py b/tests/test_space_time_astar.py new file mode 100644 index 0000000000..5290738eb4 --- /dev/null +++ b/tests/test_space_time_astar.py @@ -0,0 +1,33 @@ +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + ObstacleArrangement, + Position, +) +from PathPlanning.TimeBasedPathPlanning import SpaceTimeAStar as m +import numpy as np +import conftest + + +def test_1(): + start = Position(1, 11) + goal = Position(19, 19) + grid_side_length = 21 + grid = Grid( + np.array([grid_side_length, grid_side_length]), + obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, + ) + + m.show_animation = False + planner = m.SpaceTimeAStar(grid, start, goal) + + path = planner.plan(False) + + # path should have 28 entries + assert len(path.path) == 31 + + # path should end at the goal + assert path.path[-1].position == goal + + +if __name__ == "__main__": + conftest.run_this_test(__file__) 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__)