RRT、RRTConnect、RRT*——Matlab算法

1.RRT

RRT算法倾向于拓展到开放的未探索区域,只要时间足够,迭代次数足够多,没有不会被探索到的区域。

RRT、RRTConnect、RRT*——Matlab算法_第1张图片

 

RRT、RRTConnect、RRT*——Matlab算法_第2张图片

RRT、RRTConnect、RRT*——Matlab算法_第3张图片

2.RRT-Connect

RRT-Connect算法:基于RRT搜索空间的盲目性,节点拓展环节缺乏记忆性的缺点,为了提高空间内的搜索速。在RRT算法的基础上加上了两棵树双向抖索的引导策略,并且在生长方式的基础上加上了贪婪策略加快了搜索速度,并且减少了空白区域的无用搜索,节省了搜索时间。

RRT、RRTConnect、RRT*——Matlab算法_第4张图片

RRT、RRTConnect、RRT*——Matlab算法_第5张图片

RRT、RRTConnect、RRT*——Matlab算法_第6张图片

3.RRT*算法

RRT-Connect算法增加了启发式策略,以及贪婪思想,但RRT算法和RRT-Connect算法的共同缺点是,他们的路径都不是最优的,没有添加评价路径长短花费的函数,搜索路径策略都是基于随机采样的搜索。渐进最优的RRT*算法,该算法在原有的RRT算法上,改进了父节点选择的方式,采用代价函数来选取拓展节点领域内最小代价的节点为父节点,同时,每次迭代后都会重新连接现有树上的节点,从而保证计算的复杂度和渐进最优解。(如:基于高斯采样策略的RRT*算法)

RRT、RRTConnect、RRT*——Matlab算法_第7张图片

 

RRT、RRTConnect、RRT*——Matlab算法_第8张图片

RRT、RRTConnect、RRT*——Matlab算法_第9张图片

4.代码

代码的原地址为:https://github.com/adnanmunawar/matlab-rrt-variants

代码中包含了:RRT-Connect、LazyRRT、RRTextend、RRT*的2D和3D算法

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  : [email protected] ; [email protected]
%MS Robotics, Worcester Polytechnic Institute

function benchmarkRRT

