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

python_motion_planning/path_planner/sample_search/rrt_star.py

@@ -1,29 +1,38 @@
 """
 @file: rrt_star.py
-@author: Wu Maojia, Yang Haodong
-@update: 2025.10.3
+@author: Wu Maojia
+@update: 2025.12.19
 """
 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_until_sample_num: Stop until sample number limitation is reached, otherwise stop when goal is found.
+        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 +41,219 @@ 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), sample_num=1000, stop_until_sample_num=True)
         >>> 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_until_sample_num: int = False,
+                 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_until_sample_num = stop_until_sample_num
+        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
 
-        # Main sampling loop
-        for _ in range(self.sample_num):
-            # Generate random sample 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)
+
+        for i in range(self.sample_num):
+            # 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)
+                        self.best_results = path, {
+                            "success": True,
+                            "start": self.start,
+                            "goal": self.goal,
+                            "length": length,
+                            "cost": cost,
+                            "expand": self._tree,
+                        }
+
+                        if not self.stop_until_sample_num:
+                            return self.best_results
+
+        n = len(self._tree) + 1
+        radius = self.gamma * ((math.log(n) / n) ** (1 / self.dim))
+
+        # Planning stopped
+        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-2.0.dev1.dist-info → python_motion_planning-2.0.dev2.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: python-motion-planning
-Version: 2.0.dev1
+Version: 2.0.dev2
 Summary: Motion planning algorithms for Python
 Maintainer-email: Wu Maojia <omige@mail.nwpu.edu.cn>, Yang Haodong <913982779@qq.com>
 License: GNU GENERAL PUBLIC LICENSE
@@ -689,6 +689,9 @@ Requires-Dist: scipy
 Requires-Dist: matplotlib
 Requires-Dist: osqp
 Requires-Dist: gymnasium
+Requires-Dist: faiss-cpu
+Requires-Dist: pyvista
+Requires-Dist: pyvistaqt
 Dynamic: license-file
 
 
@@ -701,11 +704,13 @@ Dynamic: license-file
 * `Path Planning`: It's based on path constraints (such as obstacles), planning the optimal path sequence for the robot to travel without conflict between the start and goal.
 * `Trajectory planning`: It plans the motion state to approach the global path based on kinematics, dynamics constraints and path sequence.
 
