matlab-rrt-variants
===================RRT*, RRT-connect, lazy RRT and RRT extend have been implemented for 2d and 3d c-spaces with visualization#General Information:This is a basic yet meaningful implementation of RRT and its variants in Matlab.#How to run
All you need to do is fire up the benchmarkRRT.m file, it is pretty self explanatory.# Specify the number of runs for each planner
* num_of_runs =1;# Specify if we want to run the specific planner or not, 1 for yes and 0 for no.
* run_RRTconnect =0 or 1;
* run_RRTextend = 0 or 1;
* run_LazyRRT = 0 or 1;
* run_RRTstar = 0 or 1;# Specify whether to run the planner in 2D or 3D (only for now)
* dim = 3;# Specify the step size, the world is 100 \* 100 for 2D and 100 \* 100 \*100 for 3D
* stepsize = [10];# Specify whether to use random obstacles or to use pre programmed obstacles
* random_world = 0 or 1;# For RRT* only
*radius = 10;
*samples = 4000;# Showing output or not
*show_output = 0 or 1;
*show_benchmark_results = 0 or 1;

%Author : Adnan Munawar
%Email  : amunawar@wpi.edu ; adnan.munawar@live.com
%MS Robotics, Worcester Polytechnic Institutefunction benchmarkRRTclc;
close all;
clear all;num_of_runs =1;
run_RRTconnect =1;
run_RRTextend = 0;
run_LazyRRT = 0;
run_RRTstar = 0;dim = 3;
stepsize = [10];random_world = 1;
radius = 10;
samples = 4000;show_output = 1;
show_benchmark_results = 0;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%t_lazy = [];
t_extend = [];
t_connect = [];
t_star = [];l_lazy = [];
l_extend = [];
l_connect = [];
l_star = [];p_lazy = [];
p_extend = [];
p_connect = [];
p_star = [];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%for sits = 1:size(stepsize,2)segmentLength = stepsize(sits);if run_LazyRRT == 1time = 0;avg_its = 0;avg_path = 0;for i = 1:num_of_runs[n_its path_n run_time] =  LazyRRT3D(dim,segmentLength,random_world,show_output);time = time + run_time;avg_its = avg_its + n_its;avg_path = avg_path + path_n;endstr1 = ['The time taken by Lazy RRT for ', num2str(num_of_runs), ' runs is ', num2str(time)];str2 = ['The averagae time taken by Lazy RRT for each run is ', num2str(time/num_of_runs)];str3 = ['The averagae number of states explored by Lazy RRT for each run is ', num2str(avg_its/num_of_runs)];str4 = ['The averagae number of state in Path by Lazy RRT for each run is ', num2str(avg_path/num_of_runs)];disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');disp(str1);disp(str2);disp(str3);disp(str4);disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');t_lazy = [t_lazy time/num_of_runs];l_lazy = [l_lazy avg_its/num_of_runs];p_lazy = [p_lazy avg_path/num_of_runs];endif run_RRTstar == 1time = 0;avg_its = 0;avg_path = 0;for i = 1:num_of_runs[n_its path_n,run_time] = RRTstar3D(dim,segmentLength,radius,random_world,show_output,samples);time = time + run_time;avg_its = avg_its + n_its;avg_path = avg_path + path_n;endstr1 = ['The time taken by RRT-Star for ', num2str(num_of_runs), ' runs is ', num2str(time)];str2 = ['The averagae time taken by RRT_Star for each run is ', num2str(time/num_of_runs)];str3 = ['The averagae number of states explored by RRT_Star for each run is ', num2str(avg_its/num_of_runs)];str4 = ['The averagae number of state in Path by RRT-Star for each run is ', num2str(avg_path/num_of_runs)];disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');disp(str1);disp(str2);disp(str3);disp(str4);disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');t_star = [t_star time/num_of_runs];l_star = [l_star avg_its/num_of_runs];p_star = [p_star avg_path/num_of_runs];endif run_RRTextend == 1time = 0;avg_its = 0;avg_path = 0;for i = 1:num_of_runs[n_its path_n,run_time] = RRTextend3D(dim,segmentLength,random_world,show_output);time = time + run_time;avg_its = avg_its + n_its;avg_path = avg_path + path_n;endstr1 = ['The time taken by RRT-Extend for ', num2str(num_of_runs), ' runs is ', num2str(time)];str2 = ['The averagae time taken by RRT_Extend for each run is ', num2str(time/num_of_runs)];str3 = ['The averagae number of states explored by RRT_Extend for each run is ', num2str(avg_its/num_of_runs)];str4 = ['The averagae number of state in Path by RRT-Extend for each run is ', num2str(avg_path/num_of_runs)];disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');disp(str1);disp(str2);disp(str3);disp(str4);disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');t_extend = [t_extend time/num_of_runs];l_extend = [l_extend avg_its/num_of_runs];p_extend = [p_extend avg_path/num_of_runs];endif run_RRTconnect == 1time = 0;avg_its = 0;avg_path = 0;  for i = 1:num_of_runs[n_its path_n,run_time] =  RRTconnect3D(dim,segmentLength,random_world,show_output);time = time + run_time;avg_its = avg_its + n_its;avg_path = avg_path + path_n;endstr1 = ['The time taken by RRT-Connect for ', num2str(num_of_runs), ' runs is ', num2str(time)];str2 = ['The averagae time taken by RRT-Connect for each run is ', num2str(time/num_of_runs)];str3 = ['The averagae number of states explored by RRT-Connect for each run is ', num2str(avg_its/num_of_runs)];str4 = ['The averagae number of state in Path by RRT-Connect for each run is ', num2str(avg_path/num_of_runs)];disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');disp(str1);disp(str2);disp(str3);disp(str4);disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); t_connect = [t_connect time/num_of_runs];l_connect = [l_connect avg_its/num_of_runs];p_connect = [p_connect avg_path/num_of_runs];end
endif show_benchmark_results == 1figure;hold on;plot(stepsize,t_lazy,'r','LineWidth',2);plot(stepsize,t_extend,'g','LineWidth',2);plot(stepsize,t_connect,'b','LineWidth',2);ylabel('Computational Time');xlabel('Step Size');dim_str = sprintf('Comparison of computational time for %d Dimensional C-Space',dim);title(dim_str)hold off;figure;hold on;plot(stepsize,l_lazy,'r','LineWidth',2);plot(stepsize,l_extend,'g','LineWidth',2);plot(stepsize,l_connect,'b','LineWidth',2);ylabel('Number of States Explored');xlabel('Step Size');dim_str = sprintf(' Comparison of number of states explored for %d Dimensional C-Space',dim);title(dim_str)hold off;figure;hold on;plot(stepsize,p_lazy,'r','LineWidth',2);plot(stepsize,p_extend,'g','LineWidth',2);plot(stepsize,p_connect,'b','LineWidth',2);ylabel('Number of States in Path');xlabel('Step Size');dim_str = sprintf('Comparison for number of states in path for %d Dimensional C-Space',dim);title(dim_str)hold off;end    %     t_lazy
%     l_lazy
%     p_lazy
%
%
%
%     t_extend
%     l_extend
%     p_extend
%
%     t_connect
%     l_connect
%     p_connectend

