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

python_motion_planning/global_planner/graph_search/gbfs.py

@@ -0,0 +1,78 @@
+"""
+@file: gbfs.py
+@breif: Greedy Best First Search 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
+
+
+class GBFS(AStar):
+    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 "Greedy Best First Search(GBFS)"
+
+    def plan(self) -> tuple:
+        """
+        Class for Greedy Best First Search.
+
+        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.GBFS((5, 5), (45, 25), pmp.Grid(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
+        """
+        # 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):
+             
+                # hit the obstacle
+                if node_n.current in self.obstacles:
+                    continue
+                
+                # exists in CLOSED list
+                if node_n.current in CLOSED:
+                    continue
+                
+                node_n.parent = node.current
+                node_n.h = self.h(node_n, self.goal)
+                node_n.g = 0
+
+                # 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 [], [], []

python_motion_planning/global_planner/graph_search/graph_search.py

@@ -0,0 +1,87 @@
+"""
+@file: graph_search.py
+@breif: Base class for planner based on graph searching
+@author: Winter
+@update: 2023.1.13
+"""
+import math
+from python_motion_planning.utils import Env, Node, Planner
+
+
+class GraphSearcher(Planner):
+    """
+    Base class for planner based on graph searching.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        heuristic_type (str): heuristic function type
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str="euclidean") -> None:
+        super().__init__(start, goal, env)
+        # heuristic type
+        self.heuristic_type = heuristic_type
+        # allowed motions
+        self.motions = self.env.motions
+        # obstacles
+        self.obstacles = self.env.obstacles
+
+    def h(self, node: Node, goal: Node) -> float:
+        """
+        Calculate heuristic.
+
+        Parameters:
+            node (Node): current node
+            goal (Node): goal node
+
+        Returns:
+            h (float): heuristic function value of node
+        """
+        if self.heuristic_type == "manhattan":
+            return abs(goal.x - node.x) + abs(goal.y - node.y)
+        elif self.heuristic_type == "euclidean":
+            return math.hypot(goal.x - node.x, goal.y - node.y)
+
+    def cost(self, node1: Node, node2: Node) -> float:
+        """
+        Calculate cost for this motion.
+
+        Parameters:
+            node1 (Node): node 1
+            node2 (Node): node 2
+
+        Returns:
+            cost (float): cost of this motion
+        """
+        if self.isCollision(node1, node2):
+            return float("inf")
+        return self.dist(node1, node2)
+
+    def isCollision(self, node1: Node, node2: Node) -> bool:
+        """
+        Judge collision when moving from node1 to node2.
+
+        Parameters:
+            node1 (Node): node 1
+            node2 (Node): node 2
+
+        Returns:
+            collision (bool): True if collision exists else False
+        """
+        if node1.current in self.obstacles or node2.current in self.obstacles:
+            return True
+
+        x1, y1 = node1.x, node1.y
+        x2, y2 = node2.x, node2.y
+
+        if x1 != x2 and y1 != y2:
+            if x2 - x1 == y1 - y2:
+                s1 = (min(x1, x2), min(y1, y2))
+                s2 = (max(x1, x2), max(y1, y2))
+            else:
+                s1 = (min(x1, x2), max(y1, y2))
+                s2 = (max(x1, x2), min(y1, y2))
+            if s1 in self.obstacles or s2 in self.obstacles:
+                return True
+        return False

python_motion_planning/global_planner/graph_search/jps.py

@@ -0,0 +1,165 @@
+"""
+@file: jps.py
+@breif: Jump Point Search 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 JPS(AStar):
+    """
+    Class for JPS 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.JPS((5, 5), (45, 25), pmp.Grid(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] Online Graph Pruning for Pathfinding On Grid Maps
+    """
+    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 "Jump Point Search(JPS)"
+
+    def plan(self) -> tuple:
+        """
+        JPS 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())
+
+            jp_list = []
+            for motion in self.motions:
+                jp = self.jump(node, motion)
+                # exists and not in CLOSED list
+                if jp and jp.current not in CLOSED:
+                    jp.parent = node.current
+                    jp.h = self.h(jp, self.goal)
+                    jp_list.append(jp)
+
+            for jp in jp_list:
+                # update OPEN list
+                heapq.heappush(OPEN, jp)
+
+                # goal found
+                if jp == self.goal:
+                    break
+
+            CLOSED[node.current] = node
+        return [], [], []
+
+    def jump(self, node: Node, motion: Node):
+        """
+        Jumping search recursively.
+
+        Parameters:
+            node (Node): current node
+            motion (Node): the motion that current node executes
+
+        Returns:
+            jump_point (Node): jump point or None if searching fails
+        """
+        # explore a new node
+        new_node = node + motion
+        new_node.parent = node.current
+        new_node.h = self.h(new_node, self.goal)
+
+        # hit the obstacle
+        if new_node.current in self.obstacles:
+            return None
+
+        # goal found
+        if new_node == self.goal:
+            return new_node
+
+        # diagonal
+        if motion.x and motion.y:
+            # if exists jump point at horizontal or vertical
+            x_dir = Node((motion.x, 0), None, 1, None)
+            y_dir = Node((0, motion.y), None, 1, None)
+            if self.jump(new_node, x_dir) or self.jump(new_node, y_dir):
+                return new_node
+            
+        # if exists forced neighbor
+        if self.detectForceNeighbor(new_node, motion):
+            return new_node
+        else:
+            return self.jump(new_node, motion)
+
+    def detectForceNeighbor(self, node, motion):
+        """
+        Detect forced neighbor of node.
+
+        Parameters:
+            node (Node): current node
+            motion (Node): the motion that current node executes
+
+        Returns:
+            flag (bool): True if current node has forced neighbor else Flase
+        """
+        x, y = node.current
+        x_dir, y_dir = motion.current
+
+        # horizontal
+        if x_dir and not y_dir:
+            if (x, y + 1) in self.obstacles and \
+                (x + x_dir, y + 1) not in self.obstacles:
+                return True
+            if (x, y - 1) in self.obstacles and \
+                (x + x_dir, y - 1) not in self.obstacles:
+                return True
+        
+        # vertical
+        if not x_dir and y_dir:
+            if (x + 1, y) in self.obstacles and \
+                (x + 1, y + y_dir) not in self.obstacles:
+                return True
+            if (x - 1, y) in self.obstacles and \
+                (x - 1, y + y_dir) not in self.obstacles:
+                return True
+        
+        # diagonal
+        if x_dir and y_dir:
+            if (x - x_dir, y) in self.obstacles and \
+                (x - x_dir, y + y_dir) not in self.obstacles:
+                return True
+            if (x, y - y_dir) in self.obstacles and \
+                (x + x_dir, y - y_dir) not in self.obstacles:
+                return True
+        
+        return False
+

python_motion_planning/global_planner/graph_search/lazy_theta_star.py

@@ -0,0 +1,114 @@
+"""
+@file: lazy_theta_star.py
+@breif: Lazy Theta* motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import heapq
+
+from .theta_star import ThetaStar
+from python_motion_planning.utils import Env, Node
+
+class LazyThetaStar(ThetaStar):
+    """
+    Class for Lazy 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.LazyThetaStar((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] Lazy Theta*: Any-Angle Path Planning and Path Length Analysis in 3D
+    """
+    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 "Lazy Theta*"
+
+    def plan(self) -> tuple:
+        """
+        Lazy 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)
+
+            # set vertex: path 1
+            node_p = CLOSED.get(node.parent)
+            if node_p:
+                if not self.lineOfSight(node_p, node):
+                    node.g = float("inf")
+                    for node_n in self.getNeighbor(node):
+                        if node_n.current in CLOSED:
+                            node_n = CLOSED[node_n.current]
+                            if node.g > node_n.g + self.dist(node_n, node):
+                                node.g = node_n.g + self.dist(node_n, node)
+                                node.parent = node_n.current
+
+            # 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:
+                    # path2
+                    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
+        """
+        # 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  

python_motion_planning/global_planner/graph_search/lpa_star.py

@@ -0,0 +1,230 @@
+"""
+@file: lpa_star.py
+@breif: Lifelong Planning A* motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import heapq
+
+from .graph_search import GraphSearcher
+from python_motion_planning.utils import Env, Node
+
+class LNode(Node):
+    """
+    Class for LPA* nodes.
+
+    Parameters:
+        current (tuple): current coordinate
+        g (float): minimum cost moving from start(predict)
+        rhs (float): minimum cost moving from start(value)
+        key (list): priority
+    """
+    def __init__(self, current: tuple, g: float, rhs: float, key: list) -> None:
+        self.current = current
+        self.g = g
+        self.rhs = rhs
+        self.key = key
+
+    def __add__(self, node):
+        return LNode((self.x + node.x, self.y + node.y), 
+                      self.g, self.rhs, self.key)
+
+    def __lt__(self, node) -> bool:
+        return self.key < node.key
+
+    def __str__(self) -> str:
+        return "----------\ncurrent:{}\ng:{}\nrhs:{}\nkey:{}\n----------" \
+            .format(self.current, self.g, self.rhs, self.key)
+
+class LPAStar(GraphSearcher):
+    """
+    Class for LPA* 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.LPAStar((5, 5), (45, 25), pmp.Grid(51, 31))
+        >>> cost, path, _ = planner.plan()     # planning results only
+        >>> planner.plot.animation(path, str(planner), cost)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] Lifelong Planning A*
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean") -> None:
+        super().__init__(start, goal, env, heuristic_type)
+        # start and goal
+        self.start = LNode(start, float('inf'), 0.0, None)
+        self.goal = LNode(goal, float('inf'), float('inf'), None)
+        # OPEN set and expand zone
+        self.U, self.EXPAND = [], []
+
+        # intialize global information, record history infomation of map grids
+        self.map = {s: LNode(s, float('inf'), float('inf'), None) for s in self.env.grid_map}
+        self.map[self.goal.current] = self.goal
+        self.map[self.start.current] = self.start
+        # OPEN set with priority
+        self.start.key = self.calculateKey(self.start)
+        heapq.heappush(self.U, self.start)
+
+    def __str__(self) -> str:
+        return "Lifelong Planning A*"
+
+    def plan(self) -> tuple:
+        """
+        LPA* dynamic motion planning function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            _ (None): None
+        """
+        self.computeShortestPath()
+        cost, path = self.extractPath()
+        return cost, path, None
+
+    def run(self) -> None:
+        """
+        Running both plannig and animation.
+        """
+        # static planning
+        cost, path, _ = self.plan()
+        
+        # animation
+        self.plot.connect('button_press_event', self.OnPress)
+        self.plot.animation(path, str(self), cost=cost)
+
+    def OnPress(self, event):
+        """
+        Mouse button callback function.
+
+        Parameters:
+            event (MouseEvent): mouse event
+        """
+        x, y = int(event.xdata), int(event.ydata)
+        if x < 0 or x > self.env.x_range - 1 or y < 0 or y > self.env.y_range - 1:
+            print("Please choose right area!")
+        else:
+            print("Change position: x = {}, y = {}".format(x, y))
+            self.EXPAND = []
+            node_change = self.map[(x, y)]
+
+            if (x, y) not in self.obstacles:
+                self.obstacles.add((x, y))
+            else:
+                self.obstacles.remove((x, y))
+                self.updateVertex(node_change)
+            
+            self.env.update(self.obstacles)
+
+            for node_n in self.getNeighbor(node_change):
+                self.updateVertex(node_n)
+
+            cost, path, _ = self.plan()
+        
+            # animation
+            self.plot.clean()
+            self.plot.animation(path, str(self), cost, self.EXPAND)
+            self.plot.update()
+
+    def computeShortestPath(self) -> None:
+        """
+        Perceived dynamic obstacle information to optimize global path.
+        """
+        while True:
+            node = min(self.U, key=lambda node: node.key)
+            if node.key >= self.calculateKey(self.goal) and \
+                    self.goal.rhs == self.goal.g:
+                break
+
+            self.U.remove(node)
+            self.EXPAND.append(node)
+
+            # Locally over-consistent -> Locally consistent
+            if node.g > node.rhs:
+                node.g = node.rhs
+            # Locally under-consistent -> Locally over-consistent
+            else:
+                node.g = float("inf")
+                self.updateVertex(node)
+
+            for node_n in self.getNeighbor(node):
+                self.updateVertex(node_n)
+
+    def updateVertex(self, node: LNode) -> None:
+        """
+        Update the status and the current cost to node and it's neighbor.
+
+        Parameters:
+            node (LNode): current node
+        """
+        # greed correction
+        if node != self.start:
+            node.rhs = min([node_n.g + self.cost(node_n, node)
+                        for node_n in self.getNeighbor(node)])
+
+        if node in self.U:
+            self.U.remove(node)
+
+        # Locally unconsistent nodes should be added into OPEN set (set U)
+        if node.g != node.rhs:
+            node.key = self.calculateKey(node)
+            heapq.heappush(self.U, node)
+
+    def calculateKey(self, node: LNode) -> list:
+        """
+        Calculate priority of node.
+
+        Parameters:
+            node (LNode): current node
+
+        Returns:
+            key (list): priority of node
+        """
+        return [min(node.g, node.rhs) + self.h(node, self.goal),
+                min(node.g, node.rhs)]
+
+    def getNeighbor(self, node: LNode) -> list:
+        """
+        Find neighbors of node.
+
+        Parameters:
+            node (LNode): current node
+
+        Returns:
+            neighbors (list): neighbors of node
+        """
+        neighbors = []
+        for motion in self.motions:
+            n = self.map[(node + motion).current]
+            if n.current not in self.obstacles:
+                neighbors.append(n)
+        return neighbors
+
+    def extractPath(self):
+        """
+        Extract the path based on greedy policy.
+
+        Return:
+            cost (float): the cost of planning path
+            path (list): the planning path
+        """
+        node = self.goal
+        path = [node.current]
+        cost, count = 0, 0
+        while node != self.start:
+            neighbors = [node_n for node_n in self.getNeighbor(node) if not self.isCollision(node, node_n)]
+            next_node = min(neighbors, key=lambda n: n.g)
+            path.append(next_node.current)
+            cost += self.cost(node, next_node)
+            node = next_node
+            count += 1
+            if count == 1000:
+                return cost, []
+        return cost, list(reversed(path))
+