clc;
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 == 1
    time = 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;
        end
        
    str1 = ['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];

    end
    


    if run_RRTstar == 1
    time = 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;
        end
        
    str1 = ['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];
   
    end
    
    if run_RRTextend == 1
    time = 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;
        end
        
    str1 = ['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];
   
    end

    if run_RRTconnect == 1

    time = 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;
        end
        
    str1 = ['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
end

if show_benchmark_results == 1

    figure;
    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_connect
    
end
main 函数
%Author : Adnan Munawar
%Email  : [email protected] ; [email protected]
%MS Robotics, Worcester Polytechnic Institute

function [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];

else

start_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-------------%
else
  nIterations = 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 nonempty
  if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  
  elseif dim ==3;
  % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  end
end

%%%*******************************生成已知障碍物地图***************************************%%%%
function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
  if dim == 2
  % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
      disp('Not valid corner specifications!')
      world=[];  
  % create world data structure
    else
    world.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;
    end
  
  elseif dim == 3
      
    NumObstacles = 9;  
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
      disp('Not valid corner specifications!')
      world=[];
      
    % create world data structure
    else
    world.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;
     end
   end
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];
end

elseif 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:dim
   if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
       collision_flag = 1;
   end
end

%是否在障碍物的范围内
if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for i=1:world.NumObstacles,
        if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),%%求矩阵范数,即节点在障碍物范围内
            collision_flag = 1;
            break;
        end
      end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for 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;
        end
      end
    end
end
end


%%%*******************************检测节点是否可用***************************************%%%%
function collision_flag = is_point_valid(point, world,dim)

collision_flag = 0;

%是否超出地图范围
for i=1:dim
   if (point(i)>world.endcorner(i))||(point(i)<world.origincorner(i))
       collision_flag = 1;
   end
end
%是否在障碍物的范围内
if collision_flag == 0 && dim ==2
    p = point(1:dim);
      % check each obstacle
      for i=1:world.NumObstacles,
        if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
            collision_flag = 1;
            break;
        end
      end

elseif collision_flag == 0 && dim ==3
    p = point(1:dim);
      % check each obstacle
      for 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;
        end
      end
end
end






%%%*******************************检测最后一个节点是否需要**********************************%%%%
function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
  flag = 0;
  % check only last node added to tree since others have been checked
  if ( (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 point
    randomPoint = zeros(1,dim);
    for i=1:dim
       randomPoint(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)<segmentLength
            pflag = collision(randomPoint,tree(min_parent_idx,:),world,dim);
            
            if pflag == 0
                new_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;
                end
            end  
        
      %随机节点和上一个新节点(nearPoint)的距离大于segmentLength时,在上一个节点沿随机节点方向取步长为segmentLength的节点为下一个新节点
        else
            new_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 == 0
                tree = [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;
                end
            end    
        end
    end
    new_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:dim
    e_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;

end

function [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_node
    connectingNodes = [];
    for i=1:size(tree,1),
        if tree(i,dim+1)==1,
            connectingNodes = [connectingNodes ; tree(i,:)];
        end
    end

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,dim+2));
    
    % construct lowest cost path
    path = [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];
    end
    
end


function plotExpandedTree(world,tree,dim)
    ind = size(tree,1);
    while ind>0
        size(tree);
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
        while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
        end
        ind = ind - 1;
        
        if dim == 2
        X = branch(:,1);
        Y = branch(:,2);
        
        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;  
        
        elseif dim == 3
        X = 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;
        end
    end
end




function plotWorld(world,path,dim)
  % the first element is the north coordinate
  % the second element is the south coordinate
  if dim ==2
      
  N = 10;
  th = 0:2*pi/N:2*pi;
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2)]);
  hold on
  
  for 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');
  end
  
  X = path(:,1);
  Y = path(:,2);
  p = plot(X,Y);      
      
  elseif dim ==3
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2),...
      world.origincorner(3), world.endcorner(3)]);
  hold on
  
  for 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]);
  end
  
  X = path(:,1);
  Y = path(:,2);
  Z = path(:,3);
  p = plot3(X,Y,Z);
  end
  set(p,'Color','black','LineWidth',3)
  xlabel('X axis');
  ylabel('Y axis');
  zlabel('Z axis');
  title('RRT Connect Algorithm');
end
RRT-Connect
%Author : Adnan Munawar
%Email  : [email protected] ; [email protected]
%MS Robotics, Worcester Polytechnic Institute

function [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];

else
   
start_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];
else
  nIterations = 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 == 1

figure;
plotExpandedTree(world,tree,dim);
plotWorld(world,path,dim);
% figure(2);
% plotWorld(world,path,dim);
%plotExpandedTree(world,tree,dim);
end
end




function 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 == 0
        start_collision_node = LazyPath(i,:);
        start_flag = 1;
        end_flag = 1;
        breakage_from = i;
    
    elseif end_flag == 1 && cflag == 0
        
        end_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);      
        end
        repaired_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,:)];
        
    end
    

end

end



function world = createWorld(NumObstacles, endcorner, origincorner,dim)

  if dim == 2

    % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  
  elseif dim ==3;
  % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  end
end

function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
  if dim == 2
  % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
      disp('Not valid corner specifications!')
      world=[];  
  % create world data structure
    else
    world.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;
    end
  
  elseif dim == 3
      
    NumObstacles = 9;  
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
      disp('Not valid corner specifications!')
      world=[];
      
    % create world data structure
    else
    world.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;
     end
   end
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];
end

elseif 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)

collision_flag = 0;


for i=1:dim
   if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
       collision_flag = 1;
   end
end

if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for i=1:world.NumObstacles,
        if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
            collision_flag = 1;
            break;
        end
      end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for 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;
        end
      end
    end
end
end