%Author : Adnan Munawar
%Email  : amunawar@wpi.edu ; adnan.munawar@live.com
%MS Robotics, Worcester Polytechnic Institutefunction [nIterations,sizePath,run_time] =  RRTconnect_3D(dim,segmentLength,random_world,show_output)
% dim = 2;
% segmentLength = 5;
% random_world = 0;random_world表示是否使用随机障碍物（1）还是事先设定好的障碍物（0）
% standard length of path segments% start_cord - 开始节点坐标
% goal_cord - 目标节点坐标
if dim ==2
start_cord = [5,5];
goal_cord = [95,95];elsestart_cord = [5,5,5];
goal_cord = [95,95,95];
end%----------------------- create random world-----------------------------%
Size = 100;  %世界的坐标轴尺寸
NumObstacles = 100; %障碍物的个数if random_world ==1
world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end
% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)%---------------------- set starPoint and endPoint----------------------%
%%node = [point,goal_flag,cost,min_parent_idx]
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];%----------------establish tree starting with the start node------------%
tree = start_node;
a = clock;%--------check to see if start_node connects directly to end_node-------%
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...&&(collision(start_node,end_node,world,dim)==0) )path = [start_node; end_node];%------------Make randPoint as newPoint,Constant iterative-------------%
elsenIterations = 0;numPaths = 0;flag = 0;while numPaths<1,[tree,flag] = extendTree(tree,end_node,segmentLength,world,dim);  %%每次生成随机节点，从树中最近点拓展到该随机节点，并作为树中的新节点numPaths = numPaths + flag;nIterations = nIterations+1;end
end%-----------------find Minimum Path------------------------------------%
path = findMinimumPath(tree,end_node,dim);
sizePath = size(path,1);b = clock;%-----------------calculate Simulation time-----------------------------------%
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1
% find path with minimum cost to end_node
figure;
plotExpandedTree(world,tree,dim);
plotWorld(world,path,dim);end
end%%%*******************************生成随机障碍物地图***************************************%%%%
function world = createWorld(NumObstacles, endcorner, origincorner,dim)
% endcorner - 地图右上角的坐标，即终点坐标
% oringincorner - 地图左下角的坐标，即初始点的坐标
% NumObstacles - 随机障碍物的个数if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2)); %返回最小的坐标长度maxRadius = 5*maxRadius/NumObstacles/2;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;endendelseif dim ==3;% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];maxRadius = min(bounds);maxRadius = 5*maxRadius/NumObstacles;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;cz = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;world.cz(i) = cz;endendend
end%%%*******************************生成已知障碍物地图***************************************%%%%
function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),disp('Not valid corner specifications!')world=[];  % create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;world.cx(1) = cx;world.cy(1) = cy;world.radius(2) = maxRadius;cx = 75;cy = 25;world.cx(2) = cx;world.cy(2) = cy;world.radius(3) = maxRadius;cx = 25;cy = 75;world.cx(3) = cx;world.cy(3) = cy;world.radius(4) = maxRadius;cx = 25;cy = 25;world.cx(4) = cx;world.cy(4) = cy;world.radius(5) = maxRadius;cx = 75;cy = 75;world.cx(5) = cx;world.cy(5) = cy;endelseif dim == 3NumObstacles = 9;  % check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;cz = 50;world.cx(1) = cx;world.cy(1) = cy;world.cz(1) = cz;world.radius(2) = maxRadius;cx = 25;cy = 25;cz = 25;world.cx(2) = cx;world.cy(2) = cy;world.cz(2) = cz;world.radius(3) = maxRadius;cx = 75;cy = 75;cz = 75;world.cx(3) = cx;world.cy(3) = cy;world.cz(3) = cz;world.radius(4) = maxRadius;cx = 25;cy = 25;cz = 75;world.cx(4) = cx;world.cy(4) = cy;world.cz(4) = cz;world.radius(5) = maxRadius;cx = 75;cy = 75;cz = 25;world.cx(5) = cx;world.cy(5) = cy;world.cz(5) = cz;world.radius(6) = maxRadius;cx = 25;cy = 75;cz = 25;world.cx(6) = cx;world.cy(6) = cy;world.cz(6) = cz;world.radius(7) = maxRadius;cx = 75;cy = 25;cz = 25;world.cx(7) = cx;world.cy(7) = cy;world.cz(7) = cz;world.radius(8) = maxRadius;cx = 75;cy = 25;cz = 75;world.cx(8) = cx;world.cy(8) = cy;world.cz(8) = cz;world.radius(9) = maxRadius;cx = 25;cy = 75;cz = 75;world.cx(9) = cx;world.cy(9) = cy;world.cz(9) = cz;endend
end%%%*******************************生成随机节点***************************************%%%%
function node=generateRandomNode(world,dim)if dim ==2;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];
endelseif dim ==3;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];
end
end
end%%%*******************************检测是否冲突***************************************%%%%
function collision_flag = collision(node, parent, world,dim)
% node - 节点端点
% parent - 另一节点端点collision_flag = 0;%是否超出地图范围
for i=1:dimif (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))collision_flag = 1;end
end%是否在障碍物的范围内
if collision_flag == 0 && dim ==2for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),%%求矩阵范数，即节点在障碍物范围内collision_flag = 1;break;endendendelseif collision_flag == 0 && dim ==3for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),collision_flag = 1;break;endendend
end
end%%%*******************************检测节点是否可用***************************************%%%%
function collision_flag = is_point_valid(point, world,dim)collision_flag = 0;%是否超出地图范围
for i=1:dimif (point(i)>world.endcorner(i))||(point(i)<world.origincorner(i))collision_flag = 1;end
end
%是否在障碍物的范围内
if collision_flag == 0 && dim ==2p = point(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),collision_flag = 1;break;endendelseif collision_flag == 0 && dim ==3p = point(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),collision_flag = 1;break;endend
end
end%%%*******************************检测最后一个节点是否需要**********************************%%%%
function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)flag = 0;% check only last node added to tree since others have been checkedif ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...&& (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),flag = 1;end
end%%%*******************************树节点拓展***************************************%%%%
function [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,dim)flag = 0;% select a random pointrandomPoint = zeros(1,dim);for i=1:dimrandomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand; %%%在地图中产生随机节点end% find leaf on node that is closest to randomPoint% -选择tree中节点和随机节点randomPoint的欧式距离最小的点为 newpoint（最近节点nearPoint）tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;  sqrd_dist = sqr_eucl_dist(tmp,dim);[min_dist,idx] = min(sqrd_dist);min_parent_idx = idx;new_point = tree(idx,1:dim);  new_node = tree(idx,:);pflag = 0;%%%从树节点中的nearPoint沿randomPoint方向，不断向randomPoint拓展，直到到达randomPoint为止while norm(new_point-randomPoint)>0 && pflag==0  %随机节点和上一个新节点（nearPoint）的距离小于segmentLength时，将随机节点取为下一个新的节点if norm(new_point-randomPoint)<segmentLengthpflag = collision(randomPoint,tree(min_parent_idx,:),world,dim);if pflag == 0new_point = randomPoint;min_cost = cost_np(tree(min_parent_idx,:),new_point,dim);%%计算上一个newPoint（nearPoint）到下一个newPoint（randPoint）的代价值，new_node = [new_point,0,min_cost,min_parent_idx];%%min_cost为从树的主节点到最终选择的newPoint的代价值累加和tree = [tree;new_node]; %%增加新的树节点pflag = 1;goal_flag = is_goal(new_node,end_node,segmentLength,world,dim);if goal_flag == 1;tree(end,dim+1)=1;flag = 1;endend  %随机节点和上一个新节点（nearPoint）的距离大于segmentLength时，在上一个节点沿随机节点方向取步长为segmentLength的节点为下一个新节点elsenew_point = (randomPoint-tree(min_parent_idx,1:dim));new_point = tree(min_parent_idx,1:dim)+(new_point/norm(new_point))*segmentLength;min_cost  = cost_np(tree(min_parent_idx,:),new_point,dim);new_node  = [new_point, 0, min_cost, min_parent_idx];pflag = collision(new_node,tree(min_parent_idx,:),world,dim);if pflag == 0tree = [tree ; new_node];min_parent_idx = size(tree,1);goal_flag = is_goal(new_node,end_node,segmentLength,world,dim);if goal_flag == 1;    tree(end,dim+1)=1;  % mark node as connecting to end.pflag = 1;flag = 1;endend    endendnew_tree = tree;
endfunction goal_flag = is_goal(node,end_node,segmentLength,world,dim)   goal_flag = 0; if (norm(node(1:dim)-end_node(1:dim))<segmentLength )...&& (collision(node,end_node,world,dim)==0)goal_flag = 1;end
end%%%*******************计算各个节点之间的欧式距离（矩阵中向量的二范数）**********************************%%%%
function e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);%array中元素平方值
for i=1:dim   sqr_e_dist(:,i) = array(:,i).*array(:,i);
end
e_dist = zeros(size(array,1),1);
for i=1:dime_dist = e_dist+sqr_e_dist(:,i);
end
end%%%*******************用树所有节点到另一节点坐标的范数值作为节点间的代价值**********************************%%%%
%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
end%%%*******************找到所有树节点到目标点的最短路径**********************************%%%%
function path = findMinimumPath(tree,end_node,dim)% find nodes that connect to end_nodeconnectingNodes = [];for i=1:size(tree,1),if tree(i,dim+1)==1,connectingNodes = [connectingNodes ; tree(i,:)];endend% find minimum cost last node[tmp,idx] = min(connectingNodes(:,dim+2));% construct lowest cost pathpath = [connectingNodes(idx,:); end_node];parent_node = connectingNodes(idx,dim+3);while parent_node>1,parent_node = tree(parent_node,dim+3);path = [tree(parent_node,:); path];endendfunction plotExpandedTree(world,tree,dim)ind = size(tree,1);while ind>0size(tree);branch = [];node = tree(ind,:);branch = [ branch ; node ];parent_node = node(dim+3);while parent_node > 1cur_parent = parent_node;branch = [branch; tree(parent_node,:)];parent_node = tree(parent_node,dim+3);endind = ind - 1;if dim == 2X = branch(:,1);Y = branch(:,2);p = plot(X,Y);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;  elseif dim == 3X = branch(:,1);Y = branch(:,2);Z = branch(:,3);p = plot3(X,Y,Z);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;endend
endfunction plotWorld(world,path,dim)% the first element is the north coordinate% the second element is the south coordinateif dim ==2N = 10;th = 0:2*pi/N:2*pi;axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2)]);hold onfor i=1:world.NumObstacles,X = world.radius(i)*sin(th) + world.cx(i);Y = world.radius(i)*cos(th) + world.cy(i);fill(X,Y,'blue');endX = path(:,1);Y = path(:,2);p = plot(X,Y);      elseif dim ==3axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2),...world.origincorner(3), world.endcorner(3)]);hold onfor i=1:world.NumObstacles,[X Y Z] = sphere(10);X = (X*world.radius(i));Y = (Y*world.radius(i));Z = (Z*world.radius(i));surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));colormap([0.5 0.2 0.3]);endX = path(:,1);Y = path(:,2);Z = path(:,3);p = plot3(X,Y,Z);endset(p,'Color','black','LineWidth',3)xlabel('X axis');ylabel('Y axis');zlabel('Z axis');title('RRT Connect Algorithm');
end

