python-motion-planning 1.0__py3-none-any.whl

utils/helper/math_helper.py

@@ -0,0 +1,65 @@
+"""
+@file: math_helper.py
+@breif: Contains common/commonly used math function
+@author: Yang Haodong, Wu Maojia
+@update: 2024.5.20
+"""
+import math 
+
+class MathHelper:
+    @staticmethod
+    def circleSegmentIntersection(p1: tuple, p2: tuple, r: float) -> list:
+        x1, x2 = p1[0], p2[0]
+        y1, y2 = p1[1], p2[1]
+
+        dx, dy = x2 - x1, y2 - y1
+        dr2 = dx * dx + dy * dy
+        D = x1 * y2 - x2 * y1
+
+        # the first element is the point within segment
+        d1 = x1 * x1 + y1 * y1
+        d2 = x2 * x2 + y2 * y2
+        dd = d2 - d1
+
+        delta_2 = r * r * dr2 - D * D
+        if delta_2 < 0:  # no intersection
+            return []
+
+        delta = math.sqrt(delta_2)
+        if (delta == 0):
+            return [(D * dy / dr2, -D * dx / dr2)]
+        else:   # delta > 0
+            return [
+                ((D * dy + math.copysign(1.0, dd) * dx * delta) / dr2,
+                (-D * dx + math.copysign(1.0, dd) * dy * delta) / dr2),
+                ((D * dy - math.copysign(1.0, dd) * dx * delta) / dr2,
+                (-D * dx - math.copysign(1.0, dd) * dy * delta) / dr2)
+            ]
+
+    @staticmethod
+    def closestPointOnLine(a: tuple, b: tuple, p: tuple = (0.0, 0.0)) -> tuple:
+        """
+        Find the closest intersection point (foot of a perpendicular) between point p and the line ab.
+
+        Parameters:
+            a (tuple): point a of the line
+            b (tuple): point b of the line
+            p (tuple): point p to find the closest intersection point
+
+        References:
+            [1] method 2 of https://www.youtube.com/watch?v=TPDgB6136ZE
+        """
+        ap = (p[0] - a[0], p[1] - a[1])
+        ab = (b[0] - a[0], b[1] - a[1])
+        af_coef = (ap[0] * ab[0] + ap[1] * ab[1]) / (ab[0] ** 2 + ab[1] ** 2)
+        af = (af_coef * ab[0], af_coef * ab[1])
+        f = (a[0] + af[0], a[1] + af[1])
+        return f
+    
+    @staticmethod
+    def clamp(val: float, min_val: float, max_val: float) -> float:
+        if val < min_val:
+            val = min_val
+        if val > max_val :
+            val = max_val
+        return val

utils/planner/control_factory.py

@@ -0,0 +1,31 @@
+"""
+@file: control_factory.py
+@breif: Facotry class for local planner.
+@author: Winter
+@update: 2023.10.24
+"""
+from python_motion_planning.local_planner import *
+
+class ControlFactory(object):
+    def __init__(self) -> None:
+        pass
+
+    def __call__(self, planner_name, **config):
+        if planner_name == "dwa":
+            return DWA(**config)
+        elif planner_name == "pid":
+            return PID(**config)
+        elif planner_name == "apf":
+            return APF(**config)
+        elif planner_name == "rpp":
+            return RPP(**config)
+        elif planner_name == "lqr":
+            return LQR(**config)
+        elif planner_name == "mpc":
+            return MPC(**config)
+        elif planner_name == "ddpg":
+            return DDPG(actor_load_path="models/actor_best_example.pth",
+                        critic_load_path="models/critic_best_example.pth",
+                        **config)
+        else:
+            raise ValueError("The `planner_name` must be set correctly.")

utils/planner/curve_factory.py

@@ -0,0 +1,29 @@
+"""
+@file: curve_factory.py
+@breif: Facotry class for curve generation.
+@author: Winter
+@update: 2023.7.25
+"""
+from python_motion_planning.curve_generation import *
+
+class CurveFactory(object):
+    def __init__(self) -> None:
+        pass
+
+    def __call__(self, curve_name, **config):
+        if curve_name == "dubins":
+            return Dubins(**config)
+        elif curve_name == "bezier":
+            return Bezier(**config)
+        elif curve_name == "polynomial":
+            return Polynomial(**config)
+        elif curve_name == "reeds_shepp":
+            return ReedsShepp(**config)
+        elif curve_name == "cubic_spline":
+            return CubicSpline(**config)
+        elif curve_name == "bspline":
+            return BSpline(**config)
+        elif curve_name == "fem_pos_smoother":
+            return FemPosSmoother(**config)
+        else:
+            raise ValueError("The `curve_name` must be set correctly.")

