diff --git a/README.md b/README.md index c36c80e..c36df78 100644 --- a/README.md +++ b/README.md @@ -3,22 +3,6 @@ A Python 3D path planning library with visualization using Viser. Implements pathplanning algorithms with a unified architecture for easy extension and consistent visualization. - -## Features - -- πŸš€ **Unified Architecture**: All planners extend `RRTBase` or `RRGBase` for consistency -- 🎨 **Simple Visualization**: One API works for all planners - just pass the planner object -- 🌳 **Multiple Algorithms**: RRT (single-tree), RRT-Connect (bidirectional), RRG (graph-based), RRT* (optimal), Informed RRT* (heuristic-guided optimal), PRM (multi-query), and PRM* (optimal multi-query) -- πŸ“Š **Detailed Analytics**: Track successful paths and failed collision attempts -- πŸ“ **N-Dimensional**: Works with any dimensional state space (2D, 3D, 4D+) -- 🎯 **Obstacle Avoidance**: Integrated collision detection with boxes and spheres -- ⚑ **Asymptotic Optimality**: RRT*, Informed RRT*, RRG, and PRM* converge to optimal solutions -- 🎯 **Informed Sampling**: Informed RRT* uses ellipsoidal heuristic for faster convergence -- πŸ—ΊοΈ **Multi-Query Planning**: PRM and PRM* build reusable roadmaps for efficient path queries -- 🎬 **Diffusion-based Planning**: Guided reverse-diffusion sampler with value-based trajectory guidance -- πŸ”„ **Trajectory One-shot Inference**: Generate full collision-aware trajectories from start/goal constraints in one call -- 🧭 **Metric-aware Sampling**: RRT-Connect, RRG, RRT*, and PRM* can share Euclidean or terrain Riemannian planning spaces - ## Requirements - Python >= 3.10 @@ -61,7 +45,15 @@ Faster convergence using dual-tree bidirectional search. **Paper**: [Kuffner, J. J., & LaValle, S. M. (2000). "RRT-Connect: An efficient approach to single-query path planning"](https://www.cs.cmu.edu/afs/cs/academic/class/15494-s14/readings/kuffner_icra2000.pdf) -RRT-Connect Example + +**Example comparison (seed 42, 30 mixed obstacles):** + +| RRT | RRT-Connect | +| --- | --- | +| RRT Example | RRT-Connect Example | +| Single tree grows from the start.
First goal at iteration : 403
Path length : 27.04
Planning time : 0.527 seconds
Waypoints : 56
Explored nodes : 319 | Two trees grow from start and goal, then connect.
Trees connected at iteration : 14
Path length : 25.39
Planning time : 0.020 seconds
Waypoints : 52
Total nodes : 77 | + +RRT-Connect improves the time to first solution by growing two trees and repeatedly trying to connect them. It does not optimize the path like RRT*, but it can find a feasible connection much faster than single-tree RRT. **Features:** - Bidirectional search (start tree + goal tree) @@ -81,7 +73,15 @@ An optimal sampling-based path planner that creates a graph structure to find in **Paper**: [Karaman, S., & Frazzoli, E. (2011). "Sampling-based algorithms for optimal motion planning"](https://arxiv.org/pdf/1105.1186) -RRG Example + +**Example comparison (seed 42, 15 mixed obstacles):** + +| RRT | RRG | +| --- | --- | +| RRT Example | RRG Example | +| Single tree grows from the start.
First goal at iteration : 264
Path length : 27.21
Waypoints : 56
Explored nodes : 250 | Graph grows from the start.
Goal reached at iteration : 264
Path length : 24.19
Waypoints : 34
Graph nodes : 249
Graph edges : 725 | + +RRG improves path-search structure rather than time to first solution. It keeps more collision-free connections than RRT, which gives the planner a graph to search for better routes. The tradeoff is more edge checks, more memory, and usually higher planning cost. **Features:** - Builds a graph to connect samples to multiple neighbors, enabling path optimization @@ -101,7 +101,14 @@ An asymptotically optimal variant of RRT that rewires the tree to find shorter p **Paper**: [Karaman, S., & Frazzoli, E. (2011). "Sampling-based algorithms for optimal motion planning"](https://arxiv.org/pdf/1105.1186) -RRT* Example +**Example comparison (seed 42):** + +| RRT | RRT* | +| --- | --- | +| RRT Example | RRT* Example | +| Stops at the first feasible path.
First goal at iteration : 264
Path length : 27.21
Waypoints : 56
Explored nodes : 250 | Finds the first path, then continues optimizing.
First goal at iteration : 264
Optimized iterations : 2000
Path length : 23.12
Waypoints : 16
Graph nodes : 1897 | + +RRT* uses rewiring and a larger connection radius (`radius_gain=5.0` in this example) to replace long local branches with lower-cost connections. The result is slower than RRT, but the final path is shorter and has fewer waypoints. **Features:** - Asymptotically optimal path planning (converges to optimal solution) @@ -153,7 +160,14 @@ An asymptotically optimal variant of PRM that uses dynamic connection radius to **Paper**: [Karaman, S., & Frazzoli, E. (2011). "Sampling-based algorithms for optimal motion planning"](https://arxiv.org/pdf/1105.1186) -PRM* Example +**Example comparison (seed 42, 15 mixed obstacles):** + +| PRM | PRM* | +| --- | --- | +| PRM Example | PRM* Example | +| Fixed connection radius : 2.0
Sample number : 300
Path length : 28.64
Waypoints : 20
Roadmap nodes : 282
Roadmap edges : 1178 | Dynamic radius gain : 10.0
Sample number : 300
Path length : 24.48
Waypoints : 11
Roadmap nodes : 282
Roadmap edges : 2594 | + +PRM uses a fixed connection radius, while PRM* uses a radius that changes with the roadmap size. With a larger radius gain, PRM* finds a shorter path in this example, but it pays for that improvement with more roadmap edges and collision checks. **Features:** - **Asymptotic optimality**: Converges to the optimal path as samples increase @@ -183,20 +197,23 @@ uv run python examples/prm_star_example.py ### 7. Informed RRT* - Comparison with RRT* -Comparison of standard RRT* and its informed variant that uses heuristic sampling for faster convergence. +Comparison of standard RRT* and its informed variant using the same seed, start/goal, obstacle map, max iterations, step size, and radius gain. | **RRT* (RRT-Star)** | **Informed RRT*** | |---------------------|-------------------| -| RRT* Example | Informed RRT* Example | +| RRT* Example | Informed RRT* Example | +| Standard RRT* sampling with rewiring.
First goal at iteration : 213
Optimized iterations : 3000
Path length : 11.01
Planning time : 32 seconds
Waypoints : 40
Graph nodes : 2920
Graph edges : 2919
Goal candidates : 154 | Uses RRT* until a first solution exists, then samples inside the informed ellipsoid.
Optimized iterations : 3000
Path length : 11.01
Planning time : 29 seconds
Waypoints : 40
Graph nodes : 2912
Graph edges : 2911
Goal candidates : 18 | + +In this run, Informed RRT* does not find a shorter final path than RRT*. Both planners converge to the same path length and waypoint count, while Informed RRT* finishes slightly faster because its post-solution sampling focuses on states that can still improve the current best path. #### What is Informed RRT*? -An improved version of RRT* that uses informed sampling within an ellipsoidal subset of the state space, leading to faster convergence to optimal solutions. +An improved version of RRT* that uses informed sampling within an ellipsoidal subset of the state space after a first solution is found. It can improve convergence speed in optimization-focused runs, but it does not guarantee a better path in every finite run. **Paper**: [Gammell, J. D., et al. (2014). "Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic"](https://arxiv.org/pdf/1404.2334) **Features:** -- **Faster convergence**: Focuses sampling on regions that can improve the solution +- **Focused convergence**: Focuses sampling on regions that can improve the solution after a first path exists - **Ellipsoidal sampling**: After finding initial solution, samples only within prolate hyperspheroid defined by start, goal, and current best cost - **Asymptotic optimality**: Maintains optimality guarantee of RRT* - **Informed search**: Uses geometric heuristic to reject samples that cannot improve the path @@ -214,7 +231,7 @@ An improved version of RRT* that uses informed sampling within an ellipsoidal su | Aspect | RRT* | Informed RRT* | |--------|------|---------------| | **Sampling Strategy** | Uniform sampling over entire space throughout planning | Uniform initially, then focused ellipsoidal sampling after first solution | -| **Convergence Speed** | Slower - explores entire space | Faster - focuses on promising regions | +| **Convergence Speed** | Explores the full space throughout planning | Can be faster after the first solution because sampling is focused | | **Optimality** | Asymptotically optimal | Asymptotically optimal (same guarantee) | | **When to Use** | Unknown environments, first solution priority | Known start/goal, optimization priority | diff --git a/docs/images/informed_rrt_star_example.png b/docs/images/informed_rrt_star_example.png index 3dc476d..b433de5 100644 Binary files a/docs/images/informed_rrt_star_example.png and b/docs/images/informed_rrt_star_example.png differ diff --git a/docs/images/prm_example.png b/docs/images/prm_example.png index 663ee77..5646689 100644 Binary files a/docs/images/prm_example.png and b/docs/images/prm_example.png differ diff --git a/docs/images/prm_star_example.png b/docs/images/prm_star_example.png index 13a31bd..397c8af 100644 Binary files a/docs/images/prm_star_example.png and b/docs/images/prm_star_example.png differ diff --git a/docs/images/rrg_example.png b/docs/images/rrg_example.png index baf861a..7f43f2a 100644 Binary files a/docs/images/rrg_example.png and b/docs/images/rrg_example.png differ diff --git a/docs/images/rrt_connect_example.png b/docs/images/rrt_connect_example.png index 0b47385..ec03d95 100644 Binary files a/docs/images/rrt_connect_example.png and b/docs/images/rrt_connect_example.png differ diff --git a/docs/images/rrt_example.png b/docs/images/rrt_example.png index 20796d0..9acc329 100644 Binary files a/docs/images/rrt_example.png and b/docs/images/rrt_example.png differ diff --git a/docs/images/rrt_example2.png b/docs/images/rrt_example2.png new file mode 100644 index 0000000..510d019 Binary files /dev/null and b/docs/images/rrt_example2.png differ diff --git a/docs/images/rrt_star_example.png b/docs/images/rrt_star_example.png index f63774e..36c5730 100644 Binary files a/docs/images/rrt_star_example.png and b/docs/images/rrt_star_example.png differ diff --git a/docs/images/rrt_star_opt_example.png b/docs/images/rrt_star_opt_example.png index ff7b3f0..155def2 100644 Binary files a/docs/images/rrt_star_opt_example.png and b/docs/images/rrt_star_opt_example.png differ diff --git a/examples/informed_rrt_star_example.py b/examples/informed_rrt_star_example.py index f945302..f3864cb 100644 --- a/examples/informed_rrt_star_example.py +++ b/examples/informed_rrt_star_example.py @@ -69,10 +69,10 @@ def main(seed: int = 42, save_image: bool = False) -> None: sampler=GoalBiasedSampler, seed=seed, step_size=0.2, - goal_tolerance=0.2, - max_iterations=5000, + max_iterations=3000, + radius_gain=1.0, goal_bias=0.05, - radius_gain=0.4, + return_first_solution=False, ), ) diff --git a/examples/prm_example.py b/examples/prm_example.py index e0fec28..770ce29 100644 --- a/examples/prm_example.py +++ b/examples/prm_example.py @@ -119,6 +119,12 @@ def main(seed: int = 42, save_image: bool = False) -> None: # Visualize the roadmap even if no path is found visualizer.visualize_graph(prm) + # Statistics + stats = prm.get_stats() + print("\nStatistics:") + for key, value in stats.items(): + print(f" {key}: {value}") + # Save image if requested if save_image: diff --git a/examples/prm_star_example.py b/examples/prm_star_example.py index d9d52d3..343dc07 100644 --- a/examples/prm_star_example.py +++ b/examples/prm_star_example.py @@ -70,7 +70,7 @@ def main(seed: int = 42, save_image: bool = False) -> None: seed=seed, step_size=0.1, sample_number=300, - radius_gain=5.0, + radius_gain=10.0, max_retries=5, goal_tolerance=0.5, ), @@ -125,6 +125,12 @@ def main(seed: int = 42, save_image: bool = False) -> None: # Visualize the roadmap even if no path is found visualizer.visualize_graph(prm_star) + # Statistics + stats = prm_star.get_stats() + print("\nStatistics:") + for key, value in stats.items(): + print(f" {key}: {value}") + # Save image if requested if save_image: diff --git a/examples/rrg_example.py b/examples/rrg_example.py index f2ada57..3a601ad 100644 --- a/examples/rrg_example.py +++ b/examples/rrg_example.py @@ -7,7 +7,6 @@ from planning.collision import ObstacleCollisionChecker from planning.map import Map -from planning.sampling import GoalBiasedSampler # , UniformSampler from planning.sampling.rrg import RRG, RRGConfig from planning.visualization import save_docs_image, setup_camera_top_view from planning.visualization.rrg_visualizer import RRGVisualizer @@ -66,11 +65,10 @@ def main(seed: int = 42, save_image: bool = False) -> None: bounds=map_env.get_bounds(), collision_checker=collision_checker, config=RRGConfig( - sampler=GoalBiasedSampler, seed=seed, step_size=0.5, max_iterations=5000, - radius_gain=0.8, + radius_gain=2.5, ), ) @@ -87,7 +85,7 @@ def main(seed: int = 42, save_image: bool = False) -> None: if path is not None: print(f"\n Path found with {len(path)} waypoints!") - # print(f"Path length: {rrg.get_path_length():.2f}") + print(f"Path length: {rrg.get_path_length():.2f}") print(f"Total nodes in graph: {len(rrg.graph.nodes)}") print(f"Total edges in graph: {len(rrg.graph.edges)}\n") @@ -114,6 +112,12 @@ def main(seed: int = 42, save_image: bool = False) -> None: # Visualize the graph even if no path is found visualizer.visualize_graph(rrg) + # Statistics + stats = rrg.get_stats() + print("\nStatistics:") + for key, value in stats.items(): + print(f" {key}: {value}") + # Save image if requested if save_image: diff --git a/examples/rrt_connect_example.py b/examples/rrt_connect_example.py index 25da229..38b7997 100644 --- a/examples/rrt_connect_example.py +++ b/examples/rrt_connect_example.py @@ -1,6 +1,7 @@ """RRT-Connect algorithm example with obstacles.""" import argparse +import time import numpy as np import viser @@ -36,13 +37,13 @@ def main(seed: int = 42, save_image: bool = False) -> None: print(" Generating random obstacles...") obstacles = map_env.generate_obstacles( server=server, - num_obstacles=40, + num_obstacles=30, min_size=0.5, max_size=2.5, seed=seed, color=(200, 100, 50), check_overlap=True, - obstacle_type="box", + obstacle_type="mixed", ) print(f" Generated {len(obstacles)} obstacles\n") @@ -61,14 +62,16 @@ def main(seed: int = 42, save_image: bool = False) -> None: bounds=map_env.get_bounds(), collision_checker=ObstacleCollisionChecker(map_env.obstacles), config=RRTConnectConfig( - max_iterations=5000, + max_iterations=2000, seed=seed, ), ) # Plan path print(" Planning path with RRT-Connect...\n") + planning_start_time = time.perf_counter() path = rrt_connect.plan() + planning_time = time.perf_counter() - planning_start_time # Create visualizer visualizer = RRTConnectVisualizer(server) @@ -77,6 +80,7 @@ def main(seed: int = 42, save_image: bool = False) -> None: if path is not None: print(f"\n Path found with {len(path)} waypoints!") print(f"Path length: {rrt_connect.get_path_length():.2f}") + print(f"Planning time: {planning_time:.3f} seconds") print(f"Total nodes explored: {len(rrt_connect.get_all_nodes())}\n") # Use visualize_branches for consistency with RRT visualizer @@ -96,6 +100,7 @@ def main(seed: int = 42, save_image: bool = False) -> None: else: print("\n No path found!") + print(f"Planning time: {planning_time:.3f} seconds") print("Try increasing max_iterations or decreasing obstacle count.") # Statistics @@ -119,8 +124,6 @@ def handle_save(client: viser.ClientHandle) -> None: print("\nPress Ctrl+C to exit.") while True: try: - import time - time.sleep(0.1) except KeyboardInterrupt: print("\nShutting down server.") diff --git a/examples/rrt_example.py b/examples/rrt_example.py index ab1c31b..7b29fb8 100644 --- a/examples/rrt_example.py +++ b/examples/rrt_example.py @@ -1,6 +1,7 @@ """RRT algorithm example with mixed obstacle types.""" import argparse +import time import numpy as np import viser @@ -79,11 +80,14 @@ def main(seed: int = 42, save_image: bool = False) -> None: print(f" Goal tolerance: {rrt.goal_tolerance}\n") # Run planner + planning_start_time = time.perf_counter() path = rrt.plan() + planning_time = time.perf_counter() - planning_start_time if path is not None: print(f"\n Path found with {len(path)} waypoints!") print(f"Path length: {rrt.get_path_length():.2f}") + print(f"Planning time: {planning_time:.3f} seconds") print(f"Total nodes explored: {len(rrt.get_all_nodes())}\n") # Visualize all paths (success: blue, failure: red) @@ -103,6 +107,7 @@ def main(seed: int = 42, save_image: bool = False) -> None: else: print("\nNo path found!") + print(f"Planning time: {planning_time:.3f} seconds") print("Try increasing max_iterations or decreasing obstacle count.") # Statistics @@ -126,8 +131,6 @@ def handle_save(client: viser.ClientHandle) -> None: print("\nPress Ctrl+C to exit.") while True: try: - import time - time.sleep(0.1) except KeyboardInterrupt: print("\nShutting down server.") diff --git a/examples/rrt_star_example.py b/examples/rrt_star_example.py index 908d63e..e003163 100644 --- a/examples/rrt_star_example.py +++ b/examples/rrt_star_example.py @@ -72,9 +72,10 @@ def main(seed: int = 42, save_image: bool = False) -> None: sampler=GoalBiasedSampler, seed=seed, step_size=0.5, - max_iterations=5000, - radius_gain=1.0, + max_iterations=2000, + radius_gain=5.0, goal_bias=0.05, + return_first_solution=False, ), )