AStar

src.python_motion_planning.global_planner.graph_search.a_star.AStar

Bases: GraphSearcher

Class for A* motion planning.

Parameters:

Name	Type	Description	Default
start	tuple	start point coordinate	required
goal	tuple	goal point coordinate	required
env	Env	environment	required
heuristic_type	str	heuristic function type	'euclidean'

Examples:

Python Console Session

>>> 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

Source code in src\python_motion_planning\global_planner\graph_search\a_star.py

Python

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)

extractPath(closed_list)

Extract the path based on the CLOSED list.

Parameters:

Name	Type	Description	Default
closed_list	dict	CLOSED list	required

Returns:

Name	Type	Description
cost	float	the cost of planned path
path	list	the planning path

Source code in src\python_motion_planning\global_planner\graph_search\a_star.py

Python

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

getNeighbor(node)

Find neighbors of node.

Parameters:

Name	Type	Description	Default
node	Node	current node	required

Returns:

Name	Type	Description
neighbors	list	neighbors of current node

Source code in src\python_motion_planning\global_planner\graph_search\a_star.py

Python

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)]

plan()

A* motion plan function.

Returns:

Name	Type	Description
cost	float	path cost
path	list	planning path
expand	list	all nodes that planner has searched

Source code in src\python_motion_planning\global_planner\graph_search\a_star.py

Python

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 [], [], []

run()

Running both planning and animation.

Source code in src\python_motion_planning\global_planner\graph_search\a_star.py

Python

def run(self):
    """
    Running both planning and animation.
    """
    cost, path, expand = self.plan()
    self.plot.animation(path, str(self), cost, expand)