RRT* Path Planning in Python

This repository contains a Python implementation of an enhanced RRT* (Rapidly-exploring Random Tree Star) algorithm for path planning in a 3D space. The program plans an optimal (or near-optimal) collision-free path from a start point to a goal point while avoiding obstacles.

Overview

The algorithm uses an adaptive sampling strategy with informed (ellipsoid) sampling once enough nodes have been generated. Collision checking is optimized via caching, and an adaptive neighbor radius is computed to improve the rewiring process. Finally, path shortcutting is applied to further optimize the final path.

Key Features

• Adaptive Sampling: Uses informed sampling via an ellipsoidal region when a promising path is found.
• Collision Checking with Caching: Avoids redundant checks by storing previous collision results.
• Adaptive Rewiring: Dynamically computes the neighbor radius based on the number of nodes.
• Path Shortcutting: Optimizes the final path by checking for direct connections between nonadjacent nodes.

Requirements

• Python 3.x
• NumPy
• SciPy

Install the dependencies with:

pip install numpy scipy

Code Structure

• Node Class:
  Represents a node in the search tree. Each node stores its position (as a NumPy array), a reference to its parent, its cumulative cost from the start, and a set of children (to aid in efficient rewiring).

• RRTStar Class:
  Implements the RRT* algorithm with the following key methods:

  • create_environment(): Sets up the 3D planning space with defined bounds, obstacles (walls and corridor-like structures), start, and goal positions.
  • sample_point(): Implements an enhanced sampling strategy that sometimes returns the goal or uses informed (ellipsoid) sampling once enough nodes are available.
  • is_collision_free(): Checks if a path between two points is free of obstacles using a caching mechanism to speed up repeated checks.
  • find_nearest_node() and find_neighbors(): Locate the nearest node and neighbors within an adaptive radius.
  • steer(): Moves from a given node toward a sampled point by a fixed step size.
  • path_planning(): The main loop that builds the tree, performs rewiring, and checks if a node is close enough to the goal. When a better path is found, it prints an update.
  • extract_path(): Backtracks from the goal node to the start to return the final path, then performs path shortcutting.

• Module-level Functions:

  • create_environment(): Instantiates the planner and sets up the environment.
  • path_planning(): Runs the planner’s RRT* algorithm and returns the planned path.

Usage

To run the planner, simply execute the Python script. For example, if your file is named rrt_star.py:

python rrt_star.py

The program will create the environment, run the RRT* algorithm for a preset number of iterations, and print updates to the console when a better path is found. The final path is returned as a NumPy array of 3D points.

Customization

You can adjust various parameters in the RRTStar class:

• step_size: The distance moved at each step.
• search_radius: The initial maximum radius to search for neighbors.
• max_iterations: Maximum number of iterations to run the algorithm.
• goal_sample_rate: The probability of directly sampling the goal.
• goal_threshold: Distance threshold to consider that the goal has been reached.
• adaptive_radius and informed_sampling: Enable adaptive techniques for improved performance.

License

This project is licensed under the MIT License.