python-motion-planning 2.0.dev1__py3-none-any.whl → 2.0.1__py3-none-any.whl

python_motion_planning/path_planner/sample_search/rrt_star.py

@@ -1,29 +1,43 @@
 """
 @file: rrt_star.py
-@author: Wu Maojia, Yang Haodong
-@update: 2025.10.3
+@author: Wu Maojia
+@update: 2026.4.12
 """
 import math
 import random
-from typing import Union, Dict, List, Tuple
+from typing import Union, Dict, List, Tuple, Any
+
+import numpy as np
+import faiss
+
+from python_motion_planning.common import Node, Grid, TYPES
+from python_motion_planning.path_planner.sample_search import RRT
+from python_motion_planning.common.utils.child_tree import ChildTree
 
-from python_motion_planning.common import BaseMap, Node, Grid, TYPES
-from python_motion_planning.path_planner.sample_search.rrt import RRT
 
 class RRTStar(RRT):
     """
     Class for RRT* (Rapidly-exploring Random Tree Star) path planner.
-    
-    RRT* is an optimized version of RRT that provides asymptotically optimal paths
-    by rewiring the tree to maintain lower-cost connections.
+
+    RRT* extends RRT by:
+        1. Selecting the best parent (minimum cost) for a new node.
+        2. Rewiring nearby nodes through the new node if it improves their cost.
 
     Args:
-        *args: see the parent class.
-        radius: Radius for finding nearby nodes during rewiring.
-        *kwargs: see the parent class.
+        *args: see parent class.
+        rewire_radius: Neighborhood radius for rewiring (If None, adaptively calculated with gamma factor).
+        gamma: A factor for calculating rewire_radius. For details, see [1]. (Disabled when rewire_radius is not None)
+        stop_func: A callable(cur_step, first_success_step, max_sample_step) -> bool that controls when to stop sampling. Called at the beginning of each iteration.
+            Arguments:
+                - cur_step (int): Number of sampling iterations completed so far (starts from 0, incremented before each sampling, so the first completed sample has cur_step=1).
+                - first_success_step (int or None): The cur_step value at which a feasible path was first found, or None if no path has been found yet.
+                - max_sample_step (int): The planner's max_sample_step attribute, provided for convenience.
+            Returns True to stop, False to continue.
+        propagate_cost_to_children: Whether to propagate cost to children. This is a fix for ensuring the correctness of g-value of each node. But it may slow the algorithm a little.
+        *kwargs: see parent class.
 
     References:
-        [1] Sampling-based algorithms for optimal motion planning
+        [1] Sampling-based Algorithms for Optimal Motion Planning
 
     Examples:
         >>> map_ = Grid(bounds=[[0, 15], [0, 15]])
@@ -32,178 +46,234 @@ class RRTStar(RRT):
         >>> print(path_info['success'])
         True
         
-        >>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
+        >>> planner = RRTStar(map_=map_, start=(5, 5), goal=(10, 10), max_sample_step=100000, stop_func=lambda current_step, first_success_step, max_step: (first_success_step is not None) or (current_step >= max_step))
         >>> path, path_info = planner.plan()
         >>> print(path_info['success'])
         True
     """
-    def __init__(self, *args, 
-                 radius: float = 10.0,** kwargs) -> None:
+
+    def __init__(self, *args,
+                 rewire_radius: float = None,
+                 gamma: float = 50.0,
+                 stop_func: callable = lambda current_step, first_success_step, max_step: (first_success_step is not None) or (current_step >= max_step),
+                 propagate_cost_to_children: bool = True,
+                 **kwargs) -> None:
         super().__init__(*args, **kwargs)
-        # Radius for finding nearby nodes during rewiring phase
-        self.radius = radius
+        self.rewire_radius = rewire_radius
+        self.gamma = gamma
+        self.stop_func = stop_func
+
+        self.failed_info[1]["first_success_step"] = None
+        self.failed_info[1]["best_step"] = None
+        self.failed_info[1]["total_step"] = None
+
+        self.best_results = self.failed_info
+        self.propagate_cost_to_children = propagate_cost_to_children
+
+        self._tree = None
+        self._child = None
 
     def __str__(self) -> str:
-        return "Rapidly-exploring Random Tree Star (RRT*)"
+        return "RRT*"
 
