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

global_planner/graph_search/s_theta_star.py

@@ -0,0 +1,133 @@
+'''
+@file: s_theta_star.py
+@breif: S-Theta* motion planning
+@author: Wu Maojia
+@update: 2024.6.23
+'''
+import heapq
+from math import acos
+
+from .theta_star import ThetaStar
+from python_motion_planning.utils import Env, Node
+
+
+class SThetaStar(ThetaStar):
+    """
+    Class for S-Theta* motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        heuristic_type (str): heuristic function type
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.SThetaStar((5, 5), (45, 25), pmp.Grid(51, 31))
+        >>> cost, path, expand = planner.plan()
+        >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] S-Theta*: low steering path-planning algorithm
+    """
+
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean") -> None:
+        super().__init__(start, goal, env, heuristic_type)
+
+    def __str__(self) -> str:
+        return "S-Theta*"
+
+    def plan(self) -> tuple:
+        """
+        S-Theta* motion plan function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            expand (list): all nodes that planner has searched
+        """
+        # OPEN list (priority queue) and CLOSED list (hash table)
+        OPEN = []
+        heapq.heappush(OPEN, self.start)
+        CLOSED = dict()
+
+        while OPEN:
+            node = heapq.heappop(OPEN)
+
+            # exists in CLOSED list
+            if node.current in CLOSED:
+                continue
+
+            # goal found
+            if node == self.goal:
+                CLOSED[node.current] = node
+                cost, path = self.extractPath(CLOSED)
+                return cost, path, list(CLOSED.values())
+
+            for node_n in self.getNeighbor(node):
+                # exists in CLOSED list
+                if node_n.current in CLOSED:
+                    continue
+
+                # path1
+                node_n.parent = node.current
+                node_n.h = self.h(node_n, self.goal)
+
+                alpha = 0.0
+                node_p = CLOSED.get(node.parent)
+                if node_p:
+                    alpha = self.getAlpha(node_p, node_n)
+                    node_n.g += alpha
+
+                if node_p:
+                    self.updateVertex(node_p, node_n, alpha)
+
+                # goal found
+                if node_n == self.goal:
+                    heapq.heappush(OPEN, node_n)
+                    break
+
+                # update OPEN list
+                heapq.heappush(OPEN, node_n)
+
+            CLOSED[node.current] = node
+        return [], [], []
+
+    def updateVertex(self, node_p: Node, node_c: Node, alpha: float) -> None:
+        """
+        Update extend node information with current node's parent node.
+
+        Parameters:
+            node_p (Node): parent node
+            node_c (Node): current node
+            alpha (float): alpha angle
+        """
+        # if alpha == 0 or self.lineOfSight(node_c, node_p):    # "alpha == 0" will cause the path to penetrate obstacles
+        if self.lineOfSight(node_c, node_p):
+            # path 2
+            new_g = node_p.g + self.dist(node_c, node_p) + alpha
+            if new_g <= node_c.g:
+                node_c.g = new_g
+                node_c.parent = node_p.current
+
+    def getAlpha(self, node_p: Node, node_c: Node):
+        """
+        α(t) represents the deviation in the trajectory to reach the goal node g
+        through the node t in relation to the straight-line distance between the parent of its
+        predecessor (t ∈ succ(p) and parent(p) = q) and the goal node.
+
+        Parameters:
+            node_p (Node): parent node
+            node_c (Node): current node
+
+        Returns:
+            alpha (float): alpha angle
+        """
+        d_qt = self.dist(node_p, node_c)
+        d_qg = self.dist(node_p, self.goal)
+        d_tg = self.dist(node_c, self.goal)
+        value = (d_qt * d_qt + d_qg * d_qg - d_tg * d_tg) / (2.0 * d_qt * d_qg)
+        value = max(-1.0, min(1.0, value))
+        cost = acos(value)
+        return cost

global_planner/graph_search/theta_star.py