%Author : Adnan Munawar
%Email  : amunawar@wpi.edu ; adnan.munawar@live.com
%MS Robotics, Worcester Polytechnic Institutefunction [nIterations,sizePath,run_time] =  LazyRRT3D(dim,segmentLength,random_world,show_output)
% dim = 2;
% segmentLength = 5;
% random_world = 0;
if dim ==2
start_cord = [5,5];
goal_cord = [95,95];elsestart_cord = [5,5,5];
goal_cord = [95,95,95];
end% create random world
Size = 100;
NumObstacles = 100;if random_world ==1
world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];
% establish tree starting with the start node
tree = start_node;a = clock;% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...&&(collision(start_node,end_node,world,dim)==0) )path = [start_node; end_node];
elsenIterations = 0;numPaths = 0;flag = 0;while numPaths<1,[tree,flag] = extendLazyTree(tree,end_node,segmentLength,world,dim);numPaths = numPaths + flag;nIterations = nIterations+1;end
end% find path with minimum cost to end_node
LazyPath = findMinimumPath(tree,end_node,dim);path = RepairLazyPath(LazyPath,segmentLength,world,dim);
sizePath = size(path,1);b = clock;
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1figure;
plotExpandedTree(world,tree,dim);
plotWorld(world,path,dim);
% figure(2);
% plotWorld(world,path,dim);
%plotExpandedTree(world,tree,dim);
end
endfunction path = RepairLazyPath(LazyPath,segmentLength,world,dim)
path = [];start_flag = 0;
end_flag = 0;
cflag = 0;
for i=1:(size(LazyPath,1)-1)cflag = collision(LazyPath(i+1,:),LazyPath(i,:),world,dim);if  cflag == 1 && start_flag == 0start_collision_node = LazyPath(i,:);start_flag = 1;end_flag = 1;breakage_from = i;elseif end_flag == 1 && cflag == 0end_collision_node = LazyPath(i,:);start_flag = 0;end_flag = 0;        tree = start_collision_node;end_node = end_collision_node;flag = 0;while flag == 0[tree,flag] = extendTree(tree,end_node,segmentLength,world,dim);      endrepaired_segment = findMinimumPath(tree,end_node,dim);path = [path ;  repaired_segment];breakage_to = i;elseif start_flag == 0 && end_flag == 0 && cflag == 0     path = [path ; LazyPath(i,:) ; LazyPath(i+1,:)];endendendfunction world = createWorld(NumObstacles, endcorner, origincorner,dim)if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2));maxRadius = 5*maxRadius/NumObstacles/2;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;endendelseif dim ==3;% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];maxRadius = min(bounds);maxRadius = 5*maxRadius/NumObstacles;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;cz = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;world.cz(i) = cz;endendend
endfunction [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),disp('Not valid corner specifications!')world=[];  % create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;world.cx(1) = cx;world.cy(1) = cy;world.radius(2) = maxRadius;cx = 75;cy = 25;world.cx(2) = cx;world.cy(2) = cy;world.radius(3) = maxRadius;cx = 25;cy = 75;world.cx(3) = cx;world.cy(3) = cy;world.radius(4) = maxRadius;cx = 25;cy = 25;world.cx(4) = cx;world.cy(4) = cy;world.radius(5) = maxRadius;cx = 75;cy = 75;world.cx(5) = cx;world.cy(5) = cy;endelseif dim == 3NumObstacles = 9;  % check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;cz = 50;world.cx(1) = cx;world.cy(1) = cy;world.cz(1) = cz;world.radius(2) = maxRadius;cx = 25;cy = 25;cz = 25;world.cx(2) = cx;world.cy(2) = cy;world.cz(2) = cz;world.radius(3) = maxRadius;cx = 75;cy = 75;cz = 75;world.cx(3) = cx;world.cy(3) = cy;world.cz(3) = cz;world.radius(4) = maxRadius;cx = 25;cy = 25;cz = 75;world.cx(4) = cx;world.cy(4) = cy;world.cz(4) = cz;world.radius(5) = maxRadius;cx = 75;cy = 75;cz = 25;world.cx(5) = cx;world.cy(5) = cy;world.cz(5) = cz;world.radius(6) = maxRadius;cx = 25;cy = 75;cz = 25;world.cx(6) = cx;world.cy(6) = cy;world.cz(6) = cz;world.radius(7) = maxRadius;cx = 75;cy = 25;cz = 25;world.cx(7) = cx;world.cy(7) = cy;world.cz(7) = cz;world.radius(8) = maxRadius;cx = 75;cy = 25;cz = 75;world.cx(8) = cx;world.cy(8) = cy;world.cz(8) = cz;world.radius(9) = maxRadius;cx = 25;cy = 75;cz = 75;world.cx(9) = cx;world.cy(9) = cy;world.cz(9) = cz;endend
endfunction node=generateRandomNode(world,dim)if dim ==2;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];
endelseif dim ==3;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];
endendendfunction collision_flag = collision(node, parent, world,dim)collision_flag = 0;for i=1:dimif (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))collision_flag = 1;end
endif collision_flag == 0 && dim ==2for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),collision_flag = 1;break;endendendelseif collision_flag == 0 && dim ==3for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),collision_flag = 1;break;endendend
end
endfunction flag = canEndConnectToTree(tree,end_node,minDist,world,dim)flag = 0;% check only last node added to tree since others have been checkedif ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...& (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),flag = 1;endendfunction [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,dim)flag = 0;flag1 = 0;while flag1==0,% select a random pointrandomPoint = ones(1,dim);for i=1:dimrandomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;end% find leaf on node that is closest to randomPointtmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;sqrd_dist = sqr_eucl_dist(tmp,dim);[min_dist,idx] = min(sqrd_dist);new_point = (randomPoint-tree(idx,1:dim));new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength;min_cost  = cost_np(tree(idx,:),new_point,dim);new_node  = [new_point, 0, min_cost, idx];if collision(new_node, tree(idx,:), world,dim)==0new_tree = [tree;new_node];flag1 = 1;elseflag1=0;endend% check to see if new node connects directly to end_nodeif ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )...&& (collision(new_node,end_node,world,dim)==0) )flag = 1;new_tree(end,dim+1)=1;  % mark node as connecting to end.end
endfunction [new_tree,flag] = extendLazyTree(tree,end_node,segmentLength,world,dim)% select a random pointrandomPoint = ones(1,dim);for i=1:dimrandomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;end% find leaf on node that is closest to randomPointtmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;sqrd_dist = sqr_eucl_dist(tmp,dim);[min_dist,idx] = min(sqrd_dist);new_point = (randomPoint-tree(idx,1:dim));new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength;min_cost  = cost_np(tree(idx,:),new_point,dim);new_node  = [new_point, 0, min_cost, idx];new_tree = [tree;new_node];% check to see if new node connects directly to end_nodeif ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )...&& (collision(new_node,end_node,world,dim)==0) )flag = 1;new_tree(end,dim+1)=1;  % mark node as connecting to end.elseflag = 0;endendfunction e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);
for i=1:dimsqr_e_dist(:,i) = array(:,i).*array(:,i);end
e_dist = zeros(size(array,1),1);
for i=1:dime_dist = e_dist+sqr_e_dist(:,i);endend%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
endfunction path = findMinimumPath(tree,end_node,dim)% find nodes that connect to end_nodeconnectingNodes = [];for i=1:size(tree,1),if tree(i,dim+1)==1,connectingNodes = [connectingNodes ; tree(i,:)];endend% find minimum cost last node[tmp,idx] = min(connectingNodes(:,dim+2));% construct lowest cost pathpath = [connectingNodes(idx,:); end_node];parent_node = connectingNodes(idx,dim+3);while parent_node>1,parent_node = tree(parent_node,dim+3);path = [tree(parent_node,:); path];endendfunction plotExpandedTree(world,tree,dim)ind = size(tree,1);while ind>0branch = [];node = tree(ind,:);branch = [ branch ; node ];parent_node = node(dim+3);while parent_node > 1cur_parent = parent_node;branch = [branch; tree(parent_node,:)];parent_node = tree(parent_node,dim+3);endind = ind - 1;if dim == 2X = branch(:,1);Y = branch(:,2);p = plot(X,Y);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;  elseif dim == 3X = branch(:,1);Y = branch(:,2);Z = branch(:,3);p = plot3(X,Y,Z);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;endend
endfunction plotWorld(world,path,dim)% the first element is the north coordinate% the second element is the south coordinateif dim ==2N = 10;th = 0:2*pi/N:2*pi;axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2)]);hold onfor i=1:world.NumObstacles,X = world.radius(i)*sin(th) + world.cx(i);Y = world.radius(i)*cos(th) + world.cy(i);fill(X,Y,'blue');endX = path(:,1);Y = path(:,2);p = plot(X,Y);      elseif dim ==3axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2),...world.origincorner(3), world.endcorner(3)]);hold onfor i=1:world.NumObstacles,[X Y Z] = sphere(10);X = (X*world.radius(i));Y = (Y*world.radius(i));Z = (Z*world.radius(i));surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));colormap([0.5 0.2 0.3]);endX = path(:,1);Y = path(:,2);Z = path(:,3);p = plot3(X,Y,Z);endset(p,'Color','black','LineWidth',3)xlabel('X axis');ylabel('Y axis');zlabel('Z axis');title('Lazy RRT Algorithm');
end