-    def plan(self) -> Union[list, dict]:
+    def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
         """
-        RRT* path planning algorithm implementation with optimality properties.
-        
+        RRT* path planning algorithm implementation.
+
         Returns:
             path: A list containing the path waypoints
             path_info: A dictionary containing path information
         """
         # Initialize tree with start node
-        tree = {}
+        self._tree = {}
+        self._child = ChildTree()
         start_node = Node(self.start, None, 0, 0)
-        tree[self.start] = start_node
+        self._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)
+
+        i = 0
+        first_success_i = None
+        while True:
+            if self.stop_func(i, first_success_i, self.max_sample_step):
+                break
+                
+            i += 1
 
-        # Main sampling loop
-        for _ in range(self.sample_num):
-            # Generate random sample node
+            # Generate random sample
             node_rand = self._generate_random_node()
-            
-            # Skip if node already exists
-            if node_rand.current in tree:
+
+            if node_rand.current in self._tree:
                 continue
-                
-            # Find nearest node in tree
-            node_near = self._get_nearest_node(tree, node_rand)
-            
-            # Create new node towards random sample
-            node_new = self._steer(node_near, node_rand)
+
+            # Find nearest node
+            node_nearest = self._get_nearest_node(self._tree, node_rand, faiss_index, faiss_nodes)
+
+            # Steer towards random sample
+            node_new = self._steer(node_nearest, node_rand)
             if node_new is None:
                 continue
-                
-            # Check if edge is collision-free
-            if self.map_.in_collision(node_new.current, node_near.current):
-                continue
 
-            # Find all nearby nodes within radius
-            near_nodes = self._find_near_nodes(tree, node_new)
-            
-            # Select optimal parent from nearby nodes
-            node_new, node_near = self._choose_parent(node_new, node_near, near_nodes)
-            if node_new is None:
+            # Collision check
+            if self.map_.in_collision(
+                self.map_.point_float_to_int(node_nearest.current), 
+                self.map_.point_float_to_int(node_new.current)
+                ):
                 continue
-                
-            # Add new node to tree
-            tree[node_new.current] = node_new
-            
-            # Rewire tree to potentially improve existing paths
-            self._rewire(tree, node_new, near_nodes)
 
-            # Check if goal is reachable
+            # Find nearby nodes for choosing best parent
+            near_nodes = self._get_near_nodes(node_new, faiss_index, faiss_nodes)
+
+            # Choose parent with minimum cost
+            min_parent = node_nearest
+            min_cost = node_nearest.g + self.get_cost(node_nearest.current, node_new.current)
+
+            for node in near_nodes:
+                if self.map_.in_collision(
+                    self.map_.point_float_to_int(node.current), 
+                    self.map_.point_float_to_int(node_new.current)
+                    ):
+                    continue
+                cost = node.g + self.get_cost(node.current, node_new.current)
+                if cost < min_cost:
+                    min_parent = node
+                    min_cost = cost
+
+            # Add new node
+            if self.propagate_cost_to_children:
+                self._child.remove(node_new.parent, node_new.current)
+                self._child.add(min_parent.current, node_new.current)
+
+            node_new.parent = min_parent.current
+            node_new.g = min_cost
+            self._tree[node_new.current] = node_new
+            if self.use_faiss:
+                self._faiss_add_node(node_new, faiss_index, faiss_nodes)
+
+            # Rewire nearby nodes through new node
+            for node in near_nodes:
+                if node.current == min_parent.current:
+                    continue
+                if self.map_.in_collision(
+                    self.map_.point_float_to_int(node.current),
+                    self.map_.point_float_to_int(node_new.current)
+                    ):
+                    continue
+
+                new_cost = node_new.g + self.get_cost(node_new.current, node.current)
+                if new_cost < node.g:
+                    if self.propagate_cost_to_children:
+                        self._child.remove(node.parent, node.current)
+                        self._child.add(node_new.current, node.current)
+
+                    node.parent = node_new.current
+                    node.g = new_cost
+                    self._tree[node.current] = node
+
+                    if self.propagate_cost_to_children:
+                        self._propagate_cost_to_children(node)
+
+            # Check goal connection
             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)
+                    ):
                     goal_cost = node_new.g + dist_to_goal
