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

global_planner/graph_search/a_star.py

@@ -0,0 +1,124 @@
+"""
+@file: a_star.py
+@breif: 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 AStar(GraphSearcher):
+    """
+    Class for A* 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.AStar((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] A Formal Basis for the heuristic Determination of Minimum Cost Paths
+    """
+    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 "A*"
+
+    def plan(self) -> tuple:
+        """
+        A* 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
+                
+                node_n.parent = node.current
+                node_n.h = self.h(node_n, self.goal)
+
+                # 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 getNeighbor(self, node: Node) -> list:
+        """
+        Find neighbors of node.
+
+        Parameters:
+            node (Node): current node
+
+        Returns:
+            neighbors (list): neighbors of current node
+        """
+        return [node + motion for motion in self.motions
+                if not self.isCollision(node, node + motion)]
+
+    def extractPath(self, closed_list: dict) -> tuple:
+        """
+        Extract the path based on the CLOSED list.
+
+        Parameters:
+            closed_list (dict): CLOSED list
+
+        Returns:
+            cost (float): the cost of planned 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 run(self):
+        """
+        Running both planning and animation.
+        """
+        cost, path, expand = self.plan()
+        self.plot.animation(path, str(self), cost, expand)

global_planner/graph_search/d_star.py

@@ -0,0 +1,291 @@
+"""
+@file: d_star.py
+@breif: Dynamic A* motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+from .graph_search import GraphSearcher
+from python_motion_planning.utils import Env, Node
+
+
+class DNode(Node):
+    """
+    Class for D* nodes.
+
+    Parameters:
+        current (tuple): current coordinate
+        parent (tuple): coordinate of parent node
+        t (str): state of node, including `NEW` `OPEN` and `CLOSED`
+        h (float): cost from goal to current node
+        k (float): minimum cost from goal to current node in history
+    """
+    def __init__(self, current: tuple, parent: tuple, t: str, h: float, k: float) -> None:
+        self.current = current
+        self.parent = parent
+        self.t = t
+        self.h = h
+        self.k = k
+
+    def __add__(self, node):
+        return DNode((self.x + node.x, self.y + node.y), 
+                     self.parent, self.t, self.h + node.h, self.k)
+
+    def __str__(self) -> str:
+        return "----------\ncurrent:{}\nparent:{}\nt:{}\nh:{}\nk:{}\n----------" \
+            .format(self.current, self.parent, self.t, self.h, self.k)
+
+class DStar(GraphSearcher):
+    """
+    Class for D* motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.DStar((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]Optimal and Efficient Path Planning for Partially-Known Environments
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env) -> None:
+        super().__init__(start, goal, env, None)
+        self.start = DNode(start, None, 'NEW', float('inf'), float("inf"))
+        self.goal = DNode(goal, None, 'NEW', 0, float('inf'))
+        # allowed motions
+        self.motions = [DNode(motion.current, None, None, motion.g, 0) for motion in self.env.motions]
+        # OPEN list and EXPAND list
+        self.OPEN = []
+        self.EXPAND = []
+        # record history infomation of map grids
+        self.map = {s: DNode(s, None, 'NEW', float("inf"), float("inf")) for s in self.env.grid_map}
+        self.map[self.goal.current] = self.goal
+        self.map[self.start.current] = self.start
+        # intialize OPEN list
+        self.insert(self.goal, 0)
+
+    def __str__(self) -> str:
+        return "Dynamic A*(D*)"
+
+    def plan(self) -> tuple:
+        """
+        D* static motion planning function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            _ (None): None
+        """
+        while True:
+            self.processState()
+            if self.start.t == 'CLOSED':
+                break
+        cost, path = self.extractPath(self.map)
+        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) -> None:
+        """
+        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:
+            if (x, y) not in self.obstacles:
+                print("Add obstacle at: ({}, {})".format(x, y))
+                # update obstacles
+                self.obstacles.add((x, y))
+                self.env.update(self.obstacles)
+
+                # move from start to goal, replan locally when meeting collisions
+                node = self.start
+                self.EXPAND, path, cost = [], [], 0
+                while node != self.goal:
+                    node_parent = self.map[node.parent]
+                    if self.isCollision(node, node_parent):
+                        self.modify(node, node_parent)
+                        continue
+                    path.append(node.current)
+                    cost += self.cost(node, node_parent)
+                    node = node_parent
+
+                self.plot.clean()
+                self.plot.animation(path, str(self), cost, self.EXPAND)
+
+            self.plot.update()
+
+    def extractPath(self, closed_list: dict) -> tuple:
+        """
+        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 = self.start
+        path = [node.current]
+        while node != self.goal:
+            node_parent = closed_list[node.parent]
+            cost += self.cost(node, node_parent)
+            node = node_parent
+            path.append(node.current)
+
+        return cost, path
+
+    def processState(self) -> float:
+        """
+        Broadcast dynamic obstacle information.
+
+        Returns:
+            min_k (float): minimum k value of map
+        """
+        # get node in OPEN list with min k value
+        node = self.min_state
+        self.EXPAND.append(node)
+
+        if node is None:
+            return -1
+
+        # record the min k value of this iteration
+        k_old = self.min_k
+        # move node from OPEN list to CLOSED list
+        self.delete(node)  
+
+        # k_min < h[x] --> x: RAISE state (try to reduce k value by neighbor)
+        if k_old < node.h:
+            for node_n in self.getNeighbor(node):
+                if node_n.h <= k_old and node.h > node_n.h + self.cost(node, node_n):
+                    # update h_value and choose parent
+                    node.parent = node_n.current
+                    node.h = node_n.h + self.cost(node, node_n)
+
+        # k_min >= h[x] -- > x: LOWER state (cost reductions)
+        if k_old == node.h:
+            for node_n in self.getNeighbor(node):
+                if node_n.t == 'NEW' or \
+                    (node_n.parent == node.current and node_n.h != node.h + self.cost(node, node_n)) or \
+                    (node_n.parent != node.current and node_n.h > node.h + self.cost(node, node_n)):
+                    # Condition:
+                    # 1) t[node_n] == 'NEW': not visited
+                    # 2) node_n's parent: cost reduction
+                    # 3) node_n find a better parent
+                    node_n.parent = node.current
+                    self.insert(node_n, node.h + self.cost(node, node_n))
+        else:
+            for node_n in self.getNeighbor(node):
+                if node_n.t == 'NEW' or \
+                    (node_n.parent == node.current and node_n.h != node.h + self.cost(node, node_n)):
+                    # Condition:
+                    # 1) t[node_n] == 'NEW': not visited
+                    # 2) node_n's parent: cost reduction
+                    node_n.parent = node.current
+                    self.insert(node_n, node.h + self.cost(node, node_n))
+                else:
+                    if node_n.parent != node.current and \
+                        node_n.h > node.h + self.cost(node, node_n):
+                        # Condition: LOWER happened in OPEN list (s), s should be explored again
+                        self.insert(node, node.h)
+                    else:
+                        if node_n.parent != node.current and \
+                            node.h > node_n.h + self.cost(node, node_n) and \
+                            node_n.t == 'CLOSED' and \
+                            node_n.h > k_old:
+                            # Condition: LOWER happened in CLOSED list (s_n), s_n should be explored again
+                            self.insert(node_n, node_n.h)
+        return self.min_k
+
+    @property
+    def min_state(self) -> DNode:
+        """
+        Choose the node with the minimum k value in OPEN list.
+        """
+        if not self.OPEN:
+            return None
+        return min(self.OPEN, key=lambda node: node.k)
+
+    @property
+    def min_k(self) -> float:
+        """
+        Choose the minimum k value for nodes in OPEN list.
+        """
+        return self.min_state.k
+
+    def insert(self, node: DNode, h_new: float) -> None:
+        """
+        Insert node into OPEN list.
+
+        Parameters:
+            node (DNode): the node to insert
+            h_new (float): new or better cost to come value
+        """
+        if node.t == 'NEW':         node.k = h_new
+        elif node.t == 'OPEN':      node.k = min(node.k, h_new)
+        elif node.t == 'CLOSED':    node.k = min(node.h, h_new)
+        node.h, node.t = h_new, 'OPEN'
+        self.OPEN.append(node)
+
+    def delete(self, node: DNode) -> None:
+        """
+        Delete node from OPEN list.
+
+        Parameters:
+            node (DNode): the node to delete
+        """
+        if node.t == 'OPEN':
+            node.t = 'CLOSED'
+        self.OPEN.remove(node)
+
+    def modify(self, node: DNode, node_parent: DNode) -> None:
+        """
+        Start processing from node.
+
+        Parameters:
+            node (DNode): the node to modify
+            node_parent (DNode): the parent node of `node`
+        """
+        if node.t == 'CLOSED':
+            self.insert(node, node_parent.h + self.cost(node, node_parent))
+        while True:
+            k_min = self.processState()
+            if k_min >= node.h:
+                break
+
+    def getNeighbor(self, node: DNode) -> list:
+        """
+        Find neighbors of node.
+
+        Parameters:
+            node (DNode): current node
+
+        Returns:
+            neighbors (list): neighbors of current node
+        """
+        neighbors = []
+        for motion in self.motions:
+            n = self.map[(node + motion).current]
+            if not self.isCollision(node, n):
+                neighbors.append(n)
+        return neighbors

global_planner/graph_search/d_star_lite.py

@@ -0,0 +1,188 @@
+"""
+@file: d_star_lite.py
+@breif: D* Lite motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import heapq
+
+from .graph_search import GraphSearcher
+from .lpa_star import LPAStar, LNode
+from python_motion_planning.utils import Env
+
+
+class DStarLite(LPAStar):
+    """
+    Class for D* Lite 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.DStarLite((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] D* Lite
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean") -> None:
+        GraphSearcher.__init__(self, start, goal, env, heuristic_type)
+        # start and goal
+        self.start = LNode(start, float('inf'), float('inf'), None)
+        self.goal = LNode(goal, float('inf'), 0.0, None)
+        # correction
+        self.km = 0
+        # 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.goal.key = self.calculateKey(self.goal)
+        heapq.heappush(self.U, self.goal)
+
+    def __str__(self) -> str:
+        return "D* Lite"
+
+    def OnPress(self, event) -> None:
+        """
+        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))
+
+            cur_start, new_start = self.start, self.start
+            update_start = True
+            cost, count = 0, 0
+            path = [self.start.current]
+            self.EXPAND = []
+
+            while cur_start != self.goal:
+                neighbors = [node_n for node_n in self.getNeighbor(cur_start)
+                    if not self.isCollision(cur_start, node_n)]
+                next_node = min(neighbors, key=lambda n: n.g)
+                path.append(next_node.current)
+                cost += self.cost(cur_start, next_node)
+                count += 1
+                cur_start = next_node
+
+                if update_start:
+                    update_start = False
+                    self.km = self.h(cur_start, new_start)
+                    new_start = cur_start
+
+                    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)
+
+                    self.computeShortestPath()    
+        
+            # 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.start) and \
+                    self.start.rhs == self.start.g:
+                break
+
+            self.U.remove(node)
+            self.EXPAND.append(node)
+
+            # affected by obstacles
+            if node.key < self.calculateKey(node):
+                node.key = self.calculateKey(node)
+                heapq.heappush(self.U, node)
+            # Locally over-consistent -> Locally consistent
+            elif node.g > node.rhs:
+                node.g = node.rhs
+                for node_n in self.getNeighbor(node):
+                    self.updateVertex(node_n)
+            # 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): the node to be updated
+        """
+        # greed correction(reverse searching)
+        if node != self.goal:
+            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): the node to be calculated
+
+        Returns:
+            key (list): the priority of node
+        """
+        return [min(node.g, node.rhs) + self.h(node, self.start) + self.km,
+                min(node.g, node.rhs)]
+
+    def extractPath(self) -> tuple:
+        """
+        Extract the path based on greedy policy.
+
+        Returns:
+            cost (float): the cost of planning path
+            path (list): the planning path
+        """
+        node = self.start
+        path = [node.current]
+        cost, count = 0, 0
+        while node != self.goal:
+            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(path)

global_planner/graph_search/dijkstra.py

@@ -0,0 +1,77 @@
+"""
+@file: dijkstra.py
+@breif: Dijkstra 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 Dijkstra(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 "Dijkstra"
+
+    def plan(self) -> tuple:
+        """
+        Class for Dijkstra 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.Dijkstra((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 = 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 [], [], []