Merge pull request #3 from SchwartzCode/update_master

Update master

Author: SchwartzCode

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) 

Control/inverted_pendulum/inverted_pendulum_lqr_control.py renamed to InvertedPendulum/inverted_pendulum_lqr_control.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()

Control/inverted_pendulum/inverted_pendulum_mpc_control.py renamed to InvertedPendulum/inverted_pendulum_mpc_control.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}")