python-motion-planning 2.0.dev1__py3-none-any.whl → 2.0.dev2__py3-none-any.whl

python_motion_planning/path_planner/hybrid_search/voronoi_planner.py

@@ -0,0 +1,204 @@
+"""
+@file: voronoi.py
+@author: Wu Maojia
+@update: 2025.12.20
+"""
+import copy
+from typing import Union, List, Tuple, Dict, Any
+import heapq
+
+import numpy as np
+from scipy.spatial import Voronoi
+
+from python_motion_planning.common import Node, TYPES, Grid
+from python_motion_planning.path_planner.base_path_planner import BasePathPlanner
+from python_motion_planning.path_planner.graph_search.a_star import AStar
+
+
+class VoronoiPlanner(BasePathPlanner):
+    """
+    Path planner based on Voronoi diagram.
+    Core idea: find the nearest points on the Voronoi diagram to the start and goal,
+    plan the path on the Voronoi graph using a base planner, and then concatenate the full path.
+    Note that the boundary of grid map passed in Voronoi planner should be filled with obstacles using Grid.fill_boundary_with_obstacles() method. If not, the Voronoi planner will automatically fill.
+
+    Args:
+        *args: see the parent class.
+        base_planner: base planner class for path planning.
+        base_planner_kwargs: keyword arguments for the base planner.
+        cover_inflation: determine whether the voronoi candidates cover the inflation region.
+        *kwargs: see the parent class.
+
+    Examples:
+        >>> map_ = Grid(bounds=[[0, 15], [0, 15]])
+        >>> planner = VoronoiPlanner(map_=map_, start=(5, 5), goal=(10, 10))
+        >>> path, path_info = planner.plan()
+        >>> print(path_info['success'])
+        True
+        
+        >>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
+        >>> path, path_info = planner.plan()
+        >>> print(path_info['success'])
+        True
+    """
+    def __init__(self, 
+            *args, 
+            base_planner: BasePathPlanner = AStar, 
+            base_planner_kwargs: dict = {},
+            cover_inflation: bool = False,
+            **kwargs
+            ) -> None:
+        super().__init__(*args, **kwargs)
+
+        self.base_planner = base_planner
+        self.base_planner_kwargs = base_planner_kwargs
+        self.base_planner_kwargs["map_"] = self.map_
+        self.base_planner_kwargs["start"] = self.start
+        self.base_planner_kwargs["goal"] = self.goal
+        
+        self.cover_inflation = cover_inflation
+
+    def __str__(self) -> str:
+        return "Voronoi Planner"
+
+    def find_voronoi_candidates(self, map_: Grid) -> np.ndarray:
+        """
+        Find Voronoi candidate points for the grid map.
+
+        Args:
+            map_: grid map.
+
+        Returns:
+            candidates: Voronoi candidate points matrix.
+        """
+        generators = np.argwhere(map_.data == 1)
+        candidates = np.zeros_like(map_.data, dtype=np.bool_)
+        if len(generators) < 2:
+            return candidates
+
+        vor = Voronoi(generators)
+
+        for ridge in vor.ridge_vertices:
+            v1_idx, v2_idx = ridge[:2]
+            if v1_idx == -1 or v2_idx == -1:
+                continue  # skip infinite ridges
+                
+            # continuous coordinates
+            v1 = vor.vertices[v1_idx]
+            v2 = vor.vertices[v2_idx]
+            
+            v1 = map_.point_float_to_int(v1)
+            v2 = map_.point_float_to_int(v2)
+        
+            line = map_.line_of_sight(v1, v2)
+
+            for point in line:
+                candidates[point] = True
+
+        candidates[map_.data == TYPES.OBSTACLE] = False
+        if not self.cover_inflation:
+            candidates[map_.data == TYPES.INFLATION] = False
+
+        return candidates
+
+    def find_nearest_voronoi_point(self, 
+            point: Tuple[float, ...], 
+            voronoi_candidates: np.ndarray,
+        ) -> Union[Tuple[float, ...], None]:
+        """
+        Find the nearest Voronoi candidate point to the target point using brute force search.
+        
+        Args:
+            point: target point.
+            voronoi_candidates: Voronoi candidate points matrix.
+
+        Returns:
+            nearest_point: nearest Voronoi point.
+        """
+        min_dist = float('inf')
+        nearest_point = None
+
+        # Iterate through all Voronoi candidate points to find the nearest one  
+        for indices in np.ndindex(voronoi_candidates.shape):
+            if voronoi_candidates[indices]:
+                candidate_point = indices
+                dist = self.map_.get_distance(point, candidate_point)
+                if dist < min_dist:
+                    min_dist = dist
+                    nearest_point = candidate_point
+
+        return nearest_point
+
+    def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
+        """
+        Execute the path planning:
+        1. Compute Voronoi candidate points
+        2. Find the nearest Voronoi points for start and goal
+        3. Plan the path on the Voronoi graph
+        4. Concatenate the full path (start -> Voronoi start -> ... -> Voronoi goal -> goal)
+        
+        Returns:
+            path: A list containing the path waypoints
+            path_info: A dictionary containing the path information
+        """
+        voronoi_map = copy.deepcopy(self.map_)
+        voronoi_map.fill_boundary_with_obstacles()
+
+        # Compute Voronoi candidate points
+        voronoi_candidates = self.find_voronoi_candidates(voronoi_map)
+
+        # If no Voronoi candidates are found, fall back to normal base planner
+        if not np.any(voronoi_candidates):
+            return self.base_planner(**self.base_planner_kwargs).plan()
+
+        # Find the nearest Voronoi points for start and goal
+        start_voronoi = self.find_nearest_voronoi_point(self.start, voronoi_candidates)
+        goal_voronoi = self.find_nearest_voronoi_point(self.goal, voronoi_candidates)
+
+        # If no valid Voronoi points found, fall back to normal base planner
+        if start_voronoi is None or goal_voronoi is None:
+            return self.base_planner(**self.base_planner_kwargs).plan()
+
+        voronoi_map.type_map[voronoi_candidates] = TYPES.FREE
+        voronoi_map.type_map[~voronoi_candidates] = TYPES.OBSTACLE
+
+        self.base_planner_kwargs["map_"] = voronoi_map
+        self.base_planner_kwargs["start"] = start_voronoi
+        self.base_planner_kwargs["goal"] = goal_voronoi
+
+        voronoi_path, voronoi_path_info = self.base_planner(**self.base_planner_kwargs).plan()
+    
+        # If Voronoi path planning fails, fall back to normal base planner
+        if not voronoi_path_info["success"]:
+            self.base_planner_kwargs["map_"] = self.map_
+            self.base_planner_kwargs["start"] = self.start
+            self.base_planner_kwargs["goal"] = self.goal
+            return self.base_planner(**self.base_planner_kwargs).plan()
+
+        # Compute total path length and cost
+        start_segment_len = self.map_.get_distance(self.start, start_voronoi)
+        end_segment_len = self.map_.get_distance(goal_voronoi, self.goal)
+        total_length = voronoi_path_info["length"] + start_segment_len + end_segment_len
+
+        start_segment_cost = self.get_cost(self.start, start_voronoi)
+        end_segment_cost = self.get_cost(goal_voronoi, self.goal)
+        total_cost = voronoi_path_info["cost"] + start_segment_cost + end_segment_cost
+
+        # Concatenate the final path
+        final_path = [self.start] + voronoi_path + [self.goal]
+
+        # Collect path information
+        path_info = {
+            "success": True,
+            "start": self.start,
+            "goal": self.goal,
+            "length": total_length,
+            "cost": total_cost,
+            "expand": voronoi_path_info["expand"],
+            "voronoi_candidates": voronoi_candidates,
+            "voronoi_start": start_voronoi,
+            "voronoi_goal": goal_voronoi,
+            "voronoi_path": voronoi_path
+        }
+
+        return final_path, path_info