utils/planner/planner.py

@@ -0,0 +1,40 @@
+"""
+@file: planner.py
+@breif: Abstract class for planner
+@author: Winter
+@update: 2023.1.17
+"""
+import math
+from abc import abstractmethod, ABC
+from ..environment.env import Env, Node
+from ..plot.plot import Plot
+
+class Planner(ABC):
+    def __init__(self, start: tuple, goal: tuple, env: Env) -> None:
+        # plannig start and goal
+        self.start = Node(start, start, 0, 0)
+        self.goal = Node(goal, goal, 0, 0)
+        # environment
+        self.env = env
+        # graph handler
+        self.plot = Plot(start, goal, env)
+
+    def dist(self, node1: Node, node2: Node) -> float:
+        return math.hypot(node2.x - node1.x, node2.y - node1.y)
+    
+    def angle(self, node1: Node, node2: Node) -> float:
+        return math.atan2(node2.y - node1.y, node2.x - node1.x)
+
+    @abstractmethod
+    def plan(self):
+        '''
+        Interface for planning.
+        '''
+        pass
+
+    @abstractmethod
+    def run(self):
+        '''
+        Interface for running both plannig and animation.
+        '''
+        pass

utils/planner/search_factory.py

@@ -0,0 +1,51 @@
+"""
+@file: search_factory.py
+@breif: Factory class for global planner.
+@author: Winter
+@update: 2023.3.2
+"""
+from python_motion_planning.global_planner import *
+
+class SearchFactory(object):
+    def __init__(self) -> None:
+        pass
+
+    def __call__(self, planner_name, **config):
+        if planner_name == "a_star":
+            return AStar(**config)
+        elif planner_name == "dijkstra":
+            return Dijkstra(**config)
+        elif planner_name == "gbfs":
+            return GBFS(**config)
+        elif planner_name == "jps":
+            return JPS(**config)
+        elif planner_name == "d_star":
+            return DStar(**config)
+        elif planner_name == "lpa_star":
+            return LPAStar(**config)
+        elif planner_name == "d_star_lite":
+            return DStarLite(**config)
+        elif planner_name == "voronoi":
+            return VoronoiPlanner(**config)
+        elif planner_name == "theta_star":
+            return ThetaStar(**config)
+        elif planner_name == "lazy_theta_star":
+            return LazyThetaStar(**config)
+        elif planner_name == "s_theta_star":
+            return SThetaStar(**config)
+        elif planner_name == "anya":
+            return Anya(**config)
+        elif planner_name == "rrt":
+            return RRT(**config)
+        elif planner_name == "rrt_connect":
+            return RRTConnect(**config)
+        elif planner_name == "rrt_star":
+            return RRTStar(**config)
+        elif planner_name == "informed_rrt":
+            return InformedRRT(**config)
+        elif planner_name == "aco":
+            return ACO(**config)
+        elif planner_name == "pso":
+            return PSO(**config)
+        else:
+            raise ValueError("The `planner_name` must be set correctly.")

utils/plot/plot.py