%Author : Adnan Munawar
%Email  : amunawar@wpi.edu ; adnan.munawar@live.com
%MS Robotics, Worcester Polytechnic Institutefunction [its,sizePath,run_time] =  RRTextend3D(dim,segmentLength,random_world,show_output)
% dim = 2;
% radius =0;
% segmentLength = 5;
% random_world = 0;
% n_its = 1000;
% standard length of path segments
if dim ==2
start_cord = [5,5];
goal_cord = [95,95];elsestart_cord = [5,5,5];
goal_cord = [95,95,95];
end% create random world
Size = 100;
NumObstacles = 100;if random_world ==1
world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end
% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];
% establish tree starting with the start node
tree = start_node;a = clock;% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...&&(collision(start_node,end_node,world,dim)==0) )path = [start_node; end_node];
elseits = 0;numPaths = 0;flag = 0;while numPaths < 1,[tree,flag] = extendTree(tree,end_node,segmentLength,world,flag,dim);numPaths = numPaths + flag;its = its+1;end    end% find path with minimum cost to end_node
path = findMinimumPath(tree,end_node,dim);
sizePath = size(path,1);b = clock;
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1
figure;
plotExpandedTree(world,tree,dim);
plotWorld(world,path,dim);
% figure(2);
% plotWorld(world,path,dim);
%plotExpandedTree(world,tree,dim);
end
endfunction world = createWorld(NumObstacles, endcorner, origincorner,dim)if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2));maxRadius = 5*maxRadius/NumObstacles/2;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;endendelseif dim ==3;% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];maxRadius = min(bounds);maxRadius = 5*maxRadius/NumObstacles;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;cz = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;world.cz(i) = cz;endendend
endfunction [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),disp('Not valid corner specifications!')world=[];  % create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;world.cx(1) = cx;world.cy(1) = cy;world.radius(2) = maxRadius;cx = 75;cy = 25;world.cx(2) = cx;world.cy(2) = cy;world.radius(3) = maxRadius;cx = 25;cy = 75;world.cx(3) = cx;world.cy(3) = cy;world.radius(4) = maxRadius;cx = 25;cy = 25;world.cx(4) = cx;world.cy(4) = cy;world.radius(5) = maxRadius;cx = 75;cy = 75;world.cx(5) = cx;world.cy(5) = cy;endelseif dim == 3NumObstacles = 9;  % check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;cz = 50;world.cx(1) = cx;world.cy(1) = cy;world.cz(1) = cz;world.radius(2) = maxRadius;cx = 25;cy = 25;cz = 25;world.cx(2) = cx;world.cy(2) = cy;world.cz(2) = cz;world.radius(3) = maxRadius;cx = 75;cy = 75;cz = 75;world.cx(3) = cx;world.cy(3) = cy;world.cz(3) = cz;world.radius(4) = maxRadius;cx = 25;cy = 25;cz = 75;world.cx(4) = cx;world.cy(4) = cy;world.cz(4) = cz;world.radius(5) = maxRadius;cx = 75;cy = 75;cz = 25;world.cx(5) = cx;world.cy(5) = cy;world.cz(5) = cz;world.radius(6) = maxRadius;cx = 25;cy = 75;cz = 25;world.cx(6) = cx;world.cy(6) = cy;world.cz(6) = cz;world.radius(7) = maxRadius;cx = 75;cy = 25;cz = 25;world.cx(7) = cx;world.cy(7) = cy;world.cz(7) = cz;world.radius(8) = maxRadius;cx = 75;cy = 25;cz = 75;world.cx(8) = cx;world.cy(8) = cy;world.cz(8) = cz;world.radius(9) = maxRadius;cx = 25;cy = 75;cz = 75;world.cx(9) = cx;world.cy(9) = cy;world.cz(9) = cz;endend
endfunction node=generateRandomNode(world,dim)if dim ==2;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];
endelseif dim ==3;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];
endendendfunction collision_flag = collision(node, parent, world,dim)collision_flag = 0;for i=1:dimif (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))collision_flag = 1;end
endif collision_flag == 0 && dim ==2for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),collision_flag = 1;break;endendendelseif collision_flag == 0 && dim ==3for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),collision_flag = 1;break;endendend
end
endfunction flag = canEndConnectToTree(tree,end_node,minDist,world,dim)flag = 0;% check only last node added to tree since others have been checkedif ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...& (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),flag = 1;endendfunction [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,flag_chk,dim)r = 0;flag1 = 0;while flag1==0,% select a random pointrandomPoint = ones(1,dim);for i=1:dimrandomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;end% find leaf on node that is closest to randomPointtmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;sqrd_dist = sqr_eucl_dist(tmp,dim);[min_dist,idx] = min(sqrd_dist);min_parent_idx = idx;new_point = (randomPoint-tree(idx,1:dim));new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength;min_cost  = cost_np(tree(idx,:),new_point,dim);new_node  = [new_point, 0, min_cost, idx];if collision(new_node, tree(idx,:), world,dim)==0tmp_dist = tree(:,1:dim)-(ones(size(tree,1),1)*new_point);dist = sqr_eucl_dist(tmp_dist,dim);near_idx = find(dist <= r^2);if size(near_idx,1)>1size_near = size(near_idx,1);for i = 1:size_nearif collision(new_node, tree(near_idx(i),:), world,dim)==0cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim);if  cost_near < min_costmin_cost = cost_near;min_parent_idx = near_idx(i);endendendendnew_node = [new_point, 0 , min_cost, min_parent_idx];new_tree = [tree; new_node];new_node_idx = size(new_tree,1);if size(near_idx,1)>1reduced_idx = near_idx;for j = 1:size(reduced_idx,1)near_cost = new_tree(reduced_idx(j),dim+2);lcost = line_cost(new_tree(reduced_idx(j),:),new_point,dim);if near_cost > min_cost + lcost ...&& collision(new_tree(reduced_idx(j),:),new_node,world,dim)before = new_tree(reduced_idx(j),dim+3)new_tree(reduced_idx(j),dim+3) = new_node_idx;after = new_tree(reduced_idx(j),dim+3)endendendflag1=1;endendif flag_chk == 0% check to see if new node connects directly to end_nodeif ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )...&& (collision(new_node,end_node,world,dim)==0) )flag = 1;new_tree(end,dim+1)=1;  % mark node as connecting to end.elseflag = 0;endelse flag = 1;end
endfunction e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);
for i=1:dimsqr_e_dist(:,i) = array(:,i).*array(:,i);end
e_dist = zeros(size(array,1),1);
for i=1:dime_dist = e_dist+sqr_e_dist(:,i);endend%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
endfunction path = findMinimumPath(tree,end_node,dim)% find nodes that connect to end_nodeconnectingNodes = [];for i=1:size(tree,1),if tree(i,dim+1)==1,connectingNodes = [connectingNodes ; tree(i,:)];endend% find minimum cost last node[tmp,idx] = min(connectingNodes(:,dim+2));% construct lowest cost pathpath = [connectingNodes(idx,:); end_node];parent_node = connectingNodes(idx,dim+3);while parent_node>1,parent_node = tree(parent_node,dim+3);path = [tree(parent_node,:); path];endendfunction plotExpandedTree(world,tree,dim)ind = size(tree,1);while ind>0branch = [];node = tree(ind,:);branch = [ branch ; node ];parent_node = node(dim+3);while parent_node > 1cur_parent = parent_node;branch = [branch; tree(parent_node,:)];parent_node = tree(parent_node,dim+3);endind = ind - 1;if dim == 2X = branch(:,1);Y = branch(:,2);p = plot(X,Y);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;  elseif dim == 3X = branch(:,1);Y = branch(:,2);Z = branch(:,3);p = plot3(X,Y,Z);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;endend
endfunction plotWorld(world,path,dim)% the first element is the north coordinate% the second element is the south coordinateif dim ==2N = 10;th = 0:2*pi/N:2*pi;axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2)]);hold onfor i=1:world.NumObstacles,X = world.radius(i)*sin(th) + world.cx(i);Y = world.radius(i)*cos(th) + world.cy(i);fill(X,Y,'blue');endX = path(:,1);Y = path(:,2);p = plot(X,Y);      elseif dim ==3axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2),...world.origincorner(3), world.endcorner(3)]);hold onfor i=1:world.NumObstacles,[X Y Z] = sphere(10);X = (X*world.radius(i));Y = (Y*world.radius(i));Z = (Z*world.radius(i));surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));colormap([0.5 0.2 0.3]);endX = path(:,1);Y = path(:,2);Z = path(:,3);p = plot3(X,Y,Z);endset(p,'Color','black','LineWidth',3)xlabel('X axis');ylabel('Y axis');zlabel('Z axis');title('RRT Extend Algorithm');
end

