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

global_planner/__init__.py

@@ -0,0 +1,3 @@
+from .graph_search import *
+from .sample_search import *
+from .evolutionary_search import *

global_planner/evolutionary_search/__init__.py

@@ -0,0 +1,4 @@
+from .aco import ACO
+from .pso import PSO
+
+__all__ = ["ACO", "PSO"]

global_planner/evolutionary_search/aco.py

@@ -0,0 +1,186 @@
+"""
+@file: aco.py
+@breif: Ant Colony Optimization(ACO) motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import random
+from bisect import bisect_left
+
+from .evolutionary_search import EvolutionarySearcher
+from python_motion_planning.utils import Env, Node
+
+
+class ACO(EvolutionarySearcher):
+    """
+    Class for Ant Colony Optimization(ACO) 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_ants (int): number of ants
+        alpha (float): pheromone and heuristic factor weight coefficient
+        beta (float): pheromone and heuristic factor weight coefficient
+        rho (float): evaporation coefficient
+        Q (float): pheromone gain
+        max_iter (int): maximum iterations
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.ACO((5, 5), (45, 25), pmp.Grid(51, 31))
+        >>> cost, path, cost_list = planner.plan()     # planning results only
+        >>> planner.plot.animation(path, str(planner), cost, cost_curve=cost_list)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] Ant Colony Optimization: A New Meta-Heuristic
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean", 
+        n_ants: int = 50, alpha: float = 1.0, beta: float = 5.0, rho: float = 0.1, Q: float = 1.0,
+        max_iter: int = 100) -> None:
+        super().__init__(start, goal, env, heuristic_type)
+        self.n_ants = n_ants
+        self.alpha = alpha
+        self.beta = beta
+        self.rho = rho
+        self.Q = Q
+        self.max_iter = max_iter
+
+    def __str__(self) -> str:
+        return "Ant Colony Optimization(ACO)"
+
+    class Ant:
+        def __init__(self) -> None:
+            self.reset()
+        
+        def reset(self) -> None:
+            self.found_goal = False
+            self.current_node = None
+            self.path = []
+            self.path_set = set()
+            self.steps = 0
+
+    def plan(self) -> tuple:
+        """
+        Ant Colony Optimization(ACO) motion plan function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+        """
+        best_length_list, best_path = [], None
+
+        # pheromone initialization
+        pheromone_edges = {}
+        for i in range(self.env.x_range):
+            for j in range(self.env.y_range):
+                if (i, j) in self.obstacles:
+                    continue
+                cur_node = Node((i, j), (i, j), 0, 0)
+                for node_n in self.getNeighbor(cur_node):
+                    pheromone_edges[(cur_node, node_n)] = 1.0
+
+        # heuristically set max steps
+        max_steps = self.env.x_range * self.env.y_range / 2 + max(self.env.x_range, self.env.y_range)
+
+        # main loop
+        cost_list = []
+        for _ in range(self.max_iter):
+            ants_list = []
+            for _ in range(self.n_ants):
+                ant = self.Ant()
+                ant.current_node = self.start
+                while ant.current_node is not self.goal and ant.steps < max_steps:
+                    ant.path.append(ant.current_node)
+                    ant.path_set.add(ant.current_node.current)
+
+                    # candidate
+                    prob_sum = 0.0
+                    next_positions, next_probabilities = [], []
+                    for node_n in self.getNeighbor(ant.current_node):                
+                        # existed
+                        if node_n.current in ant.path_set:
+                            continue
+                        
+                        node_n.parent = ant.current_node.current
+
+                        # goal found
+                        if node_n == self.goal:
+                            ant.path.append(node_n)
+                            ant.path_set.add(node_n.current)
+                            ant.found_goal = True
+                            break
+
+                        next_positions.append(node_n)
+                        prob_new = pheromone_edges[(ant.current_node, node_n)] ** self.alpha \
+                                    * (1.0 / self.h(node_n, self.goal)) ** self.beta
+                        next_probabilities.append(prob_new)
+                        prob_sum = prob_sum + prob_new
+                    
+                    if prob_sum == 0 or ant.found_goal:
+                        break
+
+                    # roulette selection
+                    next_probabilities = list(map(lambda prob: prob / prob_sum, next_probabilities))
+                    p0, cp = 0, []
+                    for prob in next_probabilities:
+                        p0 = p0 + prob
+                        cp.append(p0)
+                    ant.current_node = next_positions[bisect_left(cp, random.random())]
+
+                    ant.steps = ant.steps + 1
+
+                ants_list.append(ant)
+
+            # pheromone deterioration
+            for key, _ in pheromone_edges.items():
+                pheromone_edges[key] *= (1 - self.rho)
+            
+            # pheromone update based on successful ants
+            bpl, bp = float("inf"), None
+            for ant in ants_list:
+                if ant.found_goal:
+                    if len(ant.path) < bpl:
+                        bpl, bp = len(ant.path), ant.path
+                    c = self.Q / len(ant.path)
+                    for i in range(len(ant.path) - 1):
+                        pheromone_edges[(ant.path[i], ant.path[i + 1])] += c
+            
+            if bpl < float("inf"):
+                best_length_list.append(bpl)
+
+            if len(best_length_list) > 0:
+                cost_list.append(min(best_length_list))
+                if bpl <= min(best_length_list):
+                    best_path = bp
+
+        if best_path:
+            cost = 0
+            path = [self.start.current]
+            for i in range(len(best_path) - 1):
+                cost += self.dist(best_path[i], best_path[i + 1])
+                path.append(best_path[i + 1].current)
+            return cost, path, cost_list
+        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 run(self) -> None:
+        """
+        Running both plannig and animation.
+        """
+        cost, path, cost_list = self.plan()
+        self.plot.animation(path, str(self), cost, cost_curve=cost_list)

