Implementing RRT in Matlab
- First, create a class to represent a node in the RRT tree:
- Then, define the sampling region and the goal region. For example:
- Now, we can implement the main RRT loop. We will start by creating the root node and adding it to the tree:
- Next, we will generate a random sample and find the closest node in the tree:
- Now, we can generate a new node in the direction of the sample, with a maximum step size:
- We will then check if the new node is in collision with any obstacles. If not, we will add it to the tree:
- Finally, we will check if the new node is within the goal region. If so, we have found a path to the goal:
- The
collision_check
function and reconstruct_path
function can be implemented as follows:
- The RRT loop can be run for a fixed number of iterations, or until a path to the goal is found. The complete implementation is shown below:
classdef RRTNode
properties
state % state of the node
parent % parent node in the tree
end
methods
function node = RRTNode(state, parent)
node.state = state;
node.parent = parent;
end
end
end
function colliding = collision_check(node, obstacles)
colliding = false;
for i = 1:size(obstacles,2)
if inpolygon(node.state(1), node.state(2), obstacles{1,i}, obstacles{2,i})
colliding = true;
break;
end
end
end
function path = reconstruct_path(node)
path = node.state;
while ~isempty(node.parent)
node = node.parent;
path = [node.state, path];
end
end
sampling_bounds = [-5, 5; -5, 5];
goal_region = [4, 4; 0.5, 0.5];
max_step_size = 0.1;
obstacles = {};
root = RRTNode(rand(2,1) .* diff(sampling_bounds)' + sampling_bounds(:,1), []);
tree = root;
for i = 1:1000
sample = rand(2,1) .* diff(sampling_bounds)' + sampling_bounds(:,1);
[~, closest_node] = min(vecnorm([tree.state] - sample, 2, 1));
new_state = tree(closest_node).state + min(norm(sample - tree(closest_node).state), max_step_size) .* (sample - tree(closest_node).state) ./ norm(sample - tree(closest_node).state);
new_node = RRTNode(new_state, closest_node);
if ~collision_check(new_node, obstacles)
tree(end+1) = new_node;
end
if norm(new_node.state - goal_region(1,:)') < goal_region(2,1)
path = reconstruct_path(new_node);
break;
end
end
if exist('path', 'var')
figure; hold on;
for i = 1:size(obstacles,2)
patch(obstacles{1,i}, obstacles{2,i}, 'r');
end
plot([tree.state(1) tree(tree(1).parent).state(1)], [tree.state(2) tree(tree(1).parent).state(2)], 'b');
for i = 2:numel(tree)
plot([tree(i).state(1) tree(tree(i).parent).state(1)], [tree(i).state(2) tree(tree(i).parent).state(2)], 'b');
end
plot(path(1,:), path(2,:), 'g', 'LineWidth', 2);
axis(sampling_bounds(:)');
title('RRT Path Planning');
else
disp('Path not found');
end