Custom Bidirectional Expansive Space Tree (BiEST) motion planner integrated with MoveIt 2 for collision-free trajectory generation on the UR5 manipulator. Combines weighted roulette-wheel sampling, goal-biased exploration, and iterative backtracking across dual trees. Tested in RViz, Gazebo, Isaac Sim, and on physical hardware.

Overview

I built this as part of Algorithms for Sensor-Based Robotics (EN.601.463/663) at Johns Hopkins. It’s a graduate course I later TA’d for two semesters, mentoring 80+ students through these exact types of motion planning problems. Having seen dozens of students struggle with sampling-based planners (EST, RRT, RRT*, PRM), I know firsthand where the intuition breaks down: why weighted sampling matters, how goal bias interacts with cluttered environments, and why a deterministic tree alternation beats a random coin flip by nearly 5x in planning time. This project is my own ground-up implementation of a Bidirectional Expansive Space Tree, written in C++ and plugged directly into MoveIt 2 as a custom planning plugin for the UR5.

The idea is straightforward: grow two trees, one from start and one from goal, and try to connect them. The hard part is making the exploration efficient. I used three techniques that made the difference: weighted roulette-wheel sampling to push the trees into under-explored regions, a tuned 18.5% goal bias to keep the growth directional, and Gaussian perturbation with joint-limit wrapping so the sampler respects the UR5’s workspace without clipping. I tested across RViz, Gazebo, NVIDIA Isaac Sim, and on a physical UR5, achieving a 95%+ success rate across all environments.

Demo

System Architecture

flowchart TB
    A["MoveIt 2 Planning Request"] --> B["solve()"]
    B --> C["est(q_init, q_goal)"]
    C --> D{"Alternate Trees"}
    D -->|"Tree 1"| E["extend(V_root → V_goal)"]
    D -->|"Tree 2"| F["extend(V_goal → V_root)"]
    E --> G["select_config_from_tree(w)"]
    F --> G
    G --> H["sample_nearby(q, q_target)"]
    H --> I["find_nearest_configuration()"]
    I --> J{"Local Path Collision Free?"}
    J -->|"Yes"| K["Add to Tree + Update Weights"]
    J -->|"No"| D
    K --> L{"Bridge Distance < Threshold?"}
    L -->|"Yes"| M["search_path() + Concatenate"]
    L -->|"No"| D
    M --> N["Interpolated Trajectory → RViz"]

How It Works

The Core Loop

Every iteration, the planner picks one of the two trees to expand. I initially tried random selection, essentially flipping a coin to pick a tree. It worked, but it was slow. When I switched to a deterministic alternation (just toggle a boolean), planning time on the Bugatti obstacle scene dropped from 351ms to 72ms. That was a big lesson: in BiEST, balanced growth matters more than randomized fairness.

Each expansion attempt follows a pipeline. First, select a vertex from the tree. Then, sample a new configuration near it. Next, check if the path to the sample is collision-free. Finally, see if the new vertex is close enough to the other tree to form a bridge.

Weighted Roulette-Wheel Sampling

The key insight behind EST is that you don’t want to keep expanding from the same well-explored nodes. You want the tree to push outward into new territory. I handle this with a weight on every vertex. When a new node gets added, its weight is:

\[w_{\text{child}} \;=\; \frac{1}{1 \;+\; w_{\text{parent}}}\]

So a vertex spawned from a heavily-explored parent (high $w$) gets a low weight, while vertices in sparse regions keep their high weights. To actually select which vertex to expand from, I use roulette-wheel sampling over the cumulative weight distribution:

\[P(\text{select vertex } i) \;=\; \frac{w_i}{\displaystyle\sum_{j=1}^{|V|} w_j}\]

I draw a random point $r \sim \mathcal{U}!\left(0,\; \sum w_j\right)$ and walk the cumulative distribution until I find the vertex that “owns” that point. Vertices with higher weights occupy larger intervals, so they get selected more often. This naturally drives exploration toward the sparse frontiers of the tree.

Gaussian Perturbation with Joint-Limit Wrapping

Once I’ve selected a vertex $\mathbf{q}$ to expand from, I need to generate a nearby sample. I add Gaussian noise independently to each joint:

\[q_i^{\,\text{rand}} \;=\; \Big(\big(q_i \;+\; \underbrace{\mathcal{N}(0,\,\pi)}_{\text{noise}}\big) + \pi\Big) \bmod 2\pi \;\;-\;\; \pi\]

The $\sigma = \pi$ gives wide coverage of the configuration space, and the modular arithmetic wraps the result back into the UR5’s joint limits $[-\pi,\,\pi]$ without clipping or rejection. If the resulting configuration is in collision, I retry up to 1000 times before falling back to the original vertex.

Goal Biasing

Pure random exploration is thorough but slow. To speed things up, I added a tunable “greediness dial.” With probability $p = 0.185$, the sampler skips the Gaussian perturbation entirely and just returns the target configuration:

\[\mathbf{q}^{\,\text{rand}} = \begin{cases} \mathbf{q}_{\text{target}} & \text{with probability } 0.185 \\ \mathbf{q} + \mathcal{N}(\mathbf{0},\, \pi\mathbf{I}) & \text{otherwise} \end{cases}\]

I landed on 18.5% after a lot of trial and error. Too high and the planner wastes iterations trying to force direct paths through obstacles. Too low and the trees grow aimlessly. This value gave me the best balance between directed growth and exploratory coverage across different obstacle scenes.

Collision Checking Along Local Paths

Before adding a new vertex to the tree, I need to verify that the straight-line path in C-space between the nearest existing vertex and the new sample is collision-free. I do this by interpolating between the two configurations at a resolution of $\Delta t = 0.005$:

\[\mathbf{q}(t) \;=\; (1-t)\,\mathbf{q}_{\text{near}} \;+\; t\,\mathbf{q}_{\text{rand}}, \qquad t \in \{0.005,\; 0.01,\; \ldots,\; 1.0\}\]

That’s 200 collision checks per local path segment. At each interpolated configuration, MoveIt’s isStateColliding() queries the full planning scene. A coarser resolution (say $\Delta t = 0.01$) would be faster, but it misses narrow passages. I learned that the hard way when the planner kept producing paths that clipped obstacle corners.

Bridge Detection and Path Recovery

After successfully adding a new vertex $\mathbf{q}_{\text{new}}$ to the expanding tree, I check its distance to the nearest vertex in the opposing tree:

\[d\big(\mathbf{q}_{\text{new}},\; \mathbf{q}_{\text{nearest}}^{\,\text{other}}\big) \;=\; \left\|\mathbf{q}_{\text{new}} - \mathbf{q}_{\text{nearest}}^{\,\text{other}}\right\|_2 \;<\; \tau\]

where the bridge threshold $\tau = 13$ (in C-space Euclidean distance). If the distance is below threshold and the local path between them is collision-free, the trees are connected. From there, I backtrack through each tree’s parent array, walking from the bridge point back to each tree’s root, and then concatenate the two half-paths. The goal-tree half gets reversed so the full path runs start to goal.

The backtracking uses an iterative approach with a visited-node set to prevent cycles. Recursive backtracking would work too, but I didn’t want to risk a stack overflow on deep trees.

Results

Metric	Value
Planning success rate	95%+ across test scenarios
Planning time (simple scene)	~72 ms
Planning time (cluttered scene)	~72 ms (alternating) vs ~351 ms (random)
Collision resolution	200 points per segment
Goal bias	18.5%
Bridge threshold	$\tau = 13$
Max iterations	11,300
Tested on	RViz, Gazebo, Isaac Sim, physical UR5

Links

• Project Page