Introduction to RRT-Connect

RRT-Connect is a path-planning algorithm designed to efficiently find a path for a robot or point from a start configuration to a goal configuration in a space containing obstacles.

The core idea of a standard Rapidly-exploring Random Tree (RRT) is to grow a tree of possible paths from the start point. It does this by picking random points in the space and extending the closest branch of the tree towards that random point.

RRT-Connect improves upon this by growing two trees simultaneously: one from the start point (Tree_A) and one from the goal point (Tree_B). After growing one tree by a small step, it then tries to "connect" the other tree to the new node it just created. This bi-directional approach allows the two trees to find each other much faster than a single tree would find the goal.

Core Data Structures

Before writing the algorithm, you need to define how you'll store your data.

The Node

A tree is made of nodes. Each node in your tree should store at least two pieces of information:

• position: The coordinates of the point in the space (e.g., (x, y) for 2D or (x, y, z) for 3D).
• parent: A reference or pointer to its parent Node. The very first node (the root) will have None as its parent. This is crucial for reconstructing the final path.

// Pseudocode for a Node class
class Node {
  constructor(position) {
    this.position = position; // e.g., [x, y]
    this.parent = null;       // Reference to the parent Node object
  }
}

The Tree

You can represent each of your two trees as a simple list or array of Node objects.

• tree_a = [start_node]
• tree_b = [goal_node]

The Main Algorithm

Here is the high-level logic of the RRT-Connect algorithm.

Initialization

1. Define your start_position and goal_position.
2. Create two trees. tree_a contains only the start_node, and tree_b contains only the goal_node.
3. Set a maximum number of iterations (e.g., 5000) to prevent an infinite loop if no path exists.

The Main Loop

The algorithm proceeds in a loop. In each iteration, you attempt to extend one tree and connect the other. For clarity, we'll always extend tree_a first and then swap the trees.

// Pseudocode for the main loop
for i in 0 to max_iterations:
    // 1. Extend tree_a towards a random point
    q_rand = generate_random_point()
    q_new_node = extend(tree_a, q_rand) // This function returns the new node if successful

    // 2. If the extension was successful, try to connect tree_b
    if q_new_node is not null:
        connection_result = connect(tree_b, q_new_node.position)

        // 3. Check for success
        if connection_result is "Reached":
            print("Path Found!")
            path = reconstruct_path(tree_a, tree_b, q_new_node, last_node_from_connect)
            return path // Exit the loop and return the path

    // 4. Swap the trees for the next iteration
    swap(tree_a, tree_b)

print("Path not found within max iterations.")

Core Functions (extend and connect)

These two functions are the heart of the algorithm. They are more complex than a single step, so let's break them down.

The extend Function

This function takes a tree and a random point (q_rand) and tries to grow the tree towards that point.

extend(tree, q_rand):

1. Find Nearest Node: Find the node in the tree that is closest to q_rand. Let's call it q_near. You'll need a find_nearest_node helper for this.
2. Steer: From q_near.position, take a step of a fixed size (epsilon or step_size) in the direction of q_rand. This gives you a new point, q_new. You'll need a steer helper function.
3. Collision Check: Check if the straight-line path from q_near.position to q_new collides with any obstacles. You'll need an is_collision_free helper.
4. Add to Tree: If the path is collision-free:
   • Create a new Node object for q_new.
   • Set its parent to q_near.
   • Add this new node to the tree.
   • Return the new node.
5. If there was a collision, do nothing and return null.

The connect Function

This function is more "greedy" than extend. It takes a tree and a target point (q_target, which is the q_new.position from the extend step) and tries to connect the tree to it, potentially adding multiple nodes in one go.

connect(tree, q_target):

1. Loop: Start a loop that will continue as long as you are making progress.
2. Find Nearest & Steer: Find the nearest node in the tree to q_target (q_near) and steer from it to get q_new.
3. Collision Check: Check for collisions between q_near and q_new.
4. Add to Tree: If collision-free, add the new node to the tree with q_near as its parent.
5. Check Status:
   • If q_new is exactly at q_target, you have successfully connected the trees. Return "Reached".
   • If you made progress (added a node), continue the loop. The new q_near will be the node you just added.
   • If there was a collision, you can't get any closer from this branch. Stop the loop and return "Trapped".
6. If the loop finishes without reaching the target, return "Advanced".

Helper Functions & Path Reconstruction

You'll need to implement these smaller, single-purpose functions.

• generate_random_point(): Returns a random [x, y] coordinate within your map boundaries. Occasionally (e.g., 5% of the time), this function should return the goal_position instead of a random point. This "biasing" helps the tree grow towards the goal more purposefully.
• find_nearest_node(tree, point): Iterates through all nodes in the tree and returns the one with the minimum Euclidean distance to the given point.
• steer(from_pos, to_pos, step_size):
  1. Calculate the vector from from_pos to to_pos.
  2. Calculate the distance between them.
  3. If the distance is less than step_size, just return to_pos.
  4. Otherwise, normalize the vector (make its length 1) and multiply it by step_size. Add this new vector to from_pos to get the result.