function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
  flag = 0;
  % check only last node added to tree since others have been checked
  if ( (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;
  flag1 = 0;
  while flag1==0,
    % select a random point
    randomPoint = ones(1,dim);
    for i=1:dim
       randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
    end

    % find leaf on node that is closest to randomPoint
    tmp = 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)==0
        
        new_tree = [tree;new_node];
        flag1 = 1;
    else
      flag1=0;
    end
  end
  
    % check to see if new node connects directly to end_node
    if ( (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
end





function [new_tree,flag] = extendLazyTree(tree,end_node,segmentLength,world,dim)

    % select a random point
    randomPoint = ones(1,dim);
    for i=1:dim
       randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
    end

    % find leaf on node that is closest to randomPoint
    tmp = 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_node
    if ( (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.
    else
    flag = 0;
    end
    
end










function e_dist = sqr_eucl_dist(array,dim)

sqr_e_dist = zeros(size(array,1),dim);
for i=1:dim
   
    sqr_e_dist(:,i) = array(:,i).*array(:,i);
    
end
e_dist = zeros(size(array,1),1);
for i=1:dim
   
    e_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;

end

function [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_node
    connectingNodes = [];
    for i=1:size(tree,1),
        if tree(i,dim+1)==1,
            connectingNodes = [connectingNodes ; tree(i,:)];
        end
    end

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,dim+2));
    
    % construct lowest cost path
    path = [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];
    end
    
end



function plotExpandedTree(world,tree,dim)
    ind = size(tree,1);
    while ind>0
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
        while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
        end
        ind = ind - 1;
        
        if dim == 2
        X = branch(:,1);
        Y = branch(:,2);
        
        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;  
        
        elseif dim == 3
        X = 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;
        end
    end
end




function plotWorld(world,path,dim)
  % the first element is the north coordinate
  % the second element is the south coordinate
  if dim ==2
      
  N = 10;
  th = 0:2*pi/N:2*pi;
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2)]);
  hold on
  
  for 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');
  end
  
  X = path(:,1);
  Y = path(:,2);
  p = plot(X,Y);      
      
  elseif dim ==3
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2),...
      world.origincorner(3), world.endcorner(3)]);
  hold on
  
  for 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]);
  end
  
  X = path(:,1);
  Y = path(:,2);
  Z = path(:,3);
  p = plot3(X,Y,Z);
  end
  set(p,'Color','black','LineWidth',3)
  xlabel('X axis');
  ylabel('Y axis');
  zlabel('Z axis');
  title('Lazy RRT Algorithm');
end
LazyRRT
%Author : Adnan Munawar
%Email  : [email protected] ; [email protected]
%MS Robotics, Worcester Polytechnic Institute

function [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];

else
   
start_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];
else
    
  its = 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
end





function world = createWorld(NumObstacles, endcorner, origincorner,dim)

  if dim == 2

    % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  
  elseif dim ==3;
  % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  end
end

function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
  if dim == 2
  % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
      disp('Not valid corner specifications!')
      world=[];  
  % create world data structure
    else
    world.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;
    end
  
  elseif dim == 3
      
    NumObstacles = 9;  
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
      disp('Not valid corner specifications!')
      world=[];
      
    % create world data structure
    else
    world.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;
     end
   end
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];
end

elseif 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)

collision_flag = 0;


for i=1:dim
   if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
       collision_flag = 1;
   end
end

if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for i=1:world.NumObstacles,
        if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
            collision_flag = 1;
            break;
        end
      end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for 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;
        end
      end
    end
end
end