@@ -0,0 +1,171 @@
+"""
+@file: theta_star.py
+@breif: Theta* motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import heapq
+
+from .a_star import AStar
+from python_motion_planning.utils import Env, Node
+
+
+class ThetaStar(AStar):
+    """
+    Class for Theta* motion planning.
+
+    Parameters:
+        start (tuple):
+            start point coordinate
+        goal (tuple):
+            goal point coordinate
+        env (Env):
+            environment
+        heuristic_type (str):
+            heuristic function type
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.ThetaStar((5, 5), (45, 25), pmp.Grid(51, 31))
+        >>> cost, path, expand = planner.plan()
+        >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] Theta*: Any-Angle Path Planning on Grids
+        [2] Any-angle path planning on non-uniform costmaps
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean") -> None:
+        super().__init__(start, goal, env, heuristic_type)
+
+    def __str__(self) -> str:
+        return "Theta*"
+
+    def plan(self) -> tuple:
+        """
+        Theta* motion plan function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            expand (list): all nodes that planner has searched
+        """
+        # OPEN list (priority queue) and CLOSED list (hash table)
+        OPEN = []
+        heapq.heappush(OPEN, self.start)
+        CLOSED = dict()
+
+        while OPEN:
+            node = heapq.heappop(OPEN)
+
+            # exists in CLOSED list
+            if node.current in CLOSED:
+                continue
+
+            # goal found
+            if node == self.goal:
+                CLOSED[node.current] = node
+                cost, path = self.extractPath(CLOSED)
+                return cost, path, list(CLOSED.values())
+
+            for node_n in self.getNeighbor(node):                
+                # exists in CLOSED list
+                if node_n.current in CLOSED:
+                    continue
+                
+                # path1
+                node_n.parent = node.current
+                node_n.h = self.h(node_n, self.goal)
+
+                node_p = CLOSED.get(node.parent)
+
+                if node_p:
+                    self.updateVertex(node_p, node_n)
+
+                # goal found
+                if node_n == self.goal:
+                    heapq.heappush(OPEN, node_n)
+                    break
+                
+                # update OPEN list
+                heapq.heappush(OPEN, node_n)
+
+            CLOSED[node.current] = node
+        return [], [], []
+
+    def updateVertex(self, node_p: Node, node_c: Node) -> None:
+        """
+        Update extend node information with current node's parent node.
+
+        Parameters:
+            node_p (Node): parent node
+            node_c (Node): current node
+        """
+        if self.lineOfSight(node_c, node_p):
+            # path 2
+            if node_p.g + self.dist(node_c, node_p) <= node_c.g:
+                node_c.g = node_p.g + self.dist(node_c, node_p)
+                node_c.parent = node_p.current
+
+    def lineOfSight(self, node1: Node, node2: Node) -> bool:
+        """
+        Judge collision when moving from node1 to node2 using Bresenham.
+
+        Parameters:
+            node1 (Node): start node
+            node2 (Node): end node
+
+        Returns:
+            line_of_sight (bool): True if line of sight exists ( no collision ) else False
+        """
+        if node1.current in self.obstacles or node2.current in self.obstacles:
+            return False
+        
+        x1, y1 = node1.current
+        x2, y2 = node2.current
+
+        if x1 < 0 or x1 >= self.env.x_range or y1 < 0 or y1 >= self.env.y_range:
+            return False
+        if x2 < 0 or x2 >= self.env.x_range or y2 < 0 or y2 >= self.env.y_range:
+            return False
+
+        d_x = abs(x2 - x1)
+        d_y = abs(y2 - y1)
+        s_x = 0 if (x2 - x1) == 0 else (x2 - x1) / d_x
+        s_y = 0 if (y2 - y1) == 0 else (y2 - y1) / d_y
+        x, y, e = x1, y1, 0
+
+        # check if any obstacle exists between node1 and node2
+        if d_x > d_y:
+            tau = (d_y - d_x) / 2
+            while not x == x2:
+                if e > tau:
+                    x = x + s_x
+                    e = e - d_y
+                elif e < tau:
+                    y = y + s_y
+                    e = e + d_x
+                else:
+                    x = x + s_x
+                    y = y + s_y
+                    e = e + d_x - d_y
+                if (x, y) in self.obstacles:
+                    return False
+        # swap x and y
+        else:
+            tau = (d_x - d_y) / 2
+            while not y == y2:
+                if e > tau:
+                    y = y + s_y
+                    e = e - d_x
+                elif e < tau:
+                    x = x + s_x
+                    e = e + d_y
+                else:
+                    x = x + s_x
+                    y = y + s_y
+                    e = e + d_y - d_x
+                if (x, y) in self.obstacles:
+                    return False
+        
+        return True