• is_collision_free(pos1, pos2): This is highly dependent on your environment. For simple polygon obstacles, you need to check if the line segment from pos1 to pos2 intersects with any of the polygon edges.
• reconstruct_path(tree_a, tree_b, connection_node_a, connection_node_b):
  1. Create an empty path.
  2. Start from connection_node_a and trace back using the .parent references, adding each node's position to the path until you reach the root of tree_a.
  3. Reverse this part of the path.
  4. Start from connection_node_b and trace back through tree_b's parents, adding each position to the path.
  5. Combine the two parts to get the full path from start to goal.
     This detailed breakdown should provide a solid foundation for your implementation. The most challenging part is often the collision detection, so focus on getting that right for your specific environment.

Reference

// Copyright (c) 2025 Junior Sundar
//
// SPDX-License-Identifier: BSD-3-Clause

use std::{
    mem,
    sync::Arc,
    time::{Duration, Instant},
};

use rand::Rng;

use crate::base::{
    error::PlanningError,
    goal::{Goal, GoalSampleableRegion},
    planner::{Path, Planner},
    problem_definition::ProblemDefinition,
    space::StateSpace,
    state::State,
    validity::StateValidityChecker,
};

#[derive(Clone)]
struct Node<S: State> {
    state: S,
    parent_index: Option<usize>,
}

/// The result of an attempt to extend a tree.
#[derive(PartialEq, Debug)]
enum ExtendResult {
    /// The tree was extended, but did not reach the target state.
    Advanced,
    /// The tree was extended and reached the target state exactly.
    Reached,
    /// The tree could not be extended because the motion was invalid.
    Trapped,
}

pub struct RRTConnect<S: State, SP: StateSpace<StateType = S>, G: Goal<S>> {
    pub max_distance: f64,

    problem_def: Option<Arc<ProblemDefinition<S, SP, G>>>,
    validity_checker: Option<Arc<dyn StateValidityChecker<S> + Send + Sync>>,
    start_tree: Vec<Node<S>>,
    goal_tree: Vec<Node<S>>,
}

impl<S, SP, G> RRTConnect<S, SP, G>
where
    S: State + Clone,
    SP: StateSpace<StateType = S>,
    G: Goal<S>,
{
    pub fn new(max_distance: f64) -> Self {
        RRTConnect {
            max_distance,
            problem_def: None,
            validity_checker: None,
            start_tree: Vec::new(),
            goal_tree: Vec::new(),
        }
    }

    /// Extends a tree towards a target state `q_target` and adds a new node if valid.
    fn extend(
        &self,
        tree: &mut Vec<Node<S>>,
        q_target: &S,
    ) -> (ExtendResult, usize) { // Returns the result and the index of the new node
        let pd = self.problem_def.as_ref().unwrap();

        // Find nearest node in the tree
        let mut nearest_node_index = 0;
        let mut min_dist = pd.space.distance(&tree[0].state, q_target);
        for (i, node) in tree.iter().enumerate().skip(1) {
            let dist = pd.space.distance(&node.state, q_target);
            if dist < min_dist {
                min_dist = dist;
                nearest_node_index = i;
            }
        }
        
        let q_near = tree[nearest_node_index].state.clone();
        let mut q_new = q_near.clone();
        let mut result = ExtendResult::Reached;

        // Steer towards q_target
        if min_dist > self.max_distance {
            let t = self.max_distance / min_dist;
            pd.space.interpolate(&q_near, q_target, t, &mut q_new);
            result = ExtendResult::Advanced;
        } else {
            q_new = q_target.clone();
            result = ExtendResult::Reached;
        }

        // Add the new node to the tree if the motion is valid
        if self.check_motion(&q_near, &q_new) {
            let new_node_idx = tree.len();
            tree.push(Node {
                state: q_new,
                parent_index: Some(nearest_node_index),
            });
            (result, new_node_idx)
        } else {
            Trapped, nearest_node_index
        }
    }

    /// An internal helper to check motion validity using the stored checker.
    fn check_motion(&self, from: &S, to: &S) -> bool {
        if let (Some(pd), Some(vc)) = (&self.problem_def, &self.validity_checker) {
            let space = &pd.space;
            let dist = space.distance(from, to);
            let num_steps = (dist / (self.max_distance * 0.1)).ceil() as usize;

            if num_steps <= 1 { return vc.is_valid(to); }

            let mut interpolated_state = from.clone();
            for i in 1..=num_steps {
                let t = i as f64 / num_steps as f64;
                space.interpolate(from, to, t, &mut interpolated_state);
                if !vc.is_valid(&interpolated_state) {
                    return false;
                }
            }
            true
        } else { false }
    }

    /// Reconstructs the final path when the two trees are connected.
    fn reconstruct_path(&self, start_tree_last_idx: usize, goal_tree_last_idx: usize) -> Path<S> {
        let mut path_states = Vec::new();
        
        // Trace back the path from the connection point in the start tree.
        let mut start_segment = Vec::new();
        let mut current_index = Some(start_tree_last_idx);
        while let Some(index) = current_index {
            start_segment.push(self.start_tree[index].state.clone());
            current_index = self.start_tree[index].parent_index;
        }
        start_segment.reverse(); // Path is now from start to connection point
        path_states.append(&mut start_segment);

        // Trace back the path from the connection point in the goal tree.
        let mut goal_segment = Vec::new();
        let mut current_index = Some(goal_tree_last_idx);
        while let Some(index) = current_index {
            goal_segment.push(self.goal_tree[index].state.clone());
            current_index = self.goal_tree[index].parent_index;
        }
        // Append the goal segment (reversed, and skipping the connection point which is already there).
        path_states.extend(goal_segment.into_iter().rev().skip(1));
        
        Path(path_states)
    }

    /// Reconstructs a path from a single tree that has reached the goal.
    fn reconstruct_single_tree_path(&self, tree: &[Node<S>], last_node_idx: usize) -> Path<S> {
        let mut path_states = Vec::new();
        let mut current_index = Some(last_node_idx);
        while let Some(index) = current_index {
            path_states.push(tree[index].state.clone());
            current_index = tree[index].parent_index;
        }
        path_states.reverse();
        Path(path_states)
    }
}