function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
  flag = 0;
  % check only last node added to tree since others have been checked
  if ( (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,flag_chk,dim)
  r = 0;
  flag1 = 0;
  while flag1==0,
    % select a random point
    randomPoint = ones(1,dim);
    for i=1:dim
       randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
    end

    % find leaf on node that is closest to randomPoint
    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 = (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)==0
        
      tmp_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)>1
      size_near = size(near_idx,1);
      
        for i = 1:size_near
            if collision(new_node, tree(near_idx(i),:), world,dim)==0
                
               cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim);
        
                if  cost_near < min_cost
                    min_cost = cost_near;
                    min_parent_idx = near_idx(i);
                end
        
            end
        end
      end
      
      new_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)>1
      reduced_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)
            end
          
        end
      end
      flag1=1;
    end
  end
  
  
  if flag_chk == 0
    % check to see if new node connects directly to end_node
    if ( (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.
    else
    flag = 0;
    end
    
  else flag = 1;
  end
end


function e_dist = sqr_eucl_dist(array,dim)

sqr_e_dist = zeros(size(array,1),dim);
for i=1:dim
   
    sqr_e_dist(:,i) = array(:,i).*array(:,i);
    
end
e_dist = zeros(size(array,1),1);
for i=1:dim
   
    e_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;

end

function [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_node
    connectingNodes = [];
    for i=1:size(tree,1),
        if tree(i,dim+1)==1,
            connectingNodes = [connectingNodes ; tree(i,:)];
        end
    end

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,dim+2));
    
    % construct lowest cost path
    path = [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];
    end
    
end


function plotExpandedTree(world,tree,dim)
    ind = size(tree,1);
    while ind>0
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
        while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
        end
        ind = ind - 1;
        
        if dim == 2
        X = branch(:,1);
        Y = branch(:,2);
        
        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;  
        
        elseif dim == 3
        X = 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;
        end
    end
end




function plotWorld(world,path,dim)
  % the first element is the north coordinate
  % the second element is the south coordinate
  if dim ==2
      
  N = 10;
  th = 0:2*pi/N:2*pi;
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2)]);
  hold on
  
  for 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');
  end
  
  X = path(:,1);
  Y = path(:,2);
  p = plot(X,Y);      
      
  elseif dim ==3
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2),...
      world.origincorner(3), world.endcorner(3)]);
  hold on
  
  for 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]);
  end
  
  X = path(:,1);
  Y = path(:,2);
  Z = path(:,3);
  p = plot3(X,Y,Z);
  end
  set(p,'Color','black','LineWidth',3)
  xlabel('X axis');
  ylabel('Y axis');
  zlabel('Z axis');
  title('RRT Extend Algorithm');
end
RRTextend
%Author : Adnan Munawar
%Email  : [email protected] ; [email protected]
%MS Robotics, Worcester Polytechnic Institute

function [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];

else
   
start_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];
else
     
  if 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*2
      tree_1000 = tree;
      elseif its == draw*3
      tree_1500 = tree;
      elseif its == draw*4
      tree_2000 = tree;
      elseif its == draw*5
      tree_2500 = tree;
      elseif its == draw*6
      tree_3000 = tree;
      elseif its == draw*7
      tree_3500 = tree;
      elseif its == draw*8
      tree_4000 = tree;    
      end
  end
  
  else
  its = 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
end





function world = createWorld(NumObstacles, endcorner, origincorner,dim)

  if dim == 2

    % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  
  elseif dim ==3;
  % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  end
end

function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
  if dim == 2
  % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
      disp('Not valid corner specifications!')
      world=[];  
  % create world data structure
    else
    world.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;
    end
  
  elseif dim == 3
      
    NumObstacles = 9;  
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
      disp('Not valid corner specifications!')
      world=[];
      
    % create world data structure
    else
    world.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;
     end
   end
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];
end

elseif 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)

collision_flag = 0;


for i=1:dim
   if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
       collision_flag = 1;
   end
end

if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for i=1:world.NumObstacles,
        if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
            collision_flag = 1;
            break;
        end
      end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for 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;
        end
      end
    end
end
end