global_planner/graph_search/voronoi.py

@@ -0,0 +1,200 @@
+"""
+@file: voronoi.py
+@breif: Voronoi-based motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import heapq, math
+import numpy as np
+from scipy.spatial import cKDTree, Voronoi
+
+from .graph_search import GraphSearcher
+from python_motion_planning.utils import Env, Node
+
+class VoronoiPlanner(GraphSearcher):
+    """
+    Class for Voronoi-based motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        heuristic_type (str): heuristic function type, default is euclidean
+        n_knn (int): number of edges from one sampled point
+        max_edge_len (float): maximum edge length
+        inflation_r (float): inflation range
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.VoronoiPlanner((5, 5), (45, 25), pmp.Grid(51, 31))
+        >>> cost, path, _ = planner.plan()     # planning results only
+        >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
+        >>> planner.run()       # run both planning and animation
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean", \
+                 n_knn: int = 10, max_edge_len: float = 10.0, inflation_r: float = 1.0) -> None:
+        super().__init__(start, goal, env, heuristic_type)
+        # number of edges from one sampled point
+        self.n_knn = n_knn
+        # maximum edge length
+        self.max_edge_len = max_edge_len
+        # inflation range
+        self.inflation_r = inflation_r
+
+    def __str__(self) -> str:
+        return "Voronoi-based Planner"
+
+    def plan(self):
+        """
+        Voronoi-based motion plan function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            expand (list): voronoi sampled nodes
+        """
+        # sampling voronoi diagram
+        vor = Voronoi(np.array(list(self.env.obstacles)))
+        vx_list = [ix for [ix, _] in vor.vertices] + [self.start.x, self.goal.x]
+        vy_list = [iy for [_, iy] in vor.vertices] + [self.start.y, self.goal.y]
+        sample_num = len(vx_list)
+        expand = [Node((vx_list[i], vy_list[i])) for i in range(sample_num)]
+
+        # generate road map for voronoi nodes
+        road_map = {}
+        node_tree = cKDTree(np.vstack((vx_list, vy_list)).T)
+
+        for node in expand:
+            edges = []
+            _, index_list = node_tree.query([node.x, node.y], k=sample_num)
+
+            for i in range(1, len(index_list)):
+                node_n = expand[index_list[i]]
+
+                if not self.isCollision(node, node_n):
+                    edges.append(node_n)
+
+                if len(edges) >= self.n_knn:
+                    break
+
+            road_map[node] = edges
+
+        # calculate shortest path using graph search algorithm
+        cost, path = self.getShortestPath(road_map)
+        return cost, path, expand
+
+    def run(self):
+        """
+        Running both plannig and animation.
+        """
+        cost, path, expand = self.plan()
+        self.plot.animation(path, str(self), cost, expand)
+    
+    def getShortestPath(self, road_map: dict, dijkstra: bool = True) -> list:
+        """
+        Calculate shortest path using graph search algorithm(A*, Dijkstra, etc).
+
+        Parameters:
+            road_map (dict): road map for voronoi diagram, which store KNN for one voronoi node
+            dijkstra (bool): using Dijkstra if true, else A*
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+        """
+        # OPEN list (priority queue) and CLOSED list (hash table)
+        OPEN = []
+        heapq.heappush(OPEN, self.start)
+        CLOSED = dict()
+
+        while OPEN:
+            node = heapq.heappop(OPEN)
+
+            # exists in CLOSED list
+            if node.current in CLOSED:
+                continue
+
+            # goal found
+            if node == self.goal:
+                CLOSED[node.current] = node
+                return self.extractPath(CLOSED)
+
+            for node_n in road_map[node]:                
+                # exists in CLOSED set
+                if node_n.current in CLOSED:
+                    continue
+                
+                node_n.parent = node.current
+                node_n.g = self.dist(node_n, node)
+                if not dijkstra:
+                    node_n.h = self.h(node_n, self.goal)
+
+                # goal found
+                if node_n == self.goal:
+                    heapq.heappush(OPEN, node_n)
+                    break
+                
+                # update OPEN set
+                heapq.heappush(OPEN, node_n)
+
+            CLOSED[node.current] = node
+        return [], []
+
+    def extractPath(self, closed_list: dict):
+        """
+        Extract the path based on the CLOSED list.
+
+        Parameters:
+            closed_list (dict): CLOSED list
+
+        Returns:
+            cost (float): the cost of planning path
+            path (list): the planning path
+        """
+        cost = 0
+        node = closed_list[self.goal.current]
+        path = [node.current]
+        while node != self.start:
+            node_parent = closed_list[node.parent]
+            cost += self.dist(node, node_parent)
+            node = node_parent
+            path.append(node.current)
+        return cost, path
+
+    def isCollision(self, node1: Node, node2: Node) -> bool:
+        """
+        Judge collision when moving from node1 to node2.
+
+        Parameters:
+            node1 (Node): start node
+            node2 (Node): end node
+
+        Returns:
+            collision (bool): True if collision exists else False
+        """
+        if node1.current in self.obstacles or node2.current in self.obstacles:
+            return True
+        
+        yaw = self.angle(node1, node2)
+        dist = self.dist(node1, node2)
+
+        if dist >= self.max_edge_len:
+            return True
+
+        d_dist = self.inflation_r
+        n_step = round(dist / d_dist)
+
+        x, y = node1.current
+        for _ in range(n_step):
+            dist_to_obs, _ = self.env.obstacles_tree.query([x, y])
+            if dist_to_obs <= self.inflation_r:
+                return True
+            x += d_dist * math.cos(yaw)
+            y += d_dist * math.sin(yaw)
+
+        # goal point check
+        dist_to_obs, _ = self.env.obstacles_tree.query(node2.current)
+        if dist_to_obs <= self.inflation_r:
+            return True
+
+        return False