%Author : Adnan Munawar
%Email  : amunawar@wpi.edu ; adnan.munawar@live.com
%MS Robotics, Worcester Polytechnic Institutefunction [its,sizePath,run_time] =  RRTstar3D(dim,segmentLength,radius,random_world,show_output,samples)
% dim = 2;
% radius =0;
% segmentLength = 5;
% random_world = 0;
% n_its = 1000;
% standard length of path segments
if dim ==2
start_cord = [5,5];
goal_cord = [95,95];elsestart_cord = [5,5,5];
goal_cord = [95,95,95];
end% create random world
Size = 100;
NumObstacles = 100;if random_world ==1
world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end
% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];
% establish tree starting with the start node
tree = start_node;a = clock;% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...&&(collision(start_node,end_node,world,dim)==0) )path = [start_node; end_node];
elseif samples >0   draw = samples/8;    its = 0;numPaths = 0;flag = 0;for i = 1:samples[tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim);numPaths = numPaths + flag;its = its+1;if its == draw tree_500 = tree;elseif its == draw*2tree_1000 = tree;elseif its == draw*3tree_1500 = tree;elseif its == draw*4tree_2000 = tree;elseif its == draw*5tree_2500 = tree;elseif its == draw*6tree_3000 = tree;elseif its == draw*7tree_3500 = tree;elseif its == draw*8tree_4000 = tree;    endendelseits = 0;numPaths = 0;flag = 0;while numPaths < 1,[tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim);numPaths = numPaths + flag;its = its+1;end end     end% find path with minimum cost to end_node
path = findMinimumPath(tree,end_node,dim);b = clock;
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));path_500 = findMinimumPath(tree_500,end_node,dim);path_1000 = findMinimumPath(tree_1000,end_node,dim);path_1500 = findMinimumPath(tree_1500,end_node,dim);path_2000 = findMinimumPath(tree_2000,end_node,dim);path_2500 = findMinimumPath(tree_2500,end_node,dim);path_3000 = findMinimumPath(tree_3000,end_node,dim);path_3500 = findMinimumPath(tree_3500,end_node,dim);path_4000 = findMinimumPath(tree_4000,end_node,dim);sizePath = size(path,1);if show_output == 1
figure;
plotExpandedTree(world,tree_500,dim);
plotWorld(world,path_500,dim);
figure;
plotExpandedTree(world,tree_1000,dim);
plotWorld(world,path_1000,dim);
figure;
plotExpandedTree(world,tree_1500,dim);
plotWorld(world,path_1500,dim);
figure;
plotExpandedTree(world,tree_2000,dim);
plotWorld(world,path_2000,dim);
figure;
plotExpandedTree(world,tree_2500,dim);
plotWorld(world,path_2500,dim);
figure;
plotExpandedTree(world,tree_3000,dim);
plotWorld(world,path_3000,dim);
figure;
plotExpandedTree(world,tree_3500,dim);
plotWorld(world,path_3500,dim);
figure;
plotExpandedTree(world,tree_4000,dim);
plotWorld(world,path_4000,dim);
figure;
plotExpandedTree(world,tree,dim);
plotWorld(world,path,dim);
end
endfunction world = createWorld(NumObstacles, endcorner, origincorner,dim)if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2));maxRadius = 5*maxRadius/NumObstacles/2;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;endendelseif dim ==3;% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];maxRadius = min(bounds);maxRadius = 5*maxRadius/NumObstacles;for i=1:NumObstacles,% randomly pick radiusworld.radius(i) = maxRadius*rand;% randomly pick center of obstaclescx = origincorner(1) + world.radius(i)...+ (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;cy = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;cz = origincorner(2) + world.radius(i)...+ (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;world.cx(i) = cx;world.cy(i) = cy;world.cz(i) = cz;endendend
endfunction [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),disp('Not valid corner specifications!')world=[];  % create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;world.cx(1) = cx;world.cy(1) = cy;world.radius(2) = maxRadius;cx = 75;cy = 25;world.cx(2) = cx;world.cy(2) = cy;world.radius(3) = maxRadius;cx = 25;cy = 75;world.cx(3) = cx;world.cy(3) = cy;world.radius(4) = maxRadius;cx = 25;cy = 25;world.cx(4) = cx;world.cy(4) = cy;world.radius(5) = maxRadius;cx = 75;cy = 75;world.cx(5) = cx;world.cy(5) = cy;endelseif dim == 3NumObstacles = 9;  % check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxRadius = 10;world.radius(1) = maxRadius;cx = 50;cy = 50;cz = 50;world.cx(1) = cx;world.cy(1) = cy;world.cz(1) = cz;world.radius(2) = maxRadius;cx = 25;cy = 25;cz = 25;world.cx(2) = cx;world.cy(2) = cy;world.cz(2) = cz;world.radius(3) = maxRadius;cx = 75;cy = 75;cz = 75;world.cx(3) = cx;world.cy(3) = cy;world.cz(3) = cz;world.radius(4) = maxRadius;cx = 25;cy = 25;cz = 75;world.cx(4) = cx;world.cy(4) = cy;world.cz(4) = cz;world.radius(5) = maxRadius;cx = 75;cy = 75;cz = 25;world.cx(5) = cx;world.cy(5) = cy;world.cz(5) = cz;world.radius(6) = maxRadius;cx = 25;cy = 75;cz = 25;world.cx(6) = cx;world.cy(6) = cy;world.cz(6) = cz;world.radius(7) = maxRadius;cx = 75;cy = 25;cz = 25;world.cx(7) = cx;world.cy(7) = cy;world.cz(7) = cz;world.radius(8) = maxRadius;cx = 75;cy = 25;cz = 75;world.cx(8) = cx;world.cy(8) = cy;world.cz(8) = cz;world.radius(9) = maxRadius;cx = 25;cy = 75;cz = 75;world.cx(9) = cx;world.cy(9) = cy;world.cz(9) = cz;endend
endfunction node=generateRandomNode(world,dim)if dim ==2;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;chi      = 0;
cost     = 0;
node     = [px, py, chi, cost, 0];
endelseif dim ==3;
% randomly pick configuration
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];% check collision with obstacle
while collision(node, node, world,dim),
px       = (world.endcorner(1)-world.origincorner(1))*rand;
py       = (world.endcorner(2)-world.origincorner(2))*rand;
pz       = (world.endcorner(3)-world.origincorner(3))*rand;chi      = 0;
cost     = 0;
node     = [px, py, pz, chi, cost, 0];
endendendfunction collision_flag = collision(node, parent, world,dim)collision_flag = 0;for i=1:dimif (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))collision_flag = 1;end
endif collision_flag == 0 && dim ==2for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),collision_flag = 1;break;endendendelseif collision_flag == 0 && dim ==3for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),collision_flag = 1;break;endendend
end
endfunction flag = canEndConnectToTree(tree,end_node,minDist,world,dim)flag = 0;% check only last node added to tree since others have been checkedif ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...& (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),flag = 1;endendfunction [new_tree,flag] = extendTree(tree,end_node,segmentLength,r,world,flag_chk,dim)flag1 = 0;while flag1==0,% select a random pointrandomPoint = ones(1,dim);for i=1:dimrandomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;end% find leaf on node that is closest to randomPointtmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;sqrd_dist = sqr_eucl_dist(tmp,dim);[min_dist,idx] = min(sqrd_dist);min_parent_idx = idx;new_point = (randomPoint-tree(idx,1:dim));new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength;min_cost  = cost_np(tree(idx,:),new_point,dim);new_node  = [new_point, 0, min_cost, idx];if collision(new_node, tree(idx,:), world,dim)==0tmp_dist = tree(:,1:dim)-(ones(size(tree,1),1)*new_point);dist = sqr_eucl_dist(tmp_dist,dim);near_idx = find(dist <= r^2);if size(near_idx,1)>1size_near = size(near_idx,1);for i = 1:size_nearif collision(new_node, tree(near_idx(i),:), world,dim)==0cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim);if  cost_near < min_costmin_cost = cost_near;min_parent_idx = near_idx(i);endendendendnew_node = [new_point, 0 , min_cost, min_parent_idx];new_tree = [tree; new_node];new_node_idx = size(new_tree,1);if size(near_idx,1)>1reduced_idx = near_idx;for j = 1:size(reduced_idx,1)near_cost = new_tree(reduced_idx(j),dim+2);lcost = line_cost(new_tree(reduced_idx(j),:),new_point,dim);if near_cost > min_cost + lcost ...&& collision(new_tree(reduced_idx(j),:),new_node,world,dim)before = new_tree(reduced_idx(j),dim+3)new_tree(reduced_idx(j),dim+3) = new_node_idx;after = new_tree(reduced_idx(j),dim+3)endendendflag1=1;endendif flag_chk == 0% check to see if new node connects directly to end_nodeif ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )...&& (collision(new_node,end_node,world,dim)==0) )flag = 1;new_tree(end,dim+1)=1;  % mark node as connecting to end.elseflag = 0;endelse flag = 1;end
endfunction e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);
for i=1:dimsqr_e_dist(:,i) = array(:,i).*array(:,i);end
e_dist = zeros(size(array,1),1);
for i=1:dime_dist = e_dist+sqr_e_dist(:,i);endend%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
endfunction path = findMinimumPath(tree,end_node,dim)% find nodes that connect to end_nodeconnectingNodes = [];for i=1:size(tree,1),if tree(i,dim+1)==1,connectingNodes = [connectingNodes ; tree(i,:)];endend% find minimum cost last node[tmp,idx] = min(connectingNodes(:,dim+2));% construct lowest cost pathpath = [connectingNodes(idx,:); end_node];parent_node = connectingNodes(idx,dim+3);while parent_node>1,parent_node = tree(parent_node,dim+3);path = [tree(parent_node,:); path];endendfunction plotExpandedTree(world,tree,dim)ind = size(tree,1);while ind>0branch = [];node = tree(ind,:);branch = [ branch ; node ];parent_node = node(dim+3);while parent_node > 1cur_parent = parent_node;branch = [branch; tree(parent_node,:)];parent_node = tree(parent_node,dim+3);endind = ind - 1;if dim == 2X = branch(:,1);Y = branch(:,2);p = plot(X,Y);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;  elseif dim == 3X = branch(:,1);Y = branch(:,2);Z = branch(:,3);p = plot3(X,Y,Z);set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');hold on;endend
endfunction plotWorld(world,path,dim)% the first element is the north coordinate% the second element is the south coordinateif dim ==2N = 10;th = 0:2*pi/N:2*pi;axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2)]);hold onfor i=1:world.NumObstacles,X = world.radius(i)*sin(th) + world.cx(i);Y = world.radius(i)*cos(th) + world.cy(i);fill(X,Y,'blue');endX = path(:,1);Y = path(:,2);p = plot(X,Y);      elseif dim ==3axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2),...world.origincorner(3), world.endcorner(3)]);hold onfor i=1:world.NumObstacles,[X Y Z] = sphere(10);X = (X*world.radius(i));Y = (Y*world.radius(i));Z = (Z*world.radius(i));surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));colormap([0.5 0.2 0.3]);endX = path(:,1);Y = path(:,2);Z = path(:,3);p = plot3(X,Y,Z);endset(p,'Color','black','LineWidth',3)xlabel('X axis');ylabel('Y axis');zlabel('Z axis');title('RRT Star Algorithm');
end