python_motion_planning/path_planner/sample_search/__init__.py

@@ -1,2 +1,3 @@
 from .rrt import *
-from .rrt_star import *
+from .rrt_star import *
+from .rrt_connect import *

python_motion_planning/path_planner/sample_search/rrt.py

@@ -1,12 +1,14 @@
 """
 @file: rrt.py
 @author: Wu Maojia, Yang Haodong
-@update: 2025.10.3
+@update: 2025.12.19
 """
 import math
 import random
+from typing import Union, Dict, List, Tuple, Any
+
 import numpy as np
-from typing import Union, Dict, List, Tuple
+import faiss
 
 from python_motion_planning.common import BaseMap, Node, TYPES, Grid
 from python_motion_planning.path_planner import BasePathPlanner
@@ -17,9 +19,11 @@ class RRT(BasePathPlanner):
 
     Args:
         *args: see the parent class.
-        max_dist: Maximum expansion distance for each step (default: 1.0).
-        sample_num: Maximum number of samples to generate (default: 10000).
-        goal_sample_rate: Probability of sampling the goal directly (default: 0.05).
+        max_dist: Maximum expansion distance for each step.
+        sample_num: Maximum number of samples to generate.
+        goal_sample_rate: Probability of sampling the goal directly.
+        discrete: Whether to use discrete or continuous space.
+        faiss: Whether to use Faiss to accelerate the search.
         *kwargs: see the parent class.
 
     References:
@@ -40,21 +44,20 @@ class RRT(BasePathPlanner):
     def __init__(self, *args, 
                  max_dist: float = 5.0, sample_num: int = 100000, 
                  goal_sample_rate: float = 0.05,
+                 discrete: bool = False,
+                 use_faiss: bool = True,
                  **kwargs) -> None:
         super().__init__(*args, **kwargs)
-        # Maximum expansion distance per step
         self.max_dist = max_dist
-        # Maximum number of samples
         self.sample_num = sample_num
-        # Goal sampling probability
         self.goal_sample_rate = goal_sample_rate
-        # Environment bounds from the map
-        self.bounds = self.map_.bounds
+        self.discrete = discrete
+        self.use_faiss = use_faiss
 
     def __str__(self) -> str:
-        return "Rapidly-exploring Random Tree (RRT)"
+        return "RRT"
 
-    def plan(self) -> Union[list, dict]:
+    def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
         """
         RRT path planning algorithm implementation.
 
@@ -67,6 +70,12 @@ class RRT(BasePathPlanner):
         start_node = Node(self.start, None, 0, 0)
         tree[self.start] = start_node
 
+        # Initialize FAISS index
+        if self.use_faiss:
+            faiss_index = faiss.IndexFlatL2(self.dim)
+            faiss_nodes = []
+            self._faiss_add_node(start_node, faiss_index, faiss_nodes)
+
         # Main sampling loop
         for _ in range(self.sample_num):
             # Generate random sample node
@@ -77,7 +86,7 @@ class RRT(BasePathPlanner):
                 continue
                 
             # Find nearest node in tree
-            node_near = self._get_nearest_node(tree, node_rand)
+            node_near = self._get_nearest_node(tree, node_rand, faiss_index, faiss_nodes)
             
             # Create new node towards random sample
             node_new = self._steer(node_near, node_rand)
@@ -85,19 +94,27 @@ class RRT(BasePathPlanner):
                 continue
                 
             # Check if edge is collision-free
-            if self.map_.in_collision(node_new.current, node_near.current):
+            if self.map_.in_collision(
+                self.map_.point_float_to_int(node_new.current), 
+                self.map_.point_float_to_int(node_near.current)
+                ):
                 continue
 
             # Add new node to tree
             node_new.parent = node_near.current
             node_new.g = node_near.g + self.get_cost(node_near.current, node_new.current)
             tree[node_new.current] = node_new
+            if self.use_faiss:
+                self._faiss_add_node(node_new, faiss_index, faiss_nodes)
 
             # Check if goal is reachable
             dist_to_goal = self.get_cost(node_new.current, self.goal)
             if dist_to_goal <= self.max_dist:
                 # Check final edge to goal
-                if not self.map_.in_collision(node_new.current, self.goal):
+                if not self.map_.in_collision(
+                    self.map_.point_float_to_int(node_new.current), 
+                    self.map_.point_float_to_int(self.goal)
+                    ):
                     if node_new.current == self.goal:
                         goal_node = node_new
                     else:
@@ -131,25 +148,37 @@ class RRT(BasePathPlanner):
             
         point = []
         # Generate random integer point within grid bounds
-        for d in range(self.map_.dim):
-            d_min, d_max = self.bounds[d]
-            point.append(random.randint(int(d_min), int(d_max)))
+        for d in range(self.dim):
+            d_min, d_max = -0.5, self.map_.shape[d] - 0.5
+            point.append(random.uniform(d_min, d_max))
         point = tuple(point)
 
+        if self.discrete:
+            point = self.map_.point_float_to_int(point)
+
         return Node(point, None, 0, 0)
 
     def _get_nearest_node(self, tree: Dict[Tuple[int, ...], Node], 
-                         node_rand: Node) -> Node:
+                         node_rand: Node, index=None, nodes=None) -> Node:
         """
         Find the nearest node in the tree to a random sample.
 
         Args:
             tree: Current tree of nodes
             node_rand: Random sample node
