How does a robot arm find a path through a cluttered workspace? Unlike grid-based planning, robot arms live in configuration space—the space of all possible joint angles. A 5-joint arm has a 5-dimensional C-space! Sampling-based planners handle this complexity by randomly exploring the space rather than discretizing it. This project implements and compares four popular algorithms.
How do you navigate a robot arm through a cluttered space? PRM builds a 'roadmap' by scattering random configurations throughout the free space and connecting nearby valid positions. Think of it like dropping breadcrumbs in the valid regions, then connecting them into a network of paths.
The roadmap is built once, then reused for many queries—great when you need to plan multiple paths in the same environment:
1while (numNodes < mapSamples) {
2 Node curr = getRandomNode(numofDOFs);
3 if (IsValidArmConfiguration(curr)) {
4 roadMap[curr] = {};
5 // Connect to neighbors within radius
6 }
7}Once the roadmap exists, finding a path is just graph search. The tricky part is choosing parameters: too few samples leaves gaps in coverage, too many wastes computation. The connection radius balances local detail against global connectivity.
Unlike PRM's roadmap, RRT grows a tree from the start toward the goal. It randomly picks a point in space, finds the nearest node in the current tree, and extends toward that point. The tree rapidly spreads to explore the space—hence the name.
radius to control growthThe clever trick is goal biasing: occasionally sample near the goal instead of purely randomly. This gives the tree direction without sacrificing exploration:
1if (prob < 0.1)
2 randomNode = getRandomGoalNode(goal, radius);
3else
4 randomNode = getRandomNode(numofDOFs);The tree extends one step at a time: find the closest existing node, step toward the sample (but not too far), and add the new node if the path is clear. When any node can directly reach the goal, we're done—no need to keep exploring.
Why grow just one tree? RRT-Connect grows two trees simultaneously—one from the start, one from the goal—trying to meet in the middle. This bidirectional approach often finds paths much faster, especially through narrow passages.
Here's the magic: while the 'leading' tree takes one careful step, the 'lagging' tree greedily extends as far as it can toward the leader's new node. If they connect—success!
1auto [res, adv] = extendRRT(*leading, random);
2if (res != Trapped) {
3 // Greedily extend lagging toward adv
4 auto conn = connectRRTCONNECT(adv, *lagging);
5 if (conn == Reached) return finalPath;
6}
7swap(leading, lagging); // Swap rolesBy swapping roles each iteration, both trees grow at similar rates and meet somewhere in the middle. This bidirectional approach shines in problems with tight corridors—a single tree might wander aimlessly, but two trees searching from opposite ends find each other much faster.
Basic RRT finds a path, but not necessarily a good one. RRT* adds path optimization: as the tree grows, it continuously improves by rewiring connections. Given enough time, it converges to the optimal path—not just any feasible one.
When adding a new node, don't just connect to the nearest neighbor. Look at all nearby nodes and pick the one that gives the lowest total cost from start.
Here's the key innovation: for each nearby node, check if routing through the new node would be cheaper. If so, rewire—change that node's parent to the new node:
1for (const Node& nb : neighbors) {
2 double costViaNw = costMap[newNode] + dist(newNode, nb);
3 if (costViaNw < costMap[nb]) {
4 updateParent(tree, nb, newNode); // Rewire
5 costMap[nb] = costViaNw;
6 }
7}As more nodes are added, the tree keeps improving. Paths that looked good early on get rewired when better routes appear. This is what makes RRT* asymptotically optimal: given enough samples, it finds the true shortest path.
Every planner has knobs to tune. I chose parameters that give each algorithm a fair shot while keeping computation reasonable.
Good news: all four planners successfully found paths in every test case. But they differ dramatically in speed and path quality.
There's no single 'best' planner—each excels at different things. The right choice depends on what matters for your application:
Match the planner to your needs: speed (RRT-Connect), optimality (RRT*), or multi-query efficiency (PRM).