function RRT
% clc;
% %close all;
% clear all;num_of_runs =1;
run_RRTconnect =1;dim = 3;
stepsize = 1;random_world = 0;show_output = 1;for sits = 1:stepsizesegmentLength = 0.1;  if run_RRTconnect == 1time = 0;avg_its = 0;avg_path = 0;  for i = 1:num_of_runs[n_its,path_n,run_time] =  RRTconnect_3D(dim,segmentLength,random_world,show_output);time = time + run_time;avg_its = avg_its + n_its;avg_path = avg_path + path_n;endstr1 = ['The time taken by RRT-Connect for ', num2str(num_of_runs), ' runs is ', num2str(time)];str2 = ['The averagae time taken by RRT-Connect for each run is ', num2str(time/num_of_runs)];str3 = ['The averagae number of states explored by RRT-Connect for each run is ', num2str(avg_its/num_of_runs)];str4 = ['The averagae number of state in Path by RRT-Connect for each run is ', num2str(avg_path/num_of_runs)];disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');disp(str1);disp(str2);disp(str3);disp(str4);disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); plot3(0.15,0,0.36,'.k','Markersize',30);plot3([0.15 0.15],[0 0],[0.27 0.45],'r','LineWidth',5);end
endend

%Author : Adnan Munawar
%Email  : amunawar@wpi.edu ; adnan.munawar@live.com
%MS Robotics, Worcester Polytechnic Institutefunction [nIterations,sizePath,run_time] =  RRTconnect_3D(dim,segmentLength,random_world,show_output)
% dim = 2;
% segmentLength = 5;
% random_world = 0;random_world表示是否使用随机障碍物（1）还是事先设定好的障碍物（0）
% standard length of path segments% start_cord - 开始节点坐标
% goal_cord - 目标节点坐标
if dim ==2
start_cord = [5,5];
goal_cord = [95,95];elsestart_cord = [-0.0048,-0.12,0.22];
goal_cord = [0.146,0,0.389];
end%----------------------- create random world-----------------------------%
Size = 1;  %世界的坐标轴尺寸
NumObstacles = 10; %障碍物的个数
OriginWorld=[-0.4,-0.4,0];
EndWorld=[0.5,0.5,0.8];if random_world ==1
world = createWorld(NumObstacles,EndWorld(1,1:dim)*Size,OriginWorld(1,1:dim)*Size,dim);
else
OriginWorld=OriginWorld';
[world NumObstacles] = createKnownWorld(EndWorld(1,1:dim)*Size,OriginWorld(1:dim,1)*Size,dim);
end%---------------------- set starPoint and endPoint----------------------%
%%node = [point,goal_flag,cost,min_parent_idx]
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];%----------------establish tree starting with the start node------------%
tree = start_node;
a = clock;%--------check to see if start_node connects directly to end_node-------%
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...&&(collision(start_node,end_node,world,dim)==0) )path = [start_node; end_node];%------------Make randPoint as newPoint,Constant iterative-------------%
elsenIterations = 0;numPaths = 0;flag = 0;while numPaths<1,[tree,flag] = extendTree(tree,end_node,segmentLength,world,dim);  %%每次生成随机节点，从树中最近点拓展到该随机节点，并作为树中的新节点numPaths = numPaths + flag;nIterations = nIterations+1;end
end%-----------------find Minimum Path------------------------------------%
path = findMinimumPath(tree,end_node,dim);
sizePath = size(path,1);b = clock;%-----------------calculate Simulation time-----------------------------------%
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1
% find path with minimum cost to end_node
figure;
plotExpandedTree(world,tree,dim);
plotWorld(world,path,dim);end
end%%%*******************************生成随机障碍物地图***************************************%%%%
function world = createWorld(NumObstacles, endcorner, origincorner,dim)
% endcorner - 地图右上角的坐标，即终点坐标
% oringincorner - 地图左下角的坐标，即初始点的坐标
% NumObstacles - 随机障碍物的个数if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles maxlength = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2)); %返回最小的坐标长度maxlength = maxlength/NumObstacles;for i=1:NumObstacles,% randomly pick radiusworld.length(i) = maxlength*rand;world.wide(i) = maxlength*rand;% randomly pick center of obstaclescx = origincorner(1) + world.length(i)...+ (endcorner(1)-origincorner(1)-2*world.length(i))*rand;cy = origincorner(2) + world.wide(i)...+ (endcorner(2)-origincorner(2)-2*world.wide(i))*rand;world.cx(i) = cx;world.cy(i) = cy;% randomly pick length\wide\highendendelseif dim ==3;% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];maxlength = min(bounds);maxlength = 2*maxlength/NumObstacles;for i=1:NumObstacles,% randomly pick length\wide\highworld.length(i) = maxlength*rand;world.wide(i) = maxlength*rand;world.high(i) = maxlength*rand;% randomly pick center of obstaclescx = origincorner(1) + world.length(i)...+ (endcorner(1)-origincorner(1)-2*world.length(i))*rand;cy = origincorner(2) + world.wide(i)...+ (endcorner(2)-origincorner(2)-2*world.wide(i))*rand;cz = origincorner(2) + world.high(i)...+ (endcorner(2)-origincorner(2)-2*world.high(i))*rand;world.cx(i) = cx;world.cy(i) = cy;world.cz(i) = cz;endendend
end%%%*******************************生成已知障碍物地图***************************************%%%%
function [world,NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;if dim == 2% check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)),disp('Not valid corner specifications!')world=[];  % create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles length = 0.4;wide = 0.6;world.length(1) = length;world.wide(1) = wide;cx = 50;cy = 50;world.cx(1) = cx;world.cy(1) = cy;endelseif dim == 3NumObstacles = 2;  % check to make sure that the region is nonemptyif (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)),disp('Not valid corner specifications!')world=[];% create world data structureelseworld.NumObstacles = NumObstacles;world.endcorner = endcorner;world.origincorner = origincorner;% create NumObstacles length(1) = 0.4;wide(1) = 0.6;high(1) = 0.27;world.length(1) = length(1);world.wide(1) = wide(1);world.high(1) = high(1);cx = 0.3;cy = 0;cz = 0.135;world.cx(1) = cx;world.cy(1) = cy;world.cz(1) = cz;length(2) = 0.1;wide(2) = 0.2;high(2) = 0.52;world.length(2) = length(2);world.wide(2) = wide(2);world.high(2) = high(2);cx = 0;cy = 0;cz = 0.26;world.cx(2) = cx;world.cy(2) = cy;world.cz(2) = cz;endend
end%%%*******************************检测是否冲突***************************************%%%%
function collision_flag = collision(node, parent, world,dim)
% node - 节点端点
% parent - 另一节点端点collision_flag = 0;%是否超出地图范围
for i=1:dimif (node(i)>world.endcorner(i))||(node(i)<world.origincorner(i))collision_flag = 1;end
end%是否在障碍物的范围内
if collision_flag == 0 && dim ==2for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2)),%%求矩阵范数，即节点在障碍物范围内collision_flag = 1;break;endendendelseif collision_flag == 0 && dim ==3for sigma = 0:.2:1,p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);% check each obstaclefor i=1:world.NumObstacles,if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2) && (abs(p(3)-world.cz(i))<=world.high(i)/2)),collision_flag = 1;break;endendend
end
end%%%*******************************检测节点是否可用***************************************%%%%
function collision_flag = is_point_valid(point, world,dim)collision_flag = 0;%是否超出地图范围
for i=1:dimif (point(i)>world.endcorner(i))||(point(i)<world.origincorner(i))collision_flag = 1;end
end
%是否在障碍物的范围内
if collision_flag == 0 && dim ==2p = point(1:dim);% check each obstaclefor i=1:world.NumObstacles,if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2)),collision_flag = 1;break;endend
elseif collision_flag == 0 && dim ==3p = point(1:dim);% check each obstaclefor i=1:world.NumObstacles,if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2) && (abs(p(3)-world.cz(i))<=world.high(i)/2)),collision_flag = 1;break;endend
end
end%%%****************************从工作空间中随机选择节点**************************************%%%%
function point=ChooseRandPointload('RHandPos_3Freedom.mat');[m,n]=size(HandPos);randIndex = randperm(m,1);point=[HandPos(randIndex,1),HandPos(randIndex,2),HandPos(randIndex,3)];
end%%%*******************************树节点拓展***************************************%%%%
function [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,dim)flag = 0;point_valid_flag = 1;% select a random pointrandomPoint = zeros(1,dim);% whether randomPoint is validwhile point_valid_flag ==1
%         for i=1:dim
%            randomPoint(i) = (world.endcorner(i)-world.origincorner(i))*rand+world.origincorner(i); %%%在地图中产生随机节点
%         endrandomPoint = ChooseRandPoint;point_valid_flag = is_point_valid(randomPoint, world,dim);end% find leaf on node that is closest to randomPoint% -选择tree中节点和随机节点randomPoint的欧式距离最小的点为 newpoint（最近节点nearPoint）tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;  sqrd_dist = sqr_eucl_dist(tmp,dim);[min_dist,idx] = min(sqrd_dist);min_parent_idx = idx;new_point = tree(idx,1:dim);  new_node = tree(idx,:);pflag = 0;%%%从树节点中的nearPoint沿randomPoint方向，不断向randomPoint拓展，直到到达randomPoint为止while norm(new_point-randomPoint)>0 && pflag==0  %随机节点和上一个新节点（nearPoint）的距离小于segmentLength时，将随机节点取为下一个新的节点if norm(new_point-randomPoint)<segmentLengthpflag = collision(randomPoint,tree(min_parent_idx,:),world,dim);if pflag == 0new_point = randomPoint;min_cost = cost_np(tree(min_parent_idx,:),new_point,dim);%%计算上一个newPoint（nearPoint）到下一个newPoint（randPoint）的代价值，new_node = [new_point,0,min_cost,min_parent_idx];%%min_cost为从树的主节点到最终选择的newPoint的代价值累加和tree = [tree;new_node]; %%增加新的树节点pflag = 1;goal_flag = is_goal(new_node,end_node,segmentLength,world,dim);if goal_flag == 1;tree(end,dim+1)=1;flag = 1;endend  %随机节点和上一个新节点（nearPoint）的距离大于segmentLength时，在上一个节点沿随机节点方向取步长为segmentLength的节点为下一个新节点elsenew_point = (randomPoint-tree(min_parent_idx,1:dim));new_point = tree(min_parent_idx,1:dim)+(new_point/norm(new_point))*segmentLength;min_cost  = cost_np(tree(min_parent_idx,:),new_point,dim);new_node  = [new_point, 0, min_cost, min_parent_idx];pflag = collision(new_node,tree(min_parent_idx,:),world,dim);if pflag == 0tree = [tree ; new_node];min_parent_idx = size(tree,1);goal_flag = is_goal(new_node,end_node,segmentLength,world,dim);if goal_flag == 1;    tree(end,dim+1)=1;  % mark node as connecting to end.pflag = 1;flag = 1;endend    endendnew_tree = tree;
end%%%*******************是否到达目标**********************************%%%%
function goal_flag = is_goal(node,end_node,segmentLength,world,dim)   goal_flag = 0; if (norm(node(1:dim)-end_node(1:dim))<segmentLength )...&& (collision(node,end_node,world,dim)==0)goal_flag = 1;end
end%%%*******************计算各个节点之间的欧式距离（矩阵中向量的二范数）**********************************%%%%
function e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);%array中元素平方值
for i=1:dim   sqr_e_dist(:,i) = array(:,i).*array(:,i);
end
e_dist = zeros(size(array,1),1);
for i=1:dime_dist = e_dist+sqr_e_dist(:,i);
end
end%%%*******************用树所有节点到另一节点坐标的范数值作为节点间的代价值**********************************%%%%
%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;end%%%*******************找到所有树节点到目标点的最短路径**********************************%%%%
function path = findMinimumPath(tree,end_node,dim)% find nodes that connect to end_nodeconnectingNodes = [];for i=1:size(tree,1),if tree(i,dim+1)==1,connectingNodes = [connectingNodes ; tree(i,:)];endend% find minimum cost last node[tmp,idx] = min(connectingNodes(:,dim+2));% construct lowest cost pathpath = [connectingNodes(idx,:); end_node];parent_node = connectingNodes(idx,dim+3);while parent_node>1,parent_node = tree(parent_node,dim+3);path = [tree(parent_node,:); path];endendfunction plotExpandedTree(world,tree,dim)
ind = size(tree,1);while ind>0size(tree);branch = [];node = tree(ind,:);branch = [ branch ; node ];parent_node = node(dim+3);while parent_node > 1cur_parent = parent_node;branch = [branch; tree(parent_node,:)];parent_node = tree(parent_node,dim+3);endind = ind - 1;if dim == 2X = branch(:,1);Y = branch(:,2);p = plot(X,Y);set(p,'Color','r','LineWidth',0.5,'Marker','*','MarkerEdgeColor','m');hold on;  elseif dim == 3X = branch(:,1);Y = branch(:,2);Z = branch(:,3);p = plot3(X,Y,Z);set(p,'Color','r','LineWidth',0.5,'Marker','*','MarkerEdgeColor','m');hold on;endend
endfunction plotWorld(world,path,dim)% the first element is the north coordinate% the second element is the south coordinateif dim ==2axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2)]);hold onfor i=1:world.NumObstaclesx0 = world.cx(i); y0 = world.cy(i); Lx = world.length(i); Ly = world.wide(i);x=[x0-Lx/2 x0-Lx/2 x0+Lx/2 x0+Lx/2];y=[y0-Ly/2 y0+Ly/2 y0+Ly/2 y0-Ly/2];fill(x,y,'blue');endX = path(:,1);Y = path(:,2);p = plot(X,Y);      elseif dim ==3axis([world.origincorner(1),world.endcorner(1),...world.origincorner(2), world.endcorner(2),...world.origincorner(3), world.endcorner(3)]);hold onfor i=1:world.NumObstacles%(x0,y0,z0)是中心点的位置; (Lx,Ly,Lz)是长方体的长宽高.x0 = world.cx(i); y0 = world.cy(i); z0 = world.cz(i);Lx = world.length(i); Ly = world.wide(i); Lz = world.high(i);x=[x0-Lx/2 x0-Lx/2 x0-Lx/2 x0-Lx/2 x0+Lx/2 x0+Lx/2 x0+Lx/2 x0+Lx/2];y=[y0-Ly/2 y0-Ly/2 y0+Ly/2 y0+Ly/2 y0-Ly/2 y0-Ly/2 y0+Ly/2 y0+Ly/2];z=[z0-Lz/2 z0+Lz/2 z0+Lz/2 z0-Lz/2 z0+Lz/2 z0-Lz/2 z0-Lz/2 z0+Lz/2];index=zeros(6,5);index(1,:)=[1 2 3 4 1];  %按一定顺序得到长方体角点的位置index(2,:)=[5 6 7 8 5];index(3,:)=[2 1 6 5 2];index(4,:)=[4 3 8 7 4];index(5,:)=[1 6 7 4 1];index(6,:)=[8 5 2 3 8];for k=1:6plot3(x(index(k,:)),y(index(k,:)),z(index(k,:)),'r');fill3(x(index(k,:)),y(index(k,:)),z(index(k,:)),'c');  %填充多边形函数hold onendendX = path(:,1)Y = path(:,2)Z = path(:,3)p = plot3(X,Y,Z);endset(p,'Color','black','LineWidth',3)xlabel('X axis');ylabel('Y axis');zlabel('Z axis');title('RRT Connect Algorithm');
end