global_planner/evolutionary_search/evolutionary_search.py

@@ -0,0 +1,87 @@
+"""
+@file: evolutionary_search.py
+@breif: Base class for planner based on evolutionary searching
+@author: Winter
+@update: 2023.7.13
+"""
+import math
+
+from python_motion_planning.utils import Env, Node, Planner
+
+class EvolutionarySearcher(Planner):
+    """
+    Base class for planner based on evolutionary 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.current
+        x2, y2 = node2.current
+
+        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

global_planner/evolutionary_search/pso.py

@@ -0,0 +1,356 @@
+"""
+@file: pso.py
+@breif: Particle Swarm Optimization (PSO) motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.24
+"""
+import random, math
+from copy import deepcopy
+
+from .evolutionary_search import EvolutionarySearcher
+from python_motion_planning.utils import Env, MathHelper
+from python_motion_planning.curve_generation import BSpline
+
+GEN_MODE_CIRCLE = 0
+GEN_MODE_RANDOM = 1
+
+
+class PSO(EvolutionarySearcher):
+    """
+    Class for Particle Swarm Optimization (PSO) motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        heuristic_type (str): heuristic function type
+        n_particles (int): number of particles
+        w_inertial (float): inertial weight
+        w_cognitive (float): cognitive weight
+        w_social (float): social weight
+        point_num (int): number of position points contained in each particle
+        max_speed (int): The maximum velocity of particle motion
+        max_iter (int): maximum iterations
+        init_mode (int): Set the generation mode for the initial position points of the particle swarm
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.PSO((5, 5), (45, 25), pmp.Grid(51, 31))
+        >>> cost, path, fitness_history = planner.plan(verbose=True)     # planning results only
+        >>> cost_curve = [-f for f in fitness_history]
+        >>> planner.plot.animation(path, str(planner), cost, cost_curve=cost_curve)  # animation
+        >>> planner.run()       # run both planning and animation
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean", 
+        n_particles: int = 300, point_num: int = 5, w_inertial: float = 1.0,
+        w_cognitive: float = 1.0, w_social: float = 1.0, max_speed: int = 6,
+        max_iter: int = 200, init_mode: int = GEN_MODE_RANDOM) -> None:
+        super().__init__(start, goal, env, heuristic_type)
+        self.max_iter = max_iter
+        self.n_particles = n_particles
+        self.w_inertial = w_inertial
+        self.w_social = w_social
+        self.w_cognitive = w_cognitive
+        self.point_num = point_num
+        self.init_mode = init_mode
+        self.max_speed = max_speed
+
+        self.particles = []
+        self.inherited_particles = []
+        self.best_particle = self.Particle()
+        self.b_spline_gen = BSpline(step=0.01, k=4)
+
+    def __str__(self) -> str:
+        return "Particle Swarm Optimization (PSO)"
+
+    class Particle:
+        def __init__(self) -> None:
+            self.reset()
+        
+        def reset(self):
+            self.position = []
+            self.velocity = []
+            self.fitness = -1
+            self.best_pos = []
+            self.best_fitness = -1
+        
+    def plan(self, verbose: bool = False):
+        """
+        Particle Swarm Optimization (PSO) motion plan function.
+
+        Parameters:
+            verbose (bool): print the best fitness value of each iteration
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+        """
+        # Generate initial position of particle swarm
+        init_positions = self.initializePositions()
+
+        # Particle initialization
+        for i in range(self.n_particles):
+            # Calculate fitness
+            init_fitness = self.calFitnessValue(init_positions[i])
+
+            if i == 0 or init_fitness > self.best_particle.fitness:
+                self.best_particle.fitness = init_fitness
+                self.best_particle.position = deepcopy(init_positions[i])
+            
+            # Create and add particle objects to containers
+            p = self.Particle()
+            p.position = init_positions[i]
+            p.velocity = [(0, 0) for _ in range(self.point_num)]
+            p.best_pos = init_positions[i]
+            p.fitness = init_fitness
+            p.best_fitness = init_fitness
+            self.particles.append(p)
+
+        # Iterative optimization
+        fitness_history = []
+        for _ in range(self.max_iter):
+            for p in self.particles:
+                self.optimizeParticle(p)
+            fitness_history.append(self.best_particle.fitness)
+            if verbose:
+                print(f"iteration {_}: best fitness = {self.best_particle.fitness}")
+
+        # Generating Paths from Optimal Particles
+        points = [self.start.current] + self.best_particle.position + [self.goal.current]
+        points = sorted(set(points), key=points.index)
+        path = self.b_spline_gen.run(points, display=False)
+
+        return self.b_spline_gen.length(path), path, fitness_history
+
+    def initializePositions(self) -> list:
+        """
+        Generate n particles with pointNum_ positions each within the map range.
+
+        Returns:
+            init_positions (list): initial position sequence of particle swarm
+        """
+        init_positions = []
+
+        # Calculate sequence direction
+        x_order = self.goal.x > self.start.x
+        y_order = self.goal.y > self.start.y
+
+        # circle generation
+        center_x, center_y, radius = None, None, None
+        if self.init_mode == GEN_MODE_CIRCLE:
+            # Calculate the center and the radius of the circle (midpoint between start and goal)
+            center_x = (self.start.x + self.goal.x) / 2
+            center_y = (self.start.y + self.goal.y) / 2
+            radius = 5 if self.dist(self.start, self.goal) / 2.0 < 5 else self.dist(self.start, self.goal) / 2.0
+
+        # initialize n_particles positions
+        for _ in range(self.n_particles):
+            point_id, visited = 0, []
+            pts_x, pts_y = [], []
+            # Generate point_num_ unique coordinates
+            while point_id < self.point_num:
+                if self.init_mode == GEN_MODE_RANDOM:
+                    pt_x = random.randint(self.start.x, self.goal.x)
+                    pt_y = random.randint(self.start.y, self.goal.y)
+                    pos_id = pt_x + self.env.x_range * pt_y
+                else:
+                    # Generate random angle in radians
+                    angle = random.random() * 2 * math.pi
+                    # Generate random distance from the center within the circle
+                    r = math.sqrt(random.random()) * radius
+                    # Convert polar coordinates to Cartesian coordinates
+                    pt_x = int(center_x + r * math.cos(angle))
+                    pt_y = int(center_y + r * math.sin(angle))
+                    # Check if the coordinates are within the map range
+                    if 0 <= pt_x < self.env.x_range and 0 <= pt_y < self.env.y_range:
+                        pos_id = pt_x + self.env.x_range * pt_y
+                    else:
+                        continue
+
+                # Check if the coordinates have already been used
+                if not pos_id in visited:
+                    point_id = point_id + 1
+                    visited.append(pos_id)
+                    pts_x.append(pt_x)
+                    pts_y.append(pt_y)
+
+            # sort
+            pts_x = sorted(pts_x, reverse=False) if x_order else sorted(pts_x, reverse=True)
+            pts_y = sorted(pts_y, reverse=False) if y_order else sorted(pts_y, reverse=True)
+
+            # Store elements from x and y in particle_positions
+            init_positions.append([(ix, iy) for (ix, iy) in zip(pts_x, pts_y)])
+        
+        return init_positions
+
+    def calFitnessValue(self, position: list) -> float:
+        """
+        Calculate the value of fitness function.
+
+        Parameters:
+            position (list): control points calculated by PSO
+
+        Returns:
+            fitness (float): the value of fitness function
+        """
+        points = [self.start.current] + position + [self.goal.current]
+        points = sorted(set(points), key=points.index)
+        try:
+            path = self.b_spline_gen.run(points, display=False)
+        except:
+            return float("inf")
+
+        # collision detection
+        obs_cost = 0
+        for i in range(len(path) - 1):
+            p1 = (round(path[i][0]), round(path[i][1]))
+            p2 = (round(path[i+1][0]), round(path[i+1][1]))
+            if self.isCollision(p1, p2):
+                obs_cost = obs_cost + 1
+
+        # Calculate particle fitness
+        return 100000.0 / (self.b_spline_gen.length(path) + 50000 * obs_cost)
+
+    def updateParticleVelocity(self, particle):
+        """
+        A function to update the particle velocity
+
+        Parameters:
+            particle (Particle): the particle
+        """
+        # update Velocity
+        for i in range(self.point_num):
+            rand1, rand2 = random.random(), random.random()
+            vx, vy = particle.velocity[i]
+            px, py = particle.position[i]
+            vx_new = self.w_inertial * vx + self.w_cognitive * rand1 * (particle.best_pos[i][0] - px) \
+                + self.w_social * rand2 * (self.best_particle.position[i][0] - px)
+
+            vy_new = self.w_inertial * vy + self.w_cognitive * rand1 * (particle.best_pos[i][1] - py) \
+                + self.w_social * rand2 * (self.best_particle.position[i][1] - py)
+
+            # Velocity Scaling
+            if self.env.x_range > self.env.y_range:
+                vx_new *= self.env.x_range / self.env.y_range
+            else:
+                vy_new *= self.env.y_range / self.env.x_range
+
+            # Velocity limit
+            vx_new = MathHelper.clamp(vx_new, -self.max_speed, self.max_speed)
+            vy_new = MathHelper.clamp(vy_new, -self.max_speed, self.max_speed)
+            particle.velocity[i] = (vx_new, vy_new)
+
+    def updateParticlePosition(self, particle):
+        """
+        A function to update the particle position
+
+        Parameters:
+            particle (Particle): the particle
+        """
+        # update Position
+        for i in range(self.point_num):
+            px = particle.position[i][0] + int(particle.velocity[i][0])
+            py = particle.position[i][1] + int(particle.velocity[i][1])
+
+            # Position limit
+            px = MathHelper.clamp(px, 0, self.env.x_range - 1)
+            py = MathHelper.clamp(py, 0, self.env.y_range - 1)
+
+            particle.position[i] = (px, py)
+        particle.position.sort(key=lambda p: p[0])
+
+    def optimizeParticle(self, particle):
+        """
+        Particle update optimization iteration
+
+        Parameters:
+            particle (Particle): the particle
+        """
+        # update speed
+        self.updateParticleVelocity(particle)
+        # update position
+        self.updateParticlePosition(particle)
+
+        # Calculate fitness
+        particle.fitness = self.calFitnessValue(particle.position)
+
+        # Update individual optima
+        if particle.fitness > particle.best_fitness:
+            particle.best_fitness = particle.fitness
+            particle.best_pos = particle.position
+
+        # Update global optimal particles
+        if particle.best_fitness > self.best_particle.fitness:
+            self.best_particle.fitness = particle.best_fitness
+            self.best_particle.position = deepcopy(particle.position)
+
+    def run(self):
+        """
+        Running both plannig and animation.
+        """
+        cost, path, fitness_history = self.plan(verbose=True)
+        cost_curve = [-f for f in fitness_history]
+        self.plot.animation(path, str(self), cost, cost_curve=cost_curve)
+
+    def isCollision(self, p1: tuple, p2: tuple) -> bool:
+        """
+        Judge collision when moving from node1 to node2 using Bresenham.
+
+        Parameters:
+            p1 (tuple): start point
+            p2 (tuple): end point
+
+        Returns:
+            collision (bool): True if collision exists, False otherwise.
+        """
+        if p1 in self.obstacles or p2 in self.obstacles:
+            return True
+
+        x1, y1 = p1
+        x2, y2 = p2
+
+        if x1 < 0 or x1 >= self.env.x_range or y1 < 0 or y1 >= self.env.y_range:
+            return True
+        if x2 < 0 or x2 >= self.env.x_range or y2 < 0 or y2 >= self.env.y_range:
+            return True
+
+        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 True
+        # 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 True
+
+        return False

global_planner/graph_search/__init__.py

@@ -0,0 +1,28 @@
+from .a_star import AStar
+from .dijkstra import Dijkstra
+from .gbfs import GBFS
+from .jps import JPS
+from .d_star import DStar
+from .lpa_star import LPAStar
+from .d_star_lite import DStarLite
+from .voronoi import VoronoiPlanner
+from .theta_star import ThetaStar
+from .lazy_theta_star import LazyThetaStar
+from .s_theta_star import SThetaStar
+# from .anya import Anya
+# from .hybrid_a_star import HybridAStar
+
+__all__ = ["AStar",
+           "Dijkstra",
+           "GBFS",
+           "JPS",
+           "DStar",
+           "LPAStar",
+           "DStarLite",
+           "VoronoiPlanner",
+           "ThetaStar",
+           "LazyThetaStar",
+           "SThetaStar",
+           # "Anya",
+           # "HybridAStar"
+        ]