function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
  flag = 0;
  % check only last node added to tree since others have been checked
  if ( (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,r,world,flag_chk,dim)

  flag1 = 0;
  while flag1==0,
    % select a random point
    randomPoint = ones(1,dim);
    for i=1:dim
       randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
    end

    % find leaf on node that is closest to randomPoint
    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 = (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)==0
        
      tmp_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)>1
      size_near = size(near_idx,1);
      
        for i = 1:size_near
            if collision(new_node, tree(near_idx(i),:), world,dim)==0
                
               cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim);
        
                if  cost_near < min_cost
                    min_cost = cost_near;
                    min_parent_idx = near_idx(i);
                end
        
            end
        end
      end
      
      new_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)>1
      reduced_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)
            end
          
        end
      end
      flag1=1;
    end
  end
  
  
  if flag_chk == 0
    % check to see if new node connects directly to end_node
    if ( (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.
    else
    flag = 0;
    end
    
  else flag = 1;
  end
end


function e_dist = sqr_eucl_dist(array,dim)

sqr_e_dist = zeros(size(array,1),dim);
for i=1:dim
   
    sqr_e_dist(:,i) = array(:,i).*array(:,i);
    
end
e_dist = zeros(size(array,1),1);
for i=1:dim
   
    e_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;

end

function [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_node
    connectingNodes = [];
    for i=1:size(tree,1),
        if tree(i,dim+1)==1,
            connectingNodes = [connectingNodes ; tree(i,:)];
        end
    end

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,dim+2));
    
    % construct lowest cost path
    path = [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];
    end
    
end


function plotExpandedTree(world,tree,dim)
    ind = size(tree,1);
    while ind>0
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
        while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
        end
        ind = ind - 1;
        
        if dim == 2
        X = branch(:,1);
        Y = branch(:,2);
        
        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;  
        
        elseif dim == 3
        X = 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;
        end
    end
end




function plotWorld(world,path,dim)
  % the first element is the north coordinate
  % the second element is the south coordinate
  if dim ==2
      
  N = 10;
  th = 0:2*pi/N:2*pi;
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2)]);
  hold on
  
  for 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');
  end
  
  X = path(:,1);
  Y = path(:,2);
  p = plot(X,Y);      
      
  elseif dim ==3
  axis([world.origincorner(1),world.endcorner(1),...
      world.origincorner(2), world.endcorner(2),...
      world.origincorner(3), world.endcorner(3)]);
  hold on
  
  for 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]);
  end
  
  X = path(:,1);
  Y = path(:,2);
  Z = path(:,3);
  p = plot3(X,Y,Z);
  end
  set(p,'Color','black','LineWidth',3)
  xlabel('X axis');
  ylabel('Y axis');
  zlabel('Z axis');
  title('RRT Star Algorithm');
end
RRT*

 修改后:

将RRT-Connect代码中的障碍物由圆形修改为长方体,并且将随机节点改为从工作空间中随机选择节点,保证求解的可用性。

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:stepsize
    segmentLength = 0.1;  
    if run_RRTconnect == 1
    time = 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;
    end
        
    str1 = ['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
end
       
end
main 函数
%Author : Adnan Munawar
%Email  : [email protected] ; [email protected]
%MS Robotics, Worcester Polytechnic Institute

function [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];

else

start_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-------------%
else
  nIterations = 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 nonempty
  if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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 radius
        world.length(i) = maxlength*rand;
        world.wide(i) = maxlength*rand;
        % randomly pick center of obstacles
        cx = 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\high
    end
  end
  
  elseif dim ==3;
  % check to make sure that the region is nonempty
  if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    world.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\high
        world.length(i) = maxlength*rand;
        world.wide(i) = maxlength*rand;
        world.high(i) = maxlength*rand;
        % randomly pick center of obstacles
        cx = 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;
    end
  end
  end
end

%%%*******************************生成已知障碍物地图***************************************%%%%
function [world,NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
  if dim == 2
  % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)),
      disp('Not valid corner specifications!')
      world=[];  
  % create world data structure
    else
    world.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;
    end
  
  elseif dim == 3
      
    NumObstacles = 2;  
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)),
      disp('Not valid corner specifications!')
      world=[];
      
    % create world data structure
    else
        world.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;
     end
   end
end


%%%*******************************检测是否冲突***************************************%%%%
function collision_flag = collision(node, parent, world,dim)
% node - 节点端点
% parent - 另一节点端点

collision_flag = 0;

%是否超出地图范围
for i=1:dim
   if (node(i)>world.endcorner(i))||(node(i)<world.origincorner(i))
       collision_flag = 1;
   end