+            index: FAISS index (required when `use_faiss`=True)
+            nodes: List of nodes in FAISS index (required when `use_faiss`=True)
 
         Returns:
             node: Nearest node in the tree
         """
+        # knn search using faiss
+        if self.use_faiss:
+            query = np.array(node_rand.current, dtype=np.float32).reshape(1, -1)
+            _, indices = index.search(query, 1)
+            return nodes[indices[0][0]]
+
+        # brute force search
         min_dist = float('inf')
         nearest_node = None
         
@@ -174,7 +203,7 @@ class RRT(BasePathPlanner):
             node: New node in direction of random sample
         """
         # Calculate differences for each dimension
-        diffs = [node_rand.current[i] - node_near.current[i] for i in range(self.map_.dim)]
+        diffs = [node_rand.current[i] - node_near.current[i] for i in range(self.dim)]
         
         # Calculate Euclidean distance in n-dimensional space
         dist = math.sqrt(sum(diff**2 for diff in diffs))
@@ -189,13 +218,26 @@ class RRT(BasePathPlanner):
             
         # Otherwise scale to maximum distance
         scale = self.max_dist / dist
-        new_coords = [
-            node_near.current[i] + scale * diffs[i] 
-            for i in range(self.map_.dim)
+        new_point = [
+            node_near.current[i] + scale * diffs[i]
+            for i in range(self.dim)
         ]
-        
-        # Round coordinates if original points were integers
-        if all(isinstance(coord, int) for coord in node_near.current):
-            new_coords = [round(coord) for coord in new_coords]
+        new_point = tuple(new_point)
+
+        if self.discrete:
+            new_point = self.map_.point_float_to_int(new_point)
             
-        return Node(tuple(new_coords), None, 0, 0)
+        return Node(new_point, None, 0, 0)
+
+    def _faiss_add_node(self, node: Node, index, nodes):
+        """
+        Add a node to the FAISS index.
+
+        Args:
+            node: Node to add
+            index: FAISS index
+            nodes: List of nodes in FAISS index
+        """
+        vec = np.array(node.current, dtype=np.float32).reshape(1, -1)
+        index.add(vec)
+        nodes.append(node)

python_motion_planning/path_planner/sample_search/rrt_connect.py

@@ -0,0 +1,237 @@
+"""
+@file: rrt_connect.py
+@author: Wu Maojia
+@update: 2025.12.19
+"""
+from typing import Union, Dict, List, Tuple, Any
+
+import faiss
+
+from python_motion_planning.common import Node, Grid, TYPES
+from python_motion_planning.path_planner.sample_search.rrt import RRT
+
+class RRTConnect(RRT):
+    """
+    Class for RRT-Connect path planner, an improved version of RRT.
+
+    RRT-Connect uses two trees (one from start, one from goal) that grow towards
+    each other, which typically results in faster convergence than standard RRT.
+
+    Args:
+        *args: see the parent class.
+        *kwargs: see the parent class.
+
+    References:
+        [1] RRT-connect: An efficient approach to single-query path planning.
+        
+    Examples:
+        >>> map_ = Grid(bounds=[[0, 15], [0, 15]])
+        >>> planner = RRTConnect(map_=map_, start=(5, 5), goal=(10, 10))
+        >>> path, path_info = planner.plan()
+        >>> print(path_info['success'])
+        True
+        
+        >>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
+        >>> path, path_info = planner.plan()
+        >>> print(path_info['success'])
+        True
+    """
+    def __init__(self, *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
+        # Two trees for bidirectional search: one from start, one from goal
+        self.tree_a = None  # Tree originating from start point
+        self.tree_b = None  # Tree originating from goal point
+
+        if self.use_faiss:
+            self.index_a = None
+            self.index_b = None
+            self.nodes_a = []
+            self.nodes_b = []
+
+    def __str__(self) -> str:
+        return "RRT-Connect"
+
+    def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
+        """
+        RRT-Connect path planning algorithm implementation.
+
+        Returns:
+            path: A list containing the path waypoints
+            path_info: A dictionary containing path information
+        """
+        # Initialize both trees with start and goal nodes respectively
+        self.tree_a = {self.start: Node(self.start, None, 0, 0)}
+        self.tree_b = {self.goal: Node(self.goal, None, 0, 0)}
+
+        # Initialize FAISS index
+        if self.use_faiss:
+            self.index_a = faiss.IndexFlatL2(self.dim)
+            self.index_b = faiss.IndexFlatL2(self.dim)
+            self.nodes_a = []
+            self.nodes_b = []
+            self._faiss_add_node(self.tree_a[self.start], self.index_a, self.nodes_a)
+            self._faiss_add_node(self.tree_b[self.goal], self.index_b, self.nodes_b)
+
+        # Main planning loop
+        for _ in range(self.sample_num):
+            # Generate random sample node
+            node_rand = self._generate_random_node()
+            
+            # Extend tree A towards random sample
+            node_new_a = self._extend(self.tree_a, node_rand, self.index_a, self.nodes_a)
+            if node_new_a:
+                # Try to connect tree B to the new node from tree A
+                if self._connect(self.tree_b, node_new_a.current, self.index_b, self.nodes_b):
+                    tree_a_goal = self.tree_a.get(self.goal)
+                    if tree_a_goal is not None and tree_a_goal.parent is None:
+                        self.tree_a, self.tree_b = self.tree_b, self.tree_a
+                    # Path found - combine paths from both trees
+                    path_a, length_a, cost_a = self._extract_subpath(self.tree_a, node_new_a.current, self.start)
+                    path_b, length_b, cost_b = self._extract_subpath(self.tree_b, node_new_a.current, self.goal)
+                    path_a = path_a[::-1]
+                    # Combine paths (remove duplicate meeting point)
+                    full_path = path_a + path_b[1:]
+                    total_length = length_a + length_b
+                    total_cost = cost_a + cost_b
+                    
+                    return full_path, {
+                        "success": True,
+                        "start": self.start,
+                        "goal": self.goal,
+                        "length": total_length,
+                        "cost": total_cost,
+                        "expand": [self.tree_a, self.tree_b],
+                    }
+            
+            # Swap trees to maintain bidirectional growth
+            self.tree_a, self.tree_b = self.tree_b, self.tree_a
+
+            if self.use_faiss:
+                self.index_a, self.index_b = self.index_b, self.index_a
+                self.nodes_a, self.nodes_b = self.nodes_b, self.nodes_a
+
+        # If loop exits without return, planning failed
+        self.failed_info[1]["expand"] = [self.tree_a, self.tree_b]
+        return self.failed_info
+
+    def _extend(self, tree: Dict[Tuple[int, ...], Node], node_rand: Node, index=None, nodes=None) -> Union[Node, None]:
+        """
+        Extend the tree towards a random node, adding at most one new node.
+        
+        Args:
+            tree: The tree to extend
+            node_rand: The target node to extend towards
+            index: FAISS index (required when `use_faiss`=True)
+            nodes: List of nodes in FAISS index (required when `use_faiss`=True)
+            
+        Returns:
+            The new node added to the tree, or None if no node was added
+        """
+        # Find nearest node in the tree
+        node_near = self._get_nearest_node(tree, node_rand, index, nodes)
+        
+        # Steer towards the random node
+        node_new = self._steer(node_near, node_rand)
+        if node_new is None:
+            return None
+            
+        # Check if path is collision-free
+        if not self.map_.in_collision(
+            self.map_.point_float_to_int(node_near.current), 
+            self.map_.point_float_to_int(node_new.current)
+            ):
+            # Add new node to the tree
+            node_new.parent = node_near.current
+            node_new.g = node_near.g + self.get_cost(node_near.current, node_new.current)
+            tree[node_new.current] = node_new
+            if self.use_faiss:
+                self._faiss_add_node(node_new, index, nodes)
+            return node_new
+            
+        return None
+
+    def _connect(self, tree: Dict[Tuple[int, ...], Node], target: Tuple, index=None, nodes=None) -> bool:
+        """
+        Connect the tree to a target point by repeatedly extending towards it.
+        
+        Args:
+            tree: The tree to connect
+            target: The target point to connect to
+            index: FAISS index (required when `use_faiss`=True)
+            nodes: List of nodes in FAISS index (required when `use_faiss`=True)
+            
+        Returns:
+            True if connection successful, False otherwise
+        """
+        while True:
+            # Create node for the target
+            node_target = Node(target, None, 0, 0)
+            
+            # Find nearest node in the tree
+            node_near = self._get_nearest_node(tree, node_target, index, nodes)
+            
+            # Check distance to target
+            dist = self.get_cost(node_near.current, target)
+            
+            # If close enough, check final connection
+            if dist <= self.max_dist:
+                if not self.map_.in_collision(
+                    self.map_.point_float_to_int(node_near.current),
+                    self.map_.point_float_to_int(target)
+                    ):
+                    # Add target to tree
+                    node_new = Node(target, node_near.current, 
+                                   node_near.g + dist, 0)
+                    tree[target] = node_new
+                    if self.use_faiss:
+                        self._faiss_add_node(node_new, index, nodes)
+                    return True
+                return False
+                
+            # Otherwise, extend towards target
+            node_new = self._steer(node_near, node_target)
+            if node_new is None:
+                return False
+                
+            # Check collision
+            if self.map_.in_collision(
+                self.map_.point_float_to_int(node_near.current), 
+                self.map_.point_float_to_int(node_new.current)
+                ):
+                return False
+                
+            # Add new node to tree
+            node_new.parent = node_near.current
+            node_new.g = node_near.g + self.get_cost(node_near.current, node_new.current)
+            tree[node_new.current] = node_new
+            if self.use_faiss:
+                self._faiss_add_node(node_new, index, nodes)
+
+    def _extract_subpath(self, tree: dict, end_point: tuple, root: tuple) -> Tuple[List[Tuple[float, ...]], float, float]:
+        """
+        Extract a subpath from the root of the tree to the end_point.
+
+        Args:
+            tree: Tree to extract path from
+            end_point: End point of the subpath
+            root: Root point of the tree
+
+        Returns:
+            path: the subpath
+            length: length of the subpath
+            cost: cost of the subpath
+        """
+        length = 0
+        cost = 0
+        node = tree.get(end_point)
+        path = [node.current]
+        
+        while node.current != root:
+            node_parent = tree.get(node.parent)
+            length += self.map_.get_distance(node.current, node_parent.current)
+            cost += self.get_cost(node.current, node_parent.current)
+            node = node_parent
+            path.append(node.current)
+            
+        return path, length, cost
+