impl<S, SP, G> Planner<S, SP, G> for RRTConnect<S, SP, G>
where
    S: State + Clone,
    SP: StateSpace<StateType = S>,
    G: Goal<S> + GoalSampleableRegion<S>,
{
    fn setup(
        &mut self,
        problem_def: Arc<ProblemDefinition<S, SP, G>>,
        validity_checker: Arc<dyn StateValidityChecker<S> + Send + Sync>,
    ) {
        self.problem_def = Some(problem_def);
        self.validity_checker = Some(validity_checker);
        self.start_tree.clear();
        self.goal_tree.clear();

        let pd = self.problem_def.as_ref().unwrap();

        // Initialize the start tree.
        let start_state = pd.start_states[0].clone();
        self.start_tree.push(Node { state: start_state, parent_index: None });

        // Initialize the goal tree.
        let goal_state = pd.goal.sample_goal(&mut rand::thread_rng()).unwrap();
        self.goal_tree.push(Node { state: goal_state, parent_index: None });
    }

    fn solve(&mut self, timeout: Duration) -> Result<Path<S>, PlanningError> {
        let pd = self.problem_def.as_ref().expect("Planner not set up");
        let start_time = Instant::now();
        let mut rng = rand::thread_rng();

        loop {
            if start_time.elapsed() > timeout {
                return ErrTimeout;
            }

            // 1. Determine which tree to grow (tree_a) and which to connect to (tree_b)
            let (tree_a, tree_b) = if self.start_tree.len() <= self.goal_tree.len() {
                (&mut self.start_tree, &mut self.goal_tree)
            } else {
                (&mut self.goal_tree, &mut self.start_tree)
            };

            // 2. Sample a state and try to extend tree_a towards it
            let q_rand = pd.space.sample_uniform(&mut rng).unwrap();
            let (extend_result, new_node_idx_a) = self.extend(tree_a, &q_rand);

            // 3. If the tree was successfully extended...
            if extend_result != ExtendResult::Trapped {
                let q_new = &tree_a[new_node_idx_a].state.clone();

                // IMPROVEMENT: Check if the new node in the start tree already satisfies the goal.
                let is_start_tree = Arc::ptr_eq(&pd.start_states[0], &tree_a[0].state);
                if is_start_tree && pd.goal.is_satisfied(q_new) {
                    println!("Solution found by start tree reaching goal directly.");
                    return Ok(self.reconstruct_single_tree_path(tree_a, new_node_idx_a));
                }
                
                // 4. Try to connect tree_b to the new node q_new
                let (connect_result, new_node_idx_b) = self.extend(tree_b, q_new);
                
                // 5. If the connection reached q_new, we have found a solution
                if connect_result == ExtendResult::Reached {
                    println!(
                        "Solution found after {} total nodes.",
                        self.start_tree.len() + self.goal_tree.len()
                    );
                    // Reconstruct path based on original tree identities
                    if is_start_tree {
                         return Ok(self.reconstruct_path(new_node_idx_a, new_node_idx_b));
                    } else {
                         return Ok(self.reconstruct_path(new_node_idx_b, new_node_idx_a));
                    }
                }
            }
        }
    }
}