function result = RRTstar(param, p_start, p_goal)
% RRT*
% credit : Anytime Motion Planning using the RRT*, S. Karaman, et. al.
% calculates the path using RRT* algorithm
% param : parameters for the problem
% 1) threshold : stopping criteria (distance between goal and current
% node)
% 2) maxNodes : maximum nodes for rrt tree
% 3) neighborhood : distance limit used in finding neighbors
% 4) obstacle : must be rectangle-shaped #limitation
% 5) step_size : the maximum distance that a robot can move at a time
% (must be equal to neighborhood size) #limitation
% 6) random_seed : to control the random number generation
% p_start : [x;y] coordinates
% p_goal : [x;y] coordinates
% variable naming : when it comes to describe node, if the name is with
% 'node', it means the coordinates of that node or it is just an index of
% rrt tree
% rrt struct : 1) p : coordinate, 2) iPrev : parent index, 3) cost :
% distance
% obstacle can only be detected at the end points but not along the line
% between the points
% for cost, Euclidean distance is considered.
% output : cost, rrt, time_taken
% whether goal is reached or not, depends on the minimum distance between
% any node and goal
field1 = 'p'; %position
field2 = 'iPrev'; %partent
field3 = 'cost';
field4 = 'goalReached';
rng(param.random_seed);
tic;
start();
function start()
rrt(1) = struct(field1, p_start, field2, 0, field3, 0, field4, 0);
N = param.maxNodes; % iterations
j = 1;
% while endcondition>param.threshold %&& j<=N
while ~rrt(end).goalReached && j<=N
sample_node = getSample(param.searchFeild);
% plot(sample_node(1), sample_node(2), '.g');
% text(sample_node(1), sample_node(2), strcat('random',num2str(j)))
% idx = 1;
% mincost = rrt(1).cost + norm(sample_node-rrt(1).p);
% for k = 2:length(rrt)
% cost = rrt(k).cost + norm(sample_node-rrt(k).p);
% if cost < mincost
% % mincost = cost;
end
function rrt=setReachGoal(rrt)
rrt(end).goalReached = 1;
end
for i = 1: length(rrt)
p1 = rrt(i).p;
% rob.x = p1(1); rob.y=p1(2);
plot(p1(1),p1(2),'.b')
child_ind = find([rrt.iPrev]==i);
for j = 1: length(child_ind)
p2 = rrt(child_ind(j)).p;
plot([p1(1),p2(1)], [p1(2),p2(2)], 'b', 'LineWidth', 1);
end
end
end
end
result.time_taken = toc;
end
function [value,min_node_ind] = getFinalResult(rrt)
goal_ind = find([rrt.goalReached]==1);
if ~(isempty(goal_ind))
disp('Goal has been reached!');
rrt_goal = rrt(goal_ind);
value = min([rrt_goal.cost]);
min_node_ind = find([rrt.cost]==value);
if length(min_node_ind)>1
min_node_ind = min_node_ind(1);
end
else
disp('Goal has not been reached!');
for i =1:length(rrt)
norm_rrt(i) = norm(p_goal-rrt(i).p);
end
[~,min_node_ind]= min(norm_rrt);
value = rrt(min_node_ind).cost;
end
end
end
% if it is obstacle-free, return 1.
% otherwise, return 0
function free=isObstacleFree(node_free) %4
free = 1;
for i = 1: length(param.obstacles(:,1))
obs = param.obstacles(i,:);
% op1 = [obstacle(1), obstacle(2)];
% op2 = [op1(1)+obstacle(3), op1(2)];
% op3 = [op2(1), op1(2) + obstacle(4)];
% op4 = [op1(1), op3(2)];
nx = node_free(1);
ny = node_free(2);
ha = (nx-obs(1))^2 / obs(3)^2 + (ny-obs(2))^2 / obs(4)^2;
if (ha <= 1)
free = 0;
end
end
end
function new_node=steering(nearest_node, random_node) %3
dist = norm(random_node-nearest_node);
ratio_distance = param.step_size/dist;
if ratio_distance < 1
x = (1-ratio_distance).* nearest_node(1)+ratio_distance .* random_node(1);
y = (1-ratio_distance).* nearest_node(2)+ratio_distance .* random_node(2);
new_node = [x;y];
else
new_node = random_node;
end
end
function rrt = reWire(rrt, neighbors, parent, new) %8
for i=1:length(neighbors)
cost = rrt(new).cost + norm(rrt(neighbors(i)).p - rrt(new).p);
if (cost<rrt(neighbors(i)).cost)
% if norm(rrt(new).p-rrt(neighbors(i)).p)<param.step_size
% % plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.b');
% rrt(neighbors(i)).p = steering(rrt(new).p, rrt(neighbors(i)).p);
% end
% plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.m');
rrt(neighbors(i)).iPrev = new;
rrt(neighbors(i)).cost = cost;
end
end
end
function rrt = insertNode(rrt, parent, new_node) %7
rrt(end+1) = struct(field1, new_node, field2, parent, field3,...
rrt(parent).cost + norm(rrt(parent).p-new_node), field4, 0);
end
function parent = chooseParent(rrt, neighbors, nearest, new_node) %6
min_cost = getCostFromRoot(rrt, nearest, new_node);
parent = nearest;
for i=1:length(neighbors)
cost = getCostFromRoot(rrt, neighbors(i), new_node);
if (cost<min_cost)
min_cost = cost;
parent = neighbors(i);
end
end
end
function cost = getCostFromRoot(rrt, parent, child_node) %6.1
cost = rrt(parent).cost + norm(child_node - rrt(parent).p);
end
if isObstacleFree(node)