-                    # Update goal node if it already exists with a lower cost path
-                    if self.goal in tree:
-                        if goal_cost < tree[self.goal].g:
-                            tree[self.goal].parent = node_new.current
-                            tree[self.goal].g = goal_cost
-                    else:
-                        goal_node = Node(self.goal, node_new.current, goal_cost, 0)
-                        tree[self.goal] = goal_node
-                    
-                    # Extract and return the path
-                    path, length, cost = self.extract_path(tree)
-                    return path, {
-                        "success": True,
-                        "start": self.start,
-                        "goal": self.goal,
-                        "length": length,
-                        "cost": cost,
-                        "expand": tree,
-                    }
-
-        # Planning failed
-        self.failed_info[1]["expand"] = tree
-        return self.failed_info
-
-    def _find_near_nodes(self, tree: Dict[tuple, Node], node_new: Node) -> List[Node]:
+                    if self.goal not in self._tree or goal_cost < self._tree[self.goal].g:
+                        if node_new.current == self.goal:
+                            self._tree[self.goal] = node_new
+                        else:
+                            self._tree[self.goal] = Node(
+                                self.goal,
+                                node_new.current,
+                                goal_cost,
+                                0
+                            )
+                        path, length, cost = self.extract_path(self._tree)
+                        
+                        if first_success_i is None:
+                            first_success_i = i
+
+                        self.best_results = path, {
+                            "success": True,
+                            "start": self.start,
+                            "goal": self.goal,
+                            "length": length,
+                            "cost": cost,
+                            "first_success_step": first_success_i,
+                            "best_step": i,
+                            "total_step": i,
+                            "expand": self._tree,
+                        }
+
+
+        # Planning stopped
+        self.best_results[1]["total_step"] = i
+        self.best_results[1]["expand"] = self._tree
+        return self.best_results
+
+    def _get_near_nodes(self, node_new: Node, index=None, nodes=None) -> List[Node]:
         """
-        Find all nodes in the tree within a specified radius of the new node.
-        
+        Get nearby nodes within rewiring radius.
+
         Args:
-            tree: Current tree of nodes
-            node_new: Newly created node
-            
+            node_new: Newly added node
+            index: FAISS index (required when `use_faiss`=True)
+            nodes: List of nodes in FAISS index (required when `use_faiss`=True)
+
         Returns:
-            near_nodes: List of nodes within the specified radius
+            near_nodes: List of nearby nodes
         """
+        # Adaptive radius from RRT* theory
+        if self.rewire_radius is None:
+            n = len(self._tree) + 1
+            radius = self.gamma * ((math.log(n) / n) ** (1 / self.dim))
+            if self.discrete:
+                radius = max(1, radius)
+        else:
+            radius = self.rewire_radius
+
         near_nodes = []
-        for node in tree.values():
-            if self.get_cost(node.current, node_new.current) <= self.radius:
-                near_nodes.append(node)
+        
+        if self.use_faiss:
+            # range search using faiss
+            query = np.asarray(node_new.current, dtype=np.float32).reshape(1, -1)
+            lims, D, I = index.range_search(query, radius * radius)
+            for idx in I:
+                near_nodes.append(nodes[idx])
+
+        else:
+            # brute force radius search
+            for node in self._tree.values():
+                if self.get_cost(node.current, node_new.current) <= radius:
+                    near_nodes.append(node)
+
         return near_nodes
 
-    def _choose_parent(self, node_new: Node, node_near: Node, near_nodes: List[Node]) -> Tuple[Union[Node, None], Node]:
-        """
-        Select the optimal parent for the new node from nearby nodes to minimize cost.
-        
-        Args:
-            node_new: Newly created node
-            node_near: Nearest node in the tree
-            near_nodes: List of nearby nodes
-            
-        Returns:
-            node_new: Updated new node with optimal parent
-            node_near: The chosen parent node
+    def _propagate_cost_to_children(self, node: Node):
         """
