gistlib - implement rrt in matlab

Implementing RRT in Matlab

First, create a class to represent a node in the RRT tree:


main.m
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
264 chars
13 lines

Then, define the sampling region and the goal region. For example:


main.m
sampling_bounds = [-5, 5; -5, 5]; % bounds of the sampling region
goal_region = [4, 4; 0.5, 0.5]; % center and size of the goal region
135 chars
3 lines

Now, we can implement the main RRT loop. We will start by creating the root node and adding it to the tree:


main.m
root = RRTNode(rand(2,1) .* diff(sampling_bounds)' + sampling_bounds(:,1), []);
tree = root;
93 chars
3 lines

Next, we will generate a random sample and find the closest node in the tree:


main.m
sample = rand(2,1) .* diff(sampling_bounds)' + sampling_bounds(:,1);
[~, closest_node] = min(vecnorm([tree.state] - sample, 2, 1));
132 chars
3 lines

Now, we can generate a new node in the direction of the sample, with a maximum step size:


main.m
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);
227 chars
3 lines

We will then check if the new node is in collision with any obstacles. If not, we will add it to the tree:


main.m
if ~collision_check(new_node, obstacles)
    % add node to tree
    tree(end+1) = new_node;
end
96 chars
5 lines

Finally, we will check if the new node is within the goal region. If so, we have found a path to the goal:


main.m
if norm(new_node.state - goal_region(1,:)') < goal_region(2,1)
    % found path to goal
    path = reconstruct_path(new_node);
    break;
end
142 chars
6 lines

The collision_check function and reconstruct_path function can be implemented as follows:


main.m
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
435 chars
18 lines

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:


main.m
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
2064 chars
69 lines

implement rrt in matlab

Implementing RRT in Matlab

similar matlab code snippets

related categories