global_planner/sample_search/__init__.py

@@ -0,0 +1,6 @@
+from .rrt import RRT
+from .rrt_connect import RRTConnect
+from .rrt_star import RRTStar
+from .informed_rrt import InformedRRT
+
+__all__ = ['RRT', 'RRTConnect', 'RRTStar', 'InformedRRT']

global_planner/sample_search/informed_rrt.py

@@ -0,0 +1,152 @@
+"""
+@file: informed_rrt.py
+@breif: Informed RRT* motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import numpy as np
+from functools import partial
+import matplotlib.pyplot as plt
+
+from .rrt_star import RRTStar
+from python_motion_planning.utils import Env, Node
+
+
+class Ellipse:
+    """
+    Ellipse sampling.
+    """
+    @staticmethod
+    def transform(a: float, c: float, p1: tuple, p2: tuple) -> np.ndarray:
+        # center
+        center_x = (p1[0] + p2[0]) / 2
+        center_y = (p1[1] + p2[1]) / 2
+
+        # rotation
+        theta = - np.arctan2(p2[1] - p1[1], p2[0] - p1[0])
+
+        # transform
+        b = np.sqrt(a ** 2 - c ** 2)
+        T = np.array([[ a * np.cos(theta), b * np.sin(theta), center_x],
+                      [-a * np.sin(theta), b * np.cos(theta), center_y],
+                      [                 0,                 0,        1]])
+        return T
+
+
+class InformedRRT(RRTStar):
+    """
+    Class for Informed RRT* motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        max_dist (float): Maximum expansion distance one step
+        sample_num (int): Maximum number of sample points
+        r (float): optimization radius
+        goal_sample_rate (float): heuristic sample
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.InformedRRT((5, 5), (45, 25), pmp.Map(51, 31))
+        >>> cost, path, expand = planner.plan()     # planning results only
+        >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal heuristic
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, max_dist: float = 0.5,
+                 sample_num: int = 1500, r: float = 12.0, goal_sample_rate: float = 0.05) -> None:
+        super().__init__(start, goal, env, max_dist, sample_num, goal_sample_rate)
+        # optimization radius
+        self.r = r
+        # best planning cost
+        self.c_best = float("inf")
+        # distance between start and goal
+        self.c_min = self.dist(self.start, self.goal)
+        # ellipse sampling
+        self.transform = partial(Ellipse.transform, c=self.c_min / 2, p1=start, p2=goal)
+    
+    def __str__(self) -> str:
+        return "Informed RRT*"
+
+    def plan(self) -> tuple:
+        """
+        Informed-RRT* motion plan function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            expand (list): expanded (sampled) nodes list
+        """
+        # Sampled list
+        sample_list = {self.start.current: self.start}
+
+        best_cost, best_path = float("inf"), None
+
+        # main loop
+        for i in range(self.sample_num):
+            # generate a random node in the map
+            node_rand = self.generateRandomNode()
+
+            # visited
+            if node_rand.current in sample_list:
+                continue
+
+            # generate new node
+            node_new = self.getNearest(list(sample_list.values()), node_rand)
+            if node_new:
+                sample_list[node_new.current] = node_new
+                dist = self.dist(node_new, self.goal)
+                # goal found
+                if dist <= self.max_dist and not self.isCollision(node_new, self.goal):
+                    self.goal.parent = node_new.current
+                    self.goal.g = node_new.g + self.dist(self.goal, node_new)
+                    sample_list[self.goal.current] = self.goal
+                    cost, path = self.extractPath(sample_list)
+                    if path and cost < best_cost:
+                        best_cost, best_path = cost, path
+                        self.c_best = best_cost
+
+        return best_cost, best_path, list(sample_list.values())
+
+    def run(self) -> None:
+        """
+        Running both plannig and animation.
+        """
+        cost, path, expand = self.plan()
+        t = np.arange(0, 2 * np.pi + 0.1, 0.1)
+        x = [np.cos(it) for it in t]
+        y = [np.sin(it) for it in t]
+        z = [1 for _ in t]
+        fx = self.transform(self.c_best / 2) @ np.array([x, y, z])
+        fx[0, :] += x
+        fx[1, :] += y
+        self.plot.animation(path, str(self), cost, expand, ellipse=fx)
+
+    def generateRandomNode(self) -> Node:
+        """
+        Generate a random node to extend exploring tree.
+
+        Returns:
+            node (Node): a random node based on sampling
+        """
+        # ellipse sample
+        if self.c_best < float("inf"):
+            while True:
+                # unit ball sample
+                p = np.array([.0, .0, 1.])
+                while True:
+                    x, y = np.random.uniform(-1, 1), np.random.uniform(-1, 1)
+                    if x ** 2 + y ** 2 < 1:
+                        p[0], p[1] = x, y
+                        break
+                # transform to ellipse
+                p_star = self.transform(self.c_best / 2) @ p.T
+                if self.delta <= p_star[0] <= self.env.x_range - self.delta and \
+                   self.delta <= p_star[1] <= self.env.y_range - self.delta:
+                    return Node((p_star[0], p_star[1]), None, 0, 0)
+        # random sample
+        else:
+            return super().generateRandomNode()