-        # Initialize with nearest node as potential parent
-        node_new.g = node_near.g + self.get_cost(node_near.current, node_new.current)
-        best_parent = node_near
-        
-        # Check all nearby nodes for potentially lower-cost paths
-        for node_near_candidate in near_nodes:
-            # Skip if candidate is the same as initial nearest node
-            if node_near_candidate.current == best_parent.current:
-                continue
-                
-            # Check if path from candidate to new node is collision-free
-            if self.map_.in_collision(node_near_candidate.current, node_new.current):
-                continue
-                
-            # Calculate cost through this candidate parent
-            new_cost = node_near_candidate.g + self.get_cost(node_near_candidate.current, node_new.current)
-            
-            # Update parent if this path is cheaper
-            if new_cost < node_new.g:
-                node_new.g = new_cost
-                best_parent = node_near_candidate
-        
-        # Set the best parent for the new node
-        node_new.parent = best_parent.current
-        return node_new, best_parent
+        Propagate cost update to children of a node.
 
-    def _rewire(self, tree: Dict[tuple, Node], node_new: Node, near_nodes: List[Node]) -> None:
-        """
-        Rewire the tree to potentially improve paths for existing nodes using the new node.
-        
         Args:
-            tree: Current tree of nodes
-            node_new: Newly added node
-            near_nodes: List of nearby nodes
+            node: Node to propagate cost to children
         """
-        for node_near in near_nodes:
-            # Skip if node is the parent of the new node
-            if node_near.current == node_new.parent:
-                continue
-                
-            # Check if path from new node to existing node is collision-free
-            if self.map_.in_collision(node_new.current, node_near.current):
-                continue
-                
-            # Calculate potential new cost for existing node through new node
-            new_cost = node_new.g + self.get_cost(node_new.current, node_near.current)
-            
-            # Update parent if new path is cheaper
-            if new_cost < node_near.g:
-                node_near.parent = node_new.current
-                node_near.g = new_cost
-                # Update the node in the tree
-                tree[node_near.current] = node_near
+        child_set = self._child[node.current]
+        if child_set is None:
+            return
+
+        for child in child_set:
+            node_child = self._tree.get(child)
+
+            if node_child is not None:
+                old_g = node_child.g
+                node_child.g = node.g + self.get_cost(
+                    node.current, node_child.current
+                )
+                if not math.isclose(node_child.g, old_g):
+                    self._propagate_cost_to_children(node_child)

python_motion_planning/traj_optimizer/__init__.py

@@ -0,0 +1,2 @@
+from .base_curve_generator import *
+from .curve_generator import *

python_motion_planning/traj_optimizer/base_curve_generator.py

@@ -0,0 +1,53 @@
+"""
+@file: base_curve_generator.py
+@author: Yang Haodong, Wu Maojia
+@update: 2026.4.12
+"""
+from typing import List, Tuple, Dict, Any
+from abc import ABC, abstractmethod
+import math
+
+
+class BaseCurveGenerator(ABC):
+    """
+    Base class for curve generator (trajectory smoother).
+
+    Args:
+        step: Step size for interpolation or discretization along the curve.
+    """
+    def __init__(self, step: float = 0.01) -> None:
+        super().__init__()
+        self.step = step
+
+    def __str__(self) -> str:
+        return "Base Curve Generator"
+
+    @abstractmethod
+    def generate(self, points: List[Tuple[float, ...]]) -> Tuple[List[Tuple[float, ...]], Dict[str, Any]]:
+        """
+        Interface for curve generation.
+
+        Args:
+            points: A list of waypoints in world frame. The exact format (2D position
+                or 2D pose with orientation) depends on the concrete generator.
+
+        Returns:
+            path: A list containing the smoothed path waypoints in world frame.
+            curve_info: A dictionary containing the curve information (success, length).
+        """
+        raise NotImplementedError
+
+    def length(self, path: List[Tuple[float, ...]]) -> float:
+        """
+        Calculate the length of a path.
+
+        Args:
+            path: A list containing the path waypoints.
+
+        Returns:
+            length: Length of the path.
+        """
+        dist = 0.0
+        for i in range(len(path) - 1):
+            dist += math.hypot(path[i + 1][0] - path[i][0], path[i + 1][1] - path[i][1])
+        return dist

python_motion_planning/traj_optimizer/curve_generator/__init__.py

@@ -0,0 +1,2 @@
+from .point_based import *
+from .pose_based import *

python_motion_planning/traj_optimizer/curve_generator/point_based/__init__.py

@@ -0,0 +1,2 @@
+from .bspline import *
+from .cubic_spline import *