@@ -0,0 +1,274 @@
+"""
+Plot tools 2D
+@author: huiming zhou
+"""
+import numpy as np
+import matplotlib
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+from ..environment.env import Env, Grid, Map, Node
+
+
+class Plot:
+    def __init__(self, start, goal, env: Env):
+        self.start = Node(start, start, 0, 0)
+        self.goal = Node(goal, goal, 0, 0)
+        self.env = env
+        self.fig = plt.figure("planning")
+        self.ax = self.fig.add_subplot()
+
+    def animation(self, path: list, name: str, cost: float = None, expand: list = None, history_pose: list = None,
+                  predict_path: list = None, lookahead_pts: list = None, cost_curve: list = None,
+                  ellipse: np.ndarray = None) -> None:
+        name = name + "\ncost: " + str(cost) if cost else name
+        self.plotEnv(name)
+        if expand is not None:
+            self.plotExpand(expand)
+        if history_pose is not None:
+            self.plotHistoryPose(history_pose, predict_path, lookahead_pts)
+        if path is not None:
+            self.plotPath(path)
+
+        if cost_curve:
+            plt.figure("cost curve")
+            self.plotCostCurve(cost_curve, name)
+
+        if ellipse is not None:
+            self.plotEllipse(ellipse)
+
+        plt.show()
+
+    def plotEnv(self, name: str) -> None:
+        '''
+        Plot environment with static obstacles.
+
+        Parameters
+        ----------
+        name: Algorithm name or some other information
+        '''
+        plt.plot(self.start.x, self.start.y, marker="s", color="#ff0000")
+        plt.plot(self.goal.x, self.goal.y, marker="s", color="#1155cc")
+
+        if isinstance(self.env, Grid):
+            obs_x = [x[0] for x in self.env.obstacles]
+            obs_y = [x[1] for x in self.env.obstacles]
+            plt.plot(obs_x, obs_y, "sk")
+
+        if isinstance(self.env, Map):
+            ax = self.fig.add_subplot()
+            # boundary
+            for (ox, oy, w, h) in self.env.boundary:
+                ax.add_patch(patches.Rectangle(
+                        (ox, oy), w, h,
+                        edgecolor='black',
+                        facecolor='black',
+                        fill=True
+                    )
+                )
+            # rectangle obstacles
+            for (ox, oy, w, h) in self.env.obs_rect:
+                ax.add_patch(patches.Rectangle(
+                        (ox, oy), w, h,
+                        edgecolor='black',
+                        facecolor='gray',
+                        fill=True
+                    )
+                )
+            # circle obstacles
+            for (ox, oy, r) in self.env.obs_circ:
+                ax.add_patch(patches.Circle(
+                        (ox, oy), r,
+                        edgecolor='black',
+                        facecolor='gray',
+                        fill=True
+                    )
+                )
+
+        plt.title(name)
+        plt.axis("equal")
+
+    def plotExpand(self, expand: list) -> None:
+        '''
+        Plot expanded grids using in graph searching.
+
+        Parameters
+        ----------
+        expand: Expanded grids during searching
+        '''
+        if self.start in expand:
+            expand.remove(self.start)
+        if self.goal in expand:
+            expand.remove(self.goal)
+
+        count = 0
+        if isinstance(self.env, Grid):
+            for x in expand:
+                count += 1
+                plt.plot(x.x, x.y, color="#dddddd", marker='s')
+                plt.gcf().canvas.mpl_connect('key_release_event',
+                                            lambda event: [exit(0) if event.key == 'escape' else None])
+                if count < len(expand) / 3:         length = 20
+                elif count < len(expand) * 2 / 3:   length = 30
+                else:                               length = 40
+                if count % length == 0:             plt.pause(0.001)
+        
+        if isinstance(self.env, Map):
+            for x in expand:
+                count += 1
+                if x.parent:
+                    plt.plot([x.parent[0], x.x], [x.parent[1], x.y], 
+                        color="#dddddd", linestyle="-")
+                    plt.gcf().canvas.mpl_connect('key_release_event',
+                                                 lambda event:
+                                                 [exit(0) if event.key == 'escape' else None])
+                    if count % 10 == 0:
+                        plt.pause(0.001)
+
+        plt.pause(0.01)
+
+    def plotPath(self, path: list, path_color: str='#13ae00', path_style: str="-") -> None:
+        '''
+        Plot path in global planning.
+
+        Parameters
+        ----------
+        path: Path found in global planning
+        '''
+        path_x = [path[i][0] for i in range(len(path))]
+        path_y = [path[i][1] for i in range(len(path))]
+        plt.plot(path_x, path_y, path_style, linewidth='2', color=path_color)
+        plt.plot(self.start.x, self.start.y, marker="s", color="#ff0000")
+        plt.plot(self.goal.x, self.goal.y, marker="s", color="#1155cc")
+
+    def plotAgent(self, pose: tuple, radius: float=1) -> None:
+        '''
+        Plot agent with specifical pose.
+
+        Parameters
+        ----------
+        pose: Pose of agent
+        radius: Radius of agent
+        '''
+        x, y, theta = pose
+        ref_vec = np.array([[radius / 2], [0]])
+        rot_mat = np.array([[np.cos(theta), -np.sin(theta)],
+                            [np.sin(theta),  np.cos(theta)]])
+        end_pt = rot_mat @ ref_vec + np.array([[x], [y]])
+
+        try:
+            self.ax.artists.pop()
+            for art in self.ax.get_children():
+                if isinstance(art, matplotlib.patches.FancyArrow):
+                    art.remove()
+        except:
+            pass
+
+        self.ax.arrow(x, y, float(end_pt[0]) - x, float(end_pt[1]) - y,
+                width=0.1, head_width=0.40, color="r")
+        circle = plt.Circle((x, y), radius, color="r", fill=False)
+        self.ax.add_artist(circle)
+
+    def plotHistoryPose(self, history_pose, predict_path=None, lookahead_pts=None) -> None:
+        lookahead_handler = None
+        for i, pose in enumerate(history_pose):
+            if i < len(history_pose) - 1:
+                plt.plot([history_pose[i][0], history_pose[i + 1][0]],
+                    [history_pose[i][1], history_pose[i + 1][1]], c="#13ae00")
+                if predict_path is not None:
+                    plt.plot(predict_path[i][:, 0], predict_path[i][:, 1], c="#ddd")
+            i += 1
+
+            # agent
+            self.plotAgent(pose)
+
+            # lookahead
+            if lookahead_handler is not None:
+                lookahead_handler.remove()
+            if lookahead_pts is not None:
+                try:
+                    lookahead_handler = self.ax.scatter(lookahead_pts[i][0], lookahead_pts[i][1], c="b")
+                except:
+                    lookahead_handler = self.ax.scatter(lookahead_pts[-1][0], lookahead_pts[-1][1], c="b")
+
+            plt.gcf().canvas.mpl_connect('key_release_event',
+                                        lambda event: [exit(0) if event.key == 'escape' else None])
+            if i % 5 == 0:             plt.pause(0.03)
+
+    def plotCostCurve(self, cost_list: list, name: str) -> None:
+        '''
+        Plot cost curve with epochs using in evolutionary searching.
+
+        Parameters
+        ----------
+        cost_list: Cost with epochs
+        name: Algorithm name or some other information
+        '''
+        plt.plot(cost_list, color="b")
+        plt.xlabel("epochs")
+        plt.ylabel("cost value")
+        plt.title(name)
+        plt.grid()
+
+    def plotEllipse(self, ellipse: np.ndarray, color: str = 'darkorange', linestyle: str = '--', linewidth: float = 2):
+        plt.plot(ellipse[0, :], ellipse[1, :], linestyle=linestyle, color=color, linewidth=linewidth)
+
+    def connect(self, name: str, func) -> None:
+        self.fig.canvas.mpl_connect(name, func)
+
+    def clean(self):
+        plt.cla()
+
+    def update(self):
+        self.fig.canvas.draw_idle()
+
+    @staticmethod
+    def plotArrow(x, y, theta, length, color):
+        angle = np.deg2rad(30)
+        d = 0.5 * length
+        w = 2
+
+        x_start, y_start = x, y
+        x_end = x + length * np.cos(theta)
+        y_end = y + length * np.sin(theta)
+
+        theta_hat_L = theta + np.pi - angle
+        theta_hat_R = theta + np.pi + angle
+
+        x_hat_start = x_end
+        x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)
+        x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)
+
+        y_hat_start = y_end
+        y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)
+        y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)
+
+        plt.plot([x_start, x_end], [y_start, y_end], color=color, linewidth=w)
+        plt.plot([x_hat_start, x_hat_end_L], [y_hat_start, y_hat_end_L], color=color, linewidth=w)
+        plt.plot([x_hat_start, x_hat_end_R], [y_hat_start, y_hat_end_R], color=color, linewidth=w)
+
+    @staticmethod
+    def plotCar(x, y, theta, width, length, color):
+        theta_B = np.pi + theta
+
+        xB = x + length / 4 * np.cos(theta_B)
+        yB = y + length / 4 * np.sin(theta_B)
+
+        theta_BL = theta_B + np.pi / 2
+        theta_BR = theta_B - np.pi / 2
+
+        x_BL = xB + width / 2 * np.cos(theta_BL)        # Bottom-Left vertex
+        y_BL = yB + width / 2 * np.sin(theta_BL)
+        x_BR = xB + width / 2 * np.cos(theta_BR)        # Bottom-Right vertex
+        y_BR = yB + width / 2 * np.sin(theta_BR)
+
+        x_FL = x_BL + length * np.cos(theta)               # Front-Left vertex
+        y_FL = y_BL + length * np.sin(theta)
+        x_FR = x_BR + length * np.cos(theta)               # Front-Right vertex
+        y_FR = y_BR + length * np.sin(theta)
+
+        plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],
+                 [y_BL, y_BR, y_FR, y_FL, y_BL],
+                 linewidth=1, color=color)
+
+        Plot.plotArrow(x, y, theta, length / 2, color)