end

%是否在障碍物的范围内
if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for 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;
        end
      end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
    p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
      % check each obstacle
      for 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;
        end
      end
    end
end
end


%%%*******************************检测节点是否可用***************************************%%%%
function collision_flag = is_point_valid(point, world,dim)

collision_flag = 0;

%是否超出地图范围
for i=1:dim
   if (point(i)>world.endcorner(i))||(point(i)<world.origincorner(i))
       collision_flag = 1;
   end
end
%是否在障碍物的范围内
if collision_flag == 0 && dim ==2
    p = point(1:dim);
      % check each obstacle
      for 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;
        end
      end
elseif collision_flag == 0 && dim ==3
    p = point(1:dim);
      % check each obstacle
      for 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;
        end
      end
end
end


%%%****************************从工作空间中随机选择节点**************************************%%%%
function point=ChooseRandPoint
    load('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 point
    randomPoint = zeros(1,dim);
    
    % whether randomPoint is valid
    while point_valid_flag ==1
%         for i=1:dim
%            randomPoint(i) = (world.endcorner(i)-world.origincorner(i))*rand+world.origincorner(i); %%%在地图中产生随机节点        
%         end
        randomPoint = 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)<segmentLength
            pflag = collision(randomPoint,tree(min_parent_idx,:),world,dim);
            
            if pflag == 0
                new_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;
                end
            end  
        
      %随机节点和上一个新节点(nearPoint)的距离大于segmentLength时,在上一个节点沿随机节点方向取步长为segmentLength的节点为下一个新节点
        else
            new_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 == 0
                tree = [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;
                end
            end    
        end
    end
    new_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:dim
    e_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_node
    connectingNodes = [];
    for i=1:size(tree,1),
        if tree(i,dim+1)==1,
            connectingNodes = [connectingNodes ; tree(i,:)];
        end
    end

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,dim+2));
    
    % construct lowest cost path
    path = [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];
    end
    
end


function plotExpandedTree(world,tree,dim)
ind = size(tree,1);
    while ind>0
        size(tree);
        branch = [];
        node = tree(ind,:);
        branch = [ branch ; node ];
        parent_node = node(dim+3);
        while parent_node > 1
            cur_parent = parent_node;
            branch = [branch; tree(parent_node,:)];
            parent_node = tree(parent_node,dim+3);
         end
         ind = ind - 1;
        
         if dim == 2
            X = branch(:,1);
            Y = branch(:,2);

            p = plot(X,Y);
            set(p,'Color','r','LineWidth',0.5,'Marker','*','MarkerEdgeColor','m');
            hold on;  

            elseif dim == 3
            X = 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;
         end
    end
end




function plotWorld(world,path,dim)
  % the first element is the north coordinate
  % the second element is the south coordinate

    if dim ==2
        axis([world.origincorner(1),world.endcorner(1),...
          world.origincorner(2), world.endcorner(2)]);
        hold on
        for i=1:world.NumObstacles
            x0 = 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');
        end
        X = path(:,1);
        Y = path(:,2);
        p = plot(X,Y);      
        
    elseif dim ==3
        axis([world.origincorner(1),world.endcorner(1),...
          world.origincorner(2), world.endcorner(2),...
          world.origincorner(3), world.endcorner(3)]);
        hold on

        for 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:6
                plot3(x(index(k,:)),y(index(k,:)),z(index(k,:)),'r');
                fill3(x(index(k,:)),y(index(k,:)),z(index(k,:)),'c');  %填充多边形函数
            hold on
            end
        end
        X = path(:,1)
        Y = path(:,2)
        Z = path(:,3)
        p = plot3(X,Y,Z);
    end
    set(p,'Color','black','LineWidth',3)
    xlabel('X axis');
    ylabel('Y axis');
    zlabel('Z axis');
    title('RRT Connect Algorithm');
end
RRT-Connect

 


转载于:https://www.cnblogs.com/guojun-junguo/p/10198144.html

你可能感兴趣的:(RRT、RRTConnect、RRT*——Matlab算法)