+This repository provides the implementations of common `Motion planning` algorithms, including path planners on N-D grid and controllers for path-tracking, a visualizer based on matplotlib and a toy physical simulator to test controllers.
+
 The theory analysis can be found at [motion-planning](https://blog.csdn.net/frigidwinter/category_11410243.html).
 
 We also provide [ROS C++](https://github.com/ai-winter/ros_motion_planning) version and [Matlab](https://github.com/ai-winter/matlab_motion_planning) version.
 
-This repository provides the implementations of common `Motion planning` algorithms. **Your stars and forks are welcome**. Submitting pull requests or joining our development team are also welcome. For trivial modification, please directly contribute to `dev` branch. For big modification, please [contact](#contact) us before you contribute.
+**Your stars and forks are welcome!**
 
 # Quick Start
 
@@ -725,8 +730,9 @@ python_motion_planning
 |   └─path_tracker
 ├─path_planner
 |   ├─graph_search
-|   └─sample_search
-└─curve_generation
+|   ├─sample_search
+|   └─hybrid_search
+└─curve_generator
 ```
 
 ## Install
@@ -740,7 +746,7 @@ conda activate pmp
 To install the repository, please run the following command in shell.
 
 ```shell
-pip install python-motion-planning==2.0.dev1
+pip install python-motion-planning==2.0.dev2
 ```
 
 ## Run
@@ -749,29 +755,40 @@ Please refer to the Tutorials part of [online documentation](https://ai-winter.g
 
 # Demos
 ## Path Planner
-
+### Graph Search
 |Planner|2D Grid|3D Grid
 |-------|-------|-------
-**GBFS**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**Dijkstra**|![dijkstra_2d.svg](assets/dijkstra_2d.svg)|![dijkstra_3d.svg](assets/dijkstra_3d.svg)
-**A\***|![a_star_2d.svg](assets/a_star_2d.svg)|![a_star_3d.svg](assets/a_star_3d.svg)
-**JPS**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
+**Dijkstra**|![dijkstra_2d.svg](assets/dijkstra_2d.svg)|![dijkstra_3d.png](assets/dijkstra_3d.png)
+**GBFS**|![gbfs_2d.svg](assets/gbfs_2d.svg)|![gbfs_3d.png](assets/gbfs_3d.png)
+**A\***|![a_star_2d.svg](assets/a_star_2d.svg)|![a_star_3d.png](assets/a_star_3d.png)
+**JPS**|![jps_2d.svg](assets/jps_2d.svg)|![jps_3d.png](assets/jps_3d.png)
+**Theta\***|![theta_star_2d.svg](assets/theta_star_2d.svg)|![theta_star_3d.png](assets/theta_star_3d.png)
+**Lazy Theta\***|![lazy_theta_star_2d.svg](assets/lazy_theta_star_2d.svg)|![lazy_theta_star_3d.png](assets/lazy_theta_star_3d.png)
 **D\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 **LPA\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 **D\* Lite**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**Theta\***|![theta_star_2d.svg](assets/theta_star_2d.svg)|![theta_star_3d.svg](assets/theta_star_3d.svg)
-**Lazy Theta\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**S-Theta\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 **Anya**|Not implemented|Not implemented
-**Voronoi**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**RRT**|![rrt_2d.svg](assets/rrt_2d.svg)|![rrt_3d.svg](assets/rrt_3d.svg)
-**RRT\***|![rrt_star_2d.svg](assets/rrt_star_2d.svg)|![rrt_star_3d.svg](assets/rrt_star_3d.svg)
+
+### Sample Search
+|Planner|2D Grid|3D Grid
+|-------|-------|-------
+**RRT**|![rrt_2d.svg](assets/rrt_2d.svg)|![rrt_3d.png](assets/rrt_3d.png)
+**RRT\***|![rrt_star_2d.svg](assets/rrt_star_2d.svg)|![rrt_star_3d.png](assets/rrt_star_3d.png)
+**RRT-Connect**|![rrt_connect_2d.svg](assets/rrt_connect_2d.svg)|![rrt_connect_3d.png](assets/rrt_connect_3d.png)
 **Informed RRT**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**RRT-Connect**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
+**PRM**|Not implemented|Not implemented
+
+### Evolutionary Search
+|Planner|2D Grid|3D Grid
+|-------|-------|-------
 | **ACO** |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **GA**  |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **PSO**  |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 
+### Hybrid Search
+|Planner|2D Grid|3D Grid
+|-------|-------|-------
+**Voronoi Planner**|![voronoi_planner_2d.svg](assets/voronoi_planner_2d.svg)|![voronoi_planner_3d.png](assets/voronoi_planner_3d.png)
 
 ## Controller
 
@@ -779,7 +796,7 @@ We provide a toy simulator with simple physical simulation to test controllers (
 
 In the following demos, the blue robot 1 is the `CircularRobot`, and the orange robot 2 is the `DiffDriveRobot`.
 
-|Planner|2D|3D
+|Controller|2D|3D
 |-------|-------|-------
 |**Path Trakcer**|![path_tracker_2d.gif](assets/path_tracker_2d.gif)|Not implemented
 | **Pure Pursuit**     |![pure_pursuit_2d.gif](assets/pure_pursuit_2d.gif)|Not implemented
@@ -799,7 +816,7 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 
 The visualization of the curve generators has not been implemented in current version. They can be visualized in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1).
 
-| Planner |2D|3D|
+|Generator|2D|3D|
 | ------- | -------------------------------------------------------- | -------------------------------------------------------- 
 | **Polynomia** | ![polynomial_curve_python.gif](assets/polynomial_curve_python.gif)|Not implemented
 | **Bezier** |![bezier_curve_python.png](assets/bezier_curve_python.png)|Not implemented
@@ -809,6 +826,24 @@ The visualization of the curve generators has not been implemented in current ve
 | **Reeds-Shepp** |![reeds_shepp_python.png](assets/reeds_shepp_python.gif)|Not implemented
 | **Fem-Pos Smoother** |![fem_pos_smoother_python.png](assets/fem_pos_smoother_python.png)|Not implemented
 
+# Future Works
+
+* N-D controllers (path-trackers).
+
+* Path planning for robotic arms.
+
+* Path planning on topological map.
+
+* Application on ROS2.
+
+* Application in mainstream robot simulation environments (e.g. Gazebo, Carla, Airsim, PyBullet, MuJoCo, Issac Sim).
+
+* More mainstream motion planning algorithms.
+
+* Performance optimization.
+
+Contributors are welcome! For trivial modification, please directly contribute to `dev` branch. For big modification, please [contact](#contact) us before you contribute.
+
 # Contact
 
 Long-term maintainers: