├── README.md ├── project code ├── RRTstar final │ ├── GB_RRT_star_final.m │ ├── RRT_final.m │ ├── RRT_star_final.m │ ├── addtreenode.m │ ├── collision.m │ ├── createfinalpath.m │ ├── eudist.m │ ├── gbsteerfn.m │ └── steerfn.m ├── RRTtest │ ├── GB_Gaussian_RRT_star_final_test.m │ ├── GB_RRT_star_final.m │ ├── GB_RRT_star_final_test.m │ ├── RRT_final.m │ ├── RRT_nocollision.m │ ├── RRT_nocollision_twotree.m │ ├── RRT_rewire.mlx │ ├── RRT_star_final.m │ ├── RRT_star_final_test.m │ ├── addtreenode.m │ ├── addxyz.m │ ├── collision.m │ ├── createfinalpath.m │ ├── eudist.m │ ├── gaussian.mlx │ ├── gaussian_test.m │ ├── gaussianwiki.m │ ├── gbsteerfn.m │ ├── html │ │ ├── RRT_nocollision.html │ │ ├── RRT_nocollision.png │ │ ├── RRT_nocollision_01.png │ │ ├── RRT_nocollision_02.png │ │ └── RRT_nocollision_03.png │ ├── nocollision.mlx │ ├── num2cell_cell2mat.mlx │ ├── pathfinish.m │ ├── samplepoint.m │ ├── startup.m │ ├── steerfn.m │ ├── test.mlx │ ├── test2.mlx │ ├── test3.mlx │ ├── test4.mlx │ ├── test5.mlx │ ├── test_graph.m │ ├── testwith_1500_20_nerrowpassage.fig │ └── whileloop.m └── readme.txt └── update on thesis with sign page.pdf /README.md: -------------------------------------------------------------------------------- 1 | # RRT-using-Matlab 2 | Path Planning Using Goal Biased Gaussian Distribution Based RRT* 3 | -------------------------------------------------------------------------------- /project code/RRTstar final/GB_RRT_star_final.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [150,500]; 21 | qgoal = [850,500]; 22 | iterations = 2000; 23 | testidealdist = eudist(qstart,qgoal); 24 | disp(testidealdist) 25 | %define OBSTACLE 26 | 27 | %OBSTACLE 1 28 | obs1.x=[400 600]; 29 | obs1.y=[300 600]; 30 | 31 | %OBSTACLE 2 32 | %obs2.x=[]; 33 | %obs2.y=[]; 34 | obs2.x = [0 0]; 35 | obs2.y = [0 0]; 36 | 37 | %create obstacle1 38 | x1box=obs1.x([1 1 2 2 1]); 39 | y1box=obs1.y([1 2 2 1 1]); 40 | 41 | %create obstacle2 42 | x2box=obs2.x([1 1 2 2 1]); 43 | y2box=obs2.y([1 2 2 1 1]); 44 | 45 | obs1area = abs(obs1.x(1)-obs1.x(2))*abs(obs1.y(1)-obs1.y(2)); 46 | obs2area = abs(obs2.x(1)-obs2.x(2))*abs(obs2.y(1)-obs2.y(2)); 47 | 48 | totalarea = height*width; 49 | maxfree = abs(totalarea - (obs1area+obs2area)); 50 | ddimension = 2; 51 | unitballvolume = 2*pi; 52 | 53 | % define tree edge, node and vertice 54 | % 55 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 56 | % order 57 | 58 | vertecies = qstart; 59 | vert_count = 1; 60 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 61 | %vertecies.cost = 0; 62 | format longG 63 | eps=(height*3)/100; 64 | eps1=(height*6)/100; 65 | val = 0; 66 | cmin(vert_count,:)=0; 67 | finalcost = 0; 68 | finaltree = qstart; 69 | qnew_cost = 0; 70 | % 71 | % 72 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 73 | % because vertecies(1) is the original point 74 | 75 | edges.x = zeros(iterations,2); 76 | edges.y = zeros(iterations,2); 77 | edge_count = 0; 78 | %% 79 | % 80 | % 81 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 82 | % closetest vertex to vertecies(i+1) 83 | %% 84 | ind_nearest = zeros(iterations,1); 85 | qnew = [0 0]; 86 | colli = 0; 87 | i=1; 88 | randcount=1; 89 | %% 90 | % define variables for optimization 91 | 92 | 93 | %% 94 | % define figure and hold 95 | %% 96 | hold on; 97 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 98 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 99 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 100 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 101 | %plot(qstart(1),qstart(2),'r*'); 102 | %plot(qgoal(1),qgoal(2),'r*'); 103 | 104 | %figure('name', 'RRT growing'); 105 | tic; 106 | %% 107 | while i <= iterations 108 | 109 | x_rand = width*rand(); % random point generation 110 | y_rand = height*rand(); 111 | temp.rand= [x_rand,y_rand]; %save temporary random point 112 | randcount=randcount+1; 113 | % drawnow 114 | %scatter(x_rand,y_rand,30,'filled'); 115 | %if random point is goal point this condition is not consider in this 116 | %programme yet 117 | 118 | % find nearest point using KD-Tree method 119 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 120 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 121 | 122 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 123 | qnew_goal_cost = eudist(qgoal,qnearest); 124 | %find new point using steer funcrtion or BVP 125 | %find new point towards goal if no obstacle eps1>eps 126 | %if there is obstacle eps>eps1 127 | %else use only eps 128 | %A goabbiasing function 129 | colli_random = collision(x1box,y1box,x2box,y2box,[x_rand,y_rand],qnearest); 130 | if colli_random == 0 131 | %this is tem new point 132 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps,eps1); 133 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 134 | if colli == 1 135 | % valus of eps and eps1 are inter change to not stay in local minima 136 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps1,eps); 137 | colli1 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 138 | if colli1 == 1 139 | %use normal steer function 140 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 141 | colli2 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 142 | if colli2 == 1 143 | continue 144 | end 145 | end 146 | end 147 | else 148 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 149 | colli3 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 150 | if colli3 == 1 151 | continue 152 | end 153 | end 154 | 155 | 156 | 157 | %check collision 158 | %colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 159 | 160 | % if colli_check == 1 161 | % continue 162 | % end 163 | 164 | %calculate radius for rangesearch 165 | Rrrt = 2*((1+(1/ddimension))^(1/ddimension))*((maxfree/unitballvolume)^(1/ddimension)); 166 | dynamicradius = Rrrt*((log(vert_count)/vert_count)^(1/ddimension)); 167 | %optimization in edge developement 168 | 169 | qnew_cost = eudist(qnearest,qnew); 170 | try1 = abs((dynamicradius-eps)/2); 171 | qrange1=rangesearch(vertecies,qnew,max(dynamicradius,eps)); 172 | %qrange1=rangesearch(vertecies,qnew,try1); 173 | qrange=cell2mat(qrange1); 174 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 175 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 176 | qmin1 = qnearest; 177 | txx = length(qrange); 178 | %rewire process 179 | for k = 1:length(qrange) 180 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 181 | % tzxzz = qrange(k); 182 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 183 | if cnew < cmin1 184 | tzz = 1; 185 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 186 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 187 | cmin1 = cnew; 188 | ind_nearest(i) = qrange(k); 189 | end 190 | end 191 | end 192 | 193 | 194 | 195 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 196 | vertecies(vert_count+1,:) = qnew; 197 | 198 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 199 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 200 | edge_count = edge_count + 1; 201 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 202 | cmin(vert_count+1,:) = cmin1; 203 | 204 | for j = 1:length(qrange) 205 | cnear = cmin(qrange(j)); 206 | cmin2 = cmin1 + eudist(qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]); 207 | if cmin2 < cnear 208 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]) 209 | % ind_nearest(i) = vert_count+1; 210 | qnewrewire = [vertecies(qrange(j),1),vertecies(qrange(j),2)]; 211 | %vert_count_rewire = qrange(j); 212 | qtree(qrange(j),:) = addtreenode(qrange(j)-1,qtree,vert_count+1); 213 | cmin(qrange(j),:) = cmin2; 214 | % rw = cell2mat(qtree(qrange(j))); 215 | %rw1 = cell2mat(qtree(vert_count+1)); 216 | %rw1(length(rw1)+1) = qrange(j); 217 | end 218 | end 219 | end 220 | 221 | 222 | 223 | % find final path 224 | 225 | goalcost = eudist (qnew,qgoal); 226 | 227 | 228 | if goalcost <= eps 229 | finalcost = cmin(vert_count+1,:) + goalcost; 230 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 231 | optimumtolarance = ((testidealdist*15)/100)+testidealdist; 232 | 233 | %if finalcost <= optimumtolarance 234 | % disp(optimumtolarance) 235 | break; 236 | %end 237 | end 238 | 239 | hold on 240 | drawnow 241 | % scatter(vertecies(vert_count+1,1),vertecies(vert_count+1,2),3,'filled'); 242 | % plot([vertecies(ind_nearest(i),1),vertecies(vert_count+1,1)],[vertecies(ind_nearest(i),2),vertecies(vert_count+1,2)],'LineWidth',0.1,'Color','y'); 243 | % line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','g'); 244 | % scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 245 | % plot(edges.x', edges.y'); 246 | vert_count = vert_count + 1; 247 | i=i+1; 248 | 249 | end 250 | 251 | 252 | %if not find final route 253 | if finalcost == 0 254 | ind_nearest1 = knnsearch(vertecies,qgoal); 255 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 256 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 257 | if finalcolli == 0 258 | 259 | goalcost = eudist (qnearest,qgoal); 260 | finalcost = cmin(ind_nearest1,:) + goalcost; 261 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 262 | else 263 | disp('no nearby points') 264 | end 265 | end 266 | disp(finalcost) 267 | 268 | toc; 269 | clear i x_rand y_rand edge_rand 270 | disp(randcount) 271 | disp(vert_count) 272 | hold on 273 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 274 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 275 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 276 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 277 | scatter(vertecies(:,1), vertecies(:,2), 0.1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 278 | plot(edges.x', edges.y','LineWidth',0.01); 279 | line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','r'); 280 | 281 | -------------------------------------------------------------------------------- /project code/RRTstar final/RRT_final.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | axis ([0 width 0 height]); 14 | %% 15 | % 16 | % 17 | % define starting and end point and iteration 18 | %% 19 | qstart = [150,250]; 20 | qgoal = [750,550]; 21 | iterations = 2000; 22 | 23 | %define OBSTACLE 24 | 25 | %OBSTACLE 1 26 | obs1.x=[350 650]; 27 | obs1.y=[400 750]; 28 | 29 | %OBSTACLE 2 30 | %obs2.x=[]; 31 | %obs2.y=[]; 32 | obs2.x = [350 650]; 33 | obs2.y = [50 380]; 34 | 35 | %create obstacle1 36 | x1box=obs1.x([1 1 2 2 1]); 37 | y1box=obs1.y([1 2 2 1 1]); 38 | 39 | %create obstacle2 40 | x2box=obs2.x([1 1 2 2 1]); 41 | y2box=obs2.y([1 2 2 1 1]); 42 | 43 | 44 | % define tree edge, node and vertice 45 | % 46 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 47 | % order 48 | 49 | vertecies = qstart; 50 | vert_count = 1; 51 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 52 | %vertecies.cost = 0; 53 | format longG 54 | eps=15; 55 | val = 0; 56 | cmin(vert_count,:)=0; 57 | finalcost = 0; 58 | finaltree = qstart; 59 | qnew_cost = 0; 60 | % 61 | % 62 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 63 | % because vertecies(1) is the original point 64 | 65 | edges.x = zeros(iterations,2); 66 | edges.y = zeros(iterations,2); 67 | edge_count = 0; 68 | %% 69 | % 70 | % 71 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 72 | % closetest vertex to vertecies(i+1) 73 | %% 74 | ind_nearest = zeros(iterations,1); 75 | qnew = [0 0]; 76 | colli = 0; 77 | i=1; 78 | randcount=1; 79 | %% 80 | % define variables for optimization 81 | 82 | 83 | %% 84 | % define figure and hold 85 | %% 86 | hold on; 87 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 88 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 89 | scatter(qstart(1), qstart(2), 45, '*','r','LineWidth',1); hold on; 90 | scatter(qgoal(1), qgoal(2), 45, '*','r','LineWidth',1); hold on; 91 | 92 | %figure('name', 'RRT growing'); 93 | tic; 94 | %% 95 | while i <= iterations 96 | 97 | x_rand = width*rand(); % random point generation 98 | y_rand = height*rand(); 99 | temp.rand= [x_rand,y_rand]; %save temporary random point 100 | randcount=randcount+1; 101 | 102 | %if random point is goal point this condition is not consider in this 103 | %programme yet 104 | 105 | % find nearest point using KD-Tree method 106 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 107 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 108 | 109 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 110 | %find new point using steer funcrtion or BVP 111 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps); %this is tem new point 112 | 113 | %check collision 114 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 115 | 116 | if colli == 1 117 | continue 118 | end 119 | 120 | 121 | %optimization in edge developement 122 | 123 | qnew_cost = eudist(qnearest,qnew); 124 | 125 | %qrange1=rangesearch(vertecies,qnew,eps+20); 126 | %qrange=cell2mat(qrange1); 127 | 128 | 129 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 130 | vertecies(vert_count+1,:) = qnew; 131 | 132 | edges.x(edge_count+1,:) = [vertecies(ind_nearest(i),1), qnew(1)]; 133 | edges.y(edge_count+1,:) = [vertecies(ind_nearest(i),2), qnew(2)]; 134 | edge_count = edge_count + 1; 135 | 136 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 137 | cmin(vert_count+1,:) = cmin(ind_nearest(i))+qnew_cost; 138 | 139 | % find final path 140 | 141 | goalcost = eudist (qnew,qgoal); 142 | 143 | if goalcost <= eps 144 | finalcost = cmin(vert_count+1,:) + goalcost; 145 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 146 | end 147 | 148 | % scatter(vertecies(vert_count+1,1),vertecies(vert_count+1,2),3,'filled'); 149 | %plot([vertecies(ind_nearest(i),1),vertecies(vert_count+1,1)],[vertecies(ind_nearest(i),2),vertecies(vert_count+1,2)],'LineWidth',0.1,'Color','k'); 150 | %line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','r'); 151 | %line(finaltree(:,1),finaltree(:,2),'LineWidth',0.5,'color','k'); 152 | %scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 153 | %plot(edges.x', edges.y'); 154 | vert_count = vert_count + 1; 155 | i=i+1; 156 | 157 | end 158 | disp(randcount) 159 | 160 | 161 | %if not find final route 162 | if finalcost == 0 163 | ind_nearest1 = knnsearch(vertecies,qgoal); 164 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 165 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 166 | if finalcolli == 0 167 | 168 | goalcost = eudist (qnearest,qgoal); 169 | finalcost = cmin(ind_nearest1,:) + goalcost; 170 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 171 | else 172 | disp('no nearby points') 173 | end 174 | end 175 | disp(finalcost) 176 | 177 | toc; 178 | clear i x_rand y_rand edge_rand 179 | testidealdist = eudist(qstart,qgoal); 180 | disp(testidealdist) 181 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 182 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 183 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 184 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 185 | scatter(vertecies(:,1), vertecies(:,2), 5,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 186 | line(edges.x', edges.y','LineWidth',0.1,'Color','k'); 187 | line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','r'); 188 | 189 | %plot(finaltree(:,1),finaltree(:,2),'MarkerEdgeColor','k','MarkerFaceColor',[0,0,0]); -------------------------------------------------------------------------------- /project code/RRTstar final/RRT_star_final.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | format longG 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [150,50]; 21 | qgoal = [850,900]; 22 | iterations = 1500; 23 | testidealdist = eudist(qstart,qgoal); 24 | disp(testidealdist) 25 | %define OBSTACLE 26 | 27 | %OBSTACLE 1 28 | obs1.x=[400 600]; 29 | obs1.y=[300 600]; 30 | 31 | %OBSTACLE 2 32 | %obs2.x=[]; 33 | %obs2.y=[]; 34 | obs2.x = [0 0]; 35 | obs2.y = [0 0]; 36 | 37 | %create obstacle1 38 | x1box=obs1.x([1 1 2 2 1]); 39 | y1box=obs1.y([1 2 2 1 1]); 40 | 41 | %create obstacle2 42 | x2box=obs2.x([1 1 2 2 1]); 43 | y2box=obs2.y([1 2 2 1 1]); 44 | 45 | obs1area = abs(obs1.x(1)-obs1.x(2))*abs(obs1.y(1)-obs1.y(2)); 46 | obs2area = abs(obs2.x(1)-obs2.x(2))*abs(obs2.y(1)-obs2.y(2)); 47 | 48 | totalarea = height*width; 49 | maxfree = abs(totalarea - (obs1area+obs2area)); 50 | ddimension = 2; 51 | unitballvolume = 2*pi; 52 | 53 | % define tree edge, node and vertice 54 | % 55 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 56 | % order 57 | 58 | vertecies = qstart; 59 | vert_count = 1; 60 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 61 | %vertecies.cost = 0; 62 | 63 | eps=(height*3)/100; 64 | val = 0; 65 | cmin(vert_count,:)=0; 66 | finalcost = 0; 67 | finaltree = qstart; 68 | qnew_cost = 0; 69 | % 70 | % 71 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 72 | % because vertecies(1) is the original point 73 | 74 | edges.x = zeros(iterations,2); 75 | edges.y = zeros(iterations,2); 76 | edge_count = 0; 77 | %% 78 | % 79 | % 80 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 81 | % closetest vertex to vertecies(i+1) 82 | %% 83 | ind_nearest = zeros(iterations,1); 84 | qnew = [0 0]; 85 | colli = 0; 86 | i=1; 87 | randcount=1; 88 | %% 89 | % define variables for optimization 90 | 91 | 92 | %% 93 | % define figure and hold 94 | %% 95 | hold on; 96 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 97 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 98 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 99 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 100 | %plot(qstart(1),qstart(2),'r*'); 101 | %plot(qgoal(1),qgoal(2),'r*'); 102 | 103 | %figure('name', 'RRT growing'); 104 | tic; 105 | %% 106 | while i <= iterations 107 | 108 | x_rand = width*rand(); % random point generation 109 | y_rand = height*rand(); 110 | temp.rand= [x_rand,y_rand]; %save temporary random point 111 | randcount=randcount+1; 112 | 113 | %if random point is goal point this condition is not consider in this 114 | %programme yet 115 | 116 | % find nearest point using KD-Tree method 117 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 118 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 119 | 120 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 121 | %find new point using steer funcrtion or BVP 122 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps); %this is tem new point 123 | 124 | %check collision 125 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 126 | 127 | if colli == 1 128 | continue 129 | end 130 | 131 | %calculate radius for rangesearch 132 | Rrrt = 2*((1+(1/ddimension))^(1/ddimension))*((maxfree/unitballvolume)^(1/ddimension)); 133 | dynamicradius = Rrrt*((log(vert_count)/vert_count)^(1/ddimension)); 134 | 135 | 136 | %optimization in edge developement 137 | 138 | qnew_cost = eudist(qnearest,qnew); 139 | qrange1=rangesearch(vertecies,qnew,max(dynamicradius,eps)); 140 | qrange=cell2mat(qrange1); 141 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 142 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 143 | qmin1 = qnearest; 144 | txx = length(qrange); 145 | %rewire process 146 | for k = 1:length(qrange) 147 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 148 | % tzxzz = qrange(k); 149 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 150 | if cnew < cmin1 151 | tzz = 1; 152 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 153 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 154 | cmin1 = cnew; 155 | ind_nearest(i) = qrange(k); 156 | end 157 | end 158 | end 159 | 160 | 161 | 162 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 163 | vertecies(vert_count+1,:) = qnew; 164 | 165 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 166 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 167 | edge_count = edge_count + 1; 168 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 169 | cmin(vert_count+1,:) = cmin1; 170 | 171 | for j = 1:length(qrange) 172 | cnear = cmin(qrange(j)); 173 | cmin2 = cmin1 + eudist(qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]); 174 | if cmin2 < cnear 175 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]) 176 | % ind_nearest(i) = vert_count+1; 177 | qnewrewire = [vertecies(qrange(j),1),vertecies(qrange(j),2)]; 178 | %vert_count_rewire = qrange(j); 179 | qtree(qrange(j),:) = addtreenode(qrange(j)-1,qtree,vert_count+1); 180 | cmin(qrange(j),:) = cmin2; 181 | % rw = cell2mat(qtree(qrange(j))); 182 | %rw1 = cell2mat(qtree(vert_count+1)); 183 | %rw1(length(rw1)+1) = qrange(j); 184 | end 185 | end 186 | end 187 | 188 | 189 | 190 | % find final path 191 | 192 | goalcost = eudist (qnew,qgoal); 193 | 194 | if goalcost <= eps 195 | finalcost = cmin(vert_count+1,:) + goalcost; 196 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 197 | optimumtolarance = ((testidealdist*1)/100)+testidealdist; 198 | % if finalcost <= optimumtolarance 199 | % disp(optimumtolarance) 200 | % break; 201 | % end 202 | 203 | end 204 | 205 | hold on 206 | drawnow 207 | % scatter(vertecies(vert_count+1,1),vertecies(vert_count+1,2),3,'filled'); 208 | % plot([vertecies(ind_nearest(i),1),vertecies(vert_count+1,1)],[vertecies(ind_nearest(i),2),vertecies(vert_count+1,2)],'LineWidth',0.1,'Color','y'); 209 | % line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','g'); 210 | % scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 211 | % plot(edges.x', edges.y'); 212 | vert_count = vert_count + 1; 213 | i=i+1; 214 | 215 | end 216 | 217 | 218 | %if not find final route 219 | if finalcost == 0 220 | ind_nearest1 = knnsearch(vertecies,qgoal); 221 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 222 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 223 | if finalcolli == 0 224 | 225 | goalcost = eudist (qnearest,qgoal); 226 | finalcost = cmin(ind_nearest1,:) + goalcost; 227 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 228 | else 229 | disp('no nearby points') 230 | end 231 | end 232 | disp(finalcost) 233 | 234 | toc; 235 | clear i x_rand y_rand edge_rand 236 | disp(randcount) 237 | disp(vert_count) 238 | testidealdist = eudist(qstart,qgoal); 239 | disp(testidealdist) 240 | hold on 241 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 242 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 243 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 244 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 245 | scatter(vertecies(:,1), vertecies(:,2), 0.1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 246 | plot(edges.x', edges.y'); 247 | line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','r'); 248 | 249 | -------------------------------------------------------------------------------- /project code/RRTstar final/addtreenode.m: -------------------------------------------------------------------------------- 1 | function qtreeout = addtreenode( vc,qt,in ) 2 | % add new nodes in TREE with minimum cost 3 | qt1=cell2mat(qt(in)); 4 | 5 | qt1(length(qt1)+1)=vc+1; 6 | 7 | qtreeout=num2cell(qt1,[1 2]); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /project code/RRTstar final/collision.m: -------------------------------------------------------------------------------- 1 | function co = collision( x1box,y1box,x2box,y2box,qnear,nearest ) 2 | %collision check 3 | % create temporary edge between nearest point in vertecies and new point 4 | tempedgex = [nearest(1),qnear(1)]; 5 | tempedgey = [nearest(2),qnear(2)]; 6 | %plot(tempedgex',tempedgey'); 7 | %check insersection point between edges of the obstacle and new point to 8 | %nearest point edge 9 | 10 | [x1i,y1i] = polyxpoly(tempedgex,tempedgey,x1box,y1box); 11 | [x2i,y2i] = polyxpoly(tempedgex,tempedgey,x2box,y2box); 12 | 13 | csi1 = size([x1i,y1i]); 14 | csi2 = size ([x2i,y2i]); 15 | %a=csi1(1); 16 | %b=csi2(1); 17 | if (csi1(1)==0 && csi2(1) ==0) 18 | co = 0; 19 | else 20 | co= 1; 21 | end 22 | 23 | end 24 | 25 | -------------------------------------------------------------------------------- /project code/RRTstar final/createfinalpath.m: -------------------------------------------------------------------------------- 1 | function ftree = createfinalpath(ver,qt,vc,qg ) 2 | %create final path 3 | qt1=cell2mat(qt(vc+1)); 4 | %fedx1 = zeros(length(qt1)+1,2); 5 | %fedx1 = zeros(length(qt1)+1,2); 6 | 7 | 8 | for j=1:length(qt1) 9 | qt2(j,:) = [ver(qt1(j),1),ver(qt1(j),2)]; 10 | 11 | end 12 | qt2(length(qt2)+1,:) = qg; 13 | 14 | ftree = qt2; 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /project code/RRTstar final/eudist.m: -------------------------------------------------------------------------------- 1 | function d1 = eudist( dist1,dist2 ) 2 | % Find Euclidean Distance between two points in cartesian system 3 | 4 | d0=[dist1;dist2]; 5 | d1=pdist(d0); 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /project code/RRTstar final/gbsteerfn.m: -------------------------------------------------------------------------------- 1 | function a1=gbsteerfn(x_rand,y_rand,qn1,qn2,qnc,qngc,xgoal,ep,ep1) 2 | N = [0 0]; 3 | xgoal_x = xgoal(1); 4 | xgoal_y = xgoal(2); 5 | % N1=[temp.rand;temp.nearest 6 | %find step size towards qgoal point 7 | if qnc>=eps 8 | %temp1= dist(temp.rand(1),temp.nearest(1)); 9 | % a = x_rand-qn1; 10 | % a2 = y_rand-qn2; 11 | N(1) = abs(qn1 + (((x_rand-qn1)/qnc)*ep)+(((xgoal_x-qn1)/qngc)*ep1)); 12 | N(2) = abs(qn2 + (((y_rand-qn2)/qnc)*ep)+(((xgoal_y-qn2)/qngc)*ep1)); 13 | 14 | else 15 | N(1)=x_rand; 16 | N(2)=y_rand; 17 | end 18 | a1= [N(1),N(2)]; 19 | end 20 | 21 | -------------------------------------------------------------------------------- /project code/RRTstar final/steerfn.m: -------------------------------------------------------------------------------- 1 | function a1=steerfn(x_rand,y_rand,qn1,qn2,qnc,eps) 2 | N = [0 0]; 3 | % N1=[temp.rand;temp.nearest 4 | %find step size towards qrandom point 5 | if qnc>=eps 6 | %temp1= dist(temp.rand(1),temp.nearest(1)); 7 | N(1) = abs(qn1 + ((x_rand-qn1)*eps)/qnc); 8 | N(2) = abs(qn2 + ((y_rand-qn2)*eps)/qnc); 9 | else 10 | N(1)=x_rand; 11 | N(2)=y_rand; 12 | end 13 | a1= [N(1),N(2)]; 14 | end 15 | 16 | -------------------------------------------------------------------------------- /project code/RRTtest/GB_Gaussian_RRT_star_final_test.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | format longG 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [100,500]; 21 | qgoal = [900,500]; 22 | iterations = 1500; 23 | costideal = eudist(qstart,qgoal); 24 | disp(costideal) 25 | slop = abs(qgoal(2)-qstart(2))/(qgoal(1)-qstart(1)); 26 | qcenter = [qstart(1)+(((qgoal(1)-qstart(1))/costideal)*(costideal/2)),qstart(2)+(((qgoal(2)-qstart(2))/costideal)*(costideal/2))]; 27 | %define OBSTACLE 28 | 29 | %OBSTACLE 1 30 | obs1.x=[0 0]; 31 | obs1.y=[0 0]; 32 | 33 | %OBSTACLE 2 34 | %obs2.x=[]; 35 | %obs2.y=[]; 36 | obs2.x = [0 0]; 37 | obs2.y = [0 0]; 38 | 39 | %create obstacle1 40 | x1box=obs1.x([1 1 2 2 1]); 41 | y1box=obs1.y([1 2 2 1 1]); 42 | 43 | %create obstacle2 44 | x2box=obs2.x([1 1 2 2 1]); 45 | y2box=obs2.y([1 2 2 1 1]); 46 | 47 | obs1area = abs(obs1.x(1)-obs1.x(2))*abs(obs1.y(1)-obs1.y(2)); 48 | obs2area = abs(obs2.x(1)-obs2.x(2))*abs(obs2.y(1)-obs2.y(2)); 49 | 50 | totalarea = height*width; 51 | maxfree = abs(totalarea - (obs1area+obs2area)); 52 | ddimension = 2; 53 | unitballvolume = 2*pi; 54 | 55 | % define tree edge, node and vertice 56 | % 57 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 58 | % order 59 | 60 | vertecies = qstart; 61 | vert_count = 1; 62 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 63 | sol_tree_count = 1; 64 | sol_tree_log(sol_tree_count,:) =num2cell(sol_tree_count,[1 2]); 65 | sol_tree_cost(sol_tree_count,:) = 0; 66 | %vertecies.cost = 0; 67 | 68 | eps=(height*3)/100; 69 | eps1=(height*5)/100; 70 | val = 0; 71 | cmin(vert_count,:)=0; 72 | solcost = 0; 73 | soltree = qstart; 74 | qnew_cost = 0; 75 | cmax = 0; 76 | % 77 | % 78 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 79 | % because vertecies(1) is the original point 80 | 81 | edges.x = zeros(iterations,2); 82 | edges.y = zeros(iterations,2); 83 | edge_count = 0; 84 | %% 85 | % 86 | % 87 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 88 | % closetest vertex to vertecies(i+1) 89 | %% 90 | ind_nearest = zeros(iterations,1); 91 | qnew = [0 0]; 92 | colli = 0; 93 | i=1; 94 | randcount=1; 95 | optimumtolarance = 0; 96 | colli_random1 =0; 97 | %% 98 | % define variables for optimization 99 | 100 | 101 | %% 102 | % define figure and hold 103 | %% 104 | hold on; 105 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 106 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 107 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 108 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 109 | %plot(qstart(1),qstart(2),'r*'); 110 | %plot(qgoal(1),qgoal(2),'r*'); 111 | 112 | %figure('name', 'RRT growing'); 113 | tic; 114 | %% 115 | while i <= iterations 116 | 117 | [x_rand,y_rand,eps,eps1] = samplepoint(height,width,cmax,costideal,slop,qcenter,eps,eps1); 118 | % x_rand = width*rand(); % random point generation 119 | % y_rand = height*rand(); 120 | temp.rand= [x_rand,y_rand]; %save temporary random point 121 | randcount=randcount+1; 122 | % drawnow 123 | %scatter(x_rand,y_rand,30,'filled'); 124 | %if random point is goal point this condition is not consider in this 125 | %programme yet 126 | 127 | % find nearest point using KD-Tree method 128 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 129 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 130 | 131 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 132 | qnew_goal_cost = eudist(qgoal,qnearest); 133 | %find new point using steer funcrtion or BVP 134 | %find new point towards goal if no obstacle eps1>eps 135 | %if there is obstacle eps>eps1 136 | %else use only eps 137 | %A goabbiasing function 138 | 139 | if colli_random1 == 0 140 | colli_random = collision(x1box,y1box,x2box,y2box,[x_rand,y_rand],qnearest); 141 | if colli_random == 0 142 | %this is tem new point 143 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps,eps1); 144 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 145 | if colli == 1 146 | % valus of eps and eps1 are inter change to not stay in local minima 147 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps1,eps); 148 | colli1 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 149 | if colli1 == 1 150 | %use normal steer function 151 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 152 | colli2 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 153 | if i<=30 154 | colli_random1 =1; 155 | end 156 | if colli2 == 1 157 | 158 | continue 159 | end 160 | end 161 | end 162 | else 163 | 164 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 165 | colli3 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 166 | if colli3 == 1 167 | continue 168 | end 169 | end 170 | else 171 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps1,eps); 172 | colli3 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 173 | if colli3 == 1 174 | continue 175 | end 176 | end 177 | 178 | if i == 200 179 | colli_random1 = 0; 180 | end 181 | 182 | if qnew(1) >= height || qnew(2) >= width 183 | continue 184 | end 185 | 186 | %check collision 187 | %colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 188 | 189 | % if colli_check == 1 190 | % continue 191 | % end 192 | 193 | %calculate radius for rangesearch 194 | Rrrt = 2*((1+(1/ddimension))^(1/ddimension))*((maxfree/unitballvolume)^(1/ddimension)); 195 | dynamicradius = Rrrt*((log(vert_count)/vert_count)^(1/ddimension)); 196 | %optimization in edge developement 197 | 198 | qnew_cost = eudist(qnearest,qnew); 199 | try1 = abs((dynamicradius-eps)/2); 200 | qrange1=rangesearch(vertecies,qnew,max(dynamicradius,eps)); 201 | %qrange1=rangesearch(vertecies,qnew,try1); 202 | qrange=cell2mat(qrange1); 203 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 204 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 205 | qmin1 = qnearest; 206 | txx = length(qrange); 207 | %rewire process 208 | for k = 1:length(qrange) 209 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 210 | % tzxzz = qrange(k); 211 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 212 | if cnew < cmin1 213 | tzz = 1; 214 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 215 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 216 | cmin1 = cnew; 217 | ind_nearest(i) = qrange(k); 218 | end 219 | end 220 | end 221 | 222 | 223 | 224 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 225 | vertecies(vert_count+1,:) = qnew; 226 | 227 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 228 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 229 | edge_count = edge_count + 1; 230 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 231 | cmin(vert_count+1,:) = cmin1; 232 | 233 | for j = 1:length(qrange) 234 | cnear = cmin(qrange(j)); 235 | cmin2 = cmin1 + eudist(qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]); 236 | if cmin2 < cnear 237 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]) 238 | % ind_nearest(i) = vert_count+1; 239 | qnewrewire = [vertecies(qrange(j),1),vertecies(qrange(j),2)]; 240 | %vert_count_rewire = qrange(j); 241 | qtree(qrange(j),:) = addtreenode(qrange(j)-1,qtree,vert_count+1); 242 | cmin(qrange(j),:) = cmin2; 243 | % rw = cell2mat(qtree(qrange(j))); 244 | %rw1 = cell2mat(qtree(vert_count+1)); 245 | %rw1(length(rw1)+1) = qrange(j); 246 | end 247 | end 248 | end 249 | 250 | 251 | 252 | % find final path 253 | 254 | goalcost = eudist (qnew,qgoal); 255 | 256 | 257 | if goalcost <= eps 258 | solcost = cmin(vert_count+1,:) + goalcost; 259 | soltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 260 | %final_tree_count = 1; 261 | sol_tree_log(sol_tree_count,:) = num2cell(soltree,[1 2]); 262 | sol_tree_cost(sol_tree_count,:) = solcost; 263 | sol_tree_count = sol_tree_count + 1; 264 | 265 | [min_cost_tree,min_cost_index] = min(sol_tree_cost); 266 | soltree = cell2mat(sol_tree_log(min_cost_index)); 267 | cmax= min_cost_tree; 268 | 269 | optimumtolarance = ((906.225*4)/100)+906.225; 270 | if cmax <= optimumtolarance 271 | %disp(optimumtolarance) 272 | break; 273 | end 274 | end 275 | 276 | 277 | % hold on 278 | % drawnow 279 | % line(soltree(:,1),soltree(:,2),'LineWidth',1,'Color','g'); 280 | % scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 281 | % plot(edges.x', edges.y'); 282 | 283 | 284 | 285 | 286 | vert_count = vert_count + 1; 287 | i=i+1; 288 | end 289 | 290 | 291 | %if not find final route 292 | 293 | if cmax == 0 294 | ind_nearest1 = knnsearch(vertecies,qgoal); 295 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 296 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 297 | if finalcolli == 0 298 | 299 | goalcost = eudist (qnearest,qgoal); 300 | cmax = cmin(ind_nearest1,:) + goalcost; 301 | soltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 302 | else 303 | disp('no nearby points') 304 | end 305 | end 306 | 307 | % path finishing 308 | %if cmax >=1 309 | % soltree = pathfinsh(soltree); 310 | %end 311 | 312 | disp(cmax) 313 | 314 | toc; 315 | clear i x_rand y_rand edge_rand 316 | disp(randcount) 317 | disp(vert_count) 318 | %disp(cmax) 319 | hold on 320 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 321 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 322 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 323 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 324 | scatter(vertecies(:,1), vertecies(:,2), 1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 325 | plot(edges.x', edges.y','LineWidth',0.01); 326 | line(soltree(:,1),soltree(:,2),'LineWidth',2,'Color','r'); 327 | 328 | -------------------------------------------------------------------------------- /project code/RRTtest/GB_RRT_star_final.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [50,500]; 21 | qgoal = [900,500]; 22 | iterations = 1500; 23 | testidealdist = eudist(qstart,qgoal); 24 | disp(testidealdist) 25 | %define OBSTACLE 26 | 27 | %OBSTACLE 1 28 | obs1.x=[0 0]; 29 | obs1.y=[0 0]; 30 | 31 | %OBSTACLE 2 32 | %obs2.x=[]; 33 | %obs2.y=[]; 34 | obs2.x = [0 0]; 35 | obs2.y = [0 0]; 36 | 37 | %create obstacle1 38 | x1box=obs1.x([1 1 2 2 1]); 39 | y1box=obs1.y([1 2 2 1 1]); 40 | 41 | %create obstacle2 42 | x2box=obs2.x([1 1 2 2 1]); 43 | y2box=obs2.y([1 2 2 1 1]); 44 | 45 | obs1area = abs(obs1.x(1)-obs1.x(2))*abs(obs1.y(1)-obs1.y(2)); 46 | obs2area = abs(obs2.x(1)-obs2.x(2))*abs(obs2.y(1)-obs2.y(2)); 47 | 48 | totalarea = height*width; 49 | maxfree = abs(totalarea - (obs1area+obs2area)); 50 | ddimension = 2; 51 | unitballvolume = 2*pi; 52 | 53 | % define tree edge, node and vertice 54 | % 55 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 56 | % order 57 | 58 | vertecies = qstart; 59 | vert_count = 1; 60 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 61 | %vertecies.cost = 0; 62 | format longG 63 | eps=(height*3)/100; 64 | eps1=(height*6)/100; 65 | val = 0; 66 | cmin(vert_count,:)=0; 67 | finalcost = 0; 68 | finaltree = qstart; 69 | qnew_cost = 0; 70 | % 71 | % 72 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 73 | % because vertecies(1) is the original point 74 | 75 | edges.x = zeros(iterations,2); 76 | edges.y = zeros(iterations,2); 77 | edge_count = 0; 78 | %% 79 | % 80 | % 81 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 82 | % closetest vertex to vertecies(i+1) 83 | %% 84 | ind_nearest = zeros(iterations,1); 85 | qnew = [0 0]; 86 | colli = 0; 87 | i=1; 88 | randcount=1; 89 | %% 90 | % define variables for optimization 91 | 92 | 93 | %% 94 | % define figure and hold 95 | %% 96 | hold on; 97 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 98 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 99 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 100 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 101 | %plot(qstart(1),qstart(2),'r*'); 102 | %plot(qgoal(1),qgoal(2),'r*'); 103 | 104 | %figure('name', 'RRT growing'); 105 | tic; 106 | %% 107 | while i <= iterations 108 | 109 | x_rand = width*rand(); % random point generation 110 | y_rand = height*rand(); 111 | temp.rand= [x_rand,y_rand]; %save temporary random point 112 | randcount=randcount+1; 113 | % drawnow 114 | %scatter(x_rand,y_rand,30,'filled'); 115 | %if random point is goal point this condition is not consider in this 116 | %programme yet 117 | 118 | % find nearest point using KD-Tree method 119 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 120 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 121 | 122 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 123 | qnew_goal_cost = eudist(qgoal,qnearest); 124 | %find new point using steer funcrtion or BVP 125 | %find new point towards goal if no obstacle eps1>eps 126 | %if there is obstacle eps>eps1 127 | %else use only eps 128 | %A goabbiasing function 129 | colli_random = collision(x1box,y1box,x2box,y2box,[x_rand,y_rand],qnearest); 130 | if colli_random == 0 131 | %this is tem new point 132 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps,eps1); 133 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 134 | if colli == 1 135 | % valus of eps and eps1 are inter change to not stay in local minima 136 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps1,eps); 137 | colli1 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 138 | if colli1 == 1 139 | %use normal steer function 140 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 141 | colli2 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 142 | if colli2 == 1 143 | continue 144 | end 145 | end 146 | end 147 | else 148 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 149 | colli3 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 150 | if colli3 == 1 151 | continue 152 | end 153 | end 154 | 155 | 156 | 157 | %check collision 158 | %colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 159 | 160 | % if colli_check == 1 161 | % continue 162 | % end 163 | 164 | %calculate radius for rangesearch 165 | Rrrt = 2*((1+(1/ddimension))^(1/ddimension))*((maxfree/unitballvolume)^(1/ddimension)); 166 | dynamicradius = Rrrt*((log(vert_count)/vert_count)^(1/ddimension)); 167 | %optimization in edge developement 168 | 169 | qnew_cost = eudist(qnearest,qnew); 170 | try1 = abs((dynamicradius-eps)/2); 171 | qrange1=rangesearch(vertecies,qnew,max(dynamicradius,eps)); 172 | %qrange1=rangesearch(vertecies,qnew,try1); 173 | qrange=cell2mat(qrange1); 174 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 175 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 176 | qmin1 = qnearest; 177 | txx = length(qrange); 178 | %rewire process 179 | for k = 1:length(qrange) 180 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 181 | % tzxzz = qrange(k); 182 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 183 | if cnew < cmin1 184 | tzz = 1; 185 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 186 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 187 | cmin1 = cnew; 188 | ind_nearest(i) = qrange(k); 189 | end 190 | end 191 | end 192 | 193 | 194 | 195 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 196 | vertecies(vert_count+1,:) = qnew; 197 | 198 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 199 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 200 | edge_count = edge_count + 1; 201 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 202 | cmin(vert_count+1,:) = cmin1; 203 | 204 | for j = 1:length(qrange) 205 | cnear = cmin(qrange(j)); 206 | cmin2 = cmin1 + eudist(qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]); 207 | if cmin2 < cnear 208 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]) 209 | % ind_nearest(i) = vert_count+1; 210 | qnewrewire = [vertecies(qrange(j),1),vertecies(qrange(j),2)]; 211 | %vert_count_rewire = qrange(j); 212 | qtree(qrange(j),:) = addtreenode(qrange(j)-1,qtree,vert_count+1); 213 | cmin(qrange(j),:) = cmin2; 214 | % rw = cell2mat(qtree(qrange(j))); 215 | %rw1 = cell2mat(qtree(vert_count+1)); 216 | %rw1(length(rw1)+1) = qrange(j); 217 | end 218 | end 219 | end 220 | 221 | 222 | 223 | % find final path 224 | 225 | goalcost = eudist (qnew,qgoal); 226 | 227 | 228 | if goalcost <= eps 229 | finalcost = cmin(vert_count+1,:) + goalcost; 230 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 231 | 232 | optimumtolarance = ((testidealdist*1)/100)+testidealdist; 233 | 234 | if finalcost <= optimumtolarance 235 | disp(optimumtolarance) 236 | break; 237 | end 238 | end 239 | 240 | % hold on 241 | % drawnow 242 | % scatter(vertecies(vert_count+1,1),vertecies(vert_count+1,2),3,'filled'); 243 | % plot([vertecies(ind_nearest(i),1),vertecies(vert_count+1,1)],[vertecies(ind_nearest(i),2),vertecies(vert_count+1,2)],'LineWidth',0.1,'Color','y'); 244 | % line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','g'); 245 | % scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 246 | % plot(edges.x', edges.y'); 247 | vert_count = vert_count + 1; 248 | i=i+1; 249 | 250 | end 251 | 252 | 253 | %if not find final route 254 | if finalcost == 0 255 | ind_nearest1 = knnsearch(vertecies,qgoal); 256 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 257 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 258 | if finalcolli == 0 259 | 260 | goalcost = eudist (qnearest,qgoal); 261 | finalcost = cmin(ind_nearest1,:) + goalcost; 262 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 263 | else 264 | disp('no nearby points') 265 | end 266 | end 267 | disp(finalcost) 268 | 269 | toc; 270 | clear i x_rand y_rand edge_rand 271 | disp(randcount) 272 | disp(vert_count) 273 | hold on 274 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 275 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 276 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 277 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 278 | scatter(vertecies(:,1), vertecies(:,2), 0.1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 279 | plot(edges.x', edges.y','LineWidth',0.01); 280 | line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','r'); 281 | 282 | -------------------------------------------------------------------------------- /project code/RRTtest/GB_RRT_star_final_test.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | format longG 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [50,500]; 21 | qgoal = [900,500]; 22 | iterations = 1500; 23 | costideal = eudist(qstart,qgoal); 24 | disp(costideal) 25 | slop = abs(qgoal(2)-qstart(2))/(qgoal(1)-qstart(1)); 26 | qcenter = [qstart(1)+(((qgoal(1)-qstart(1))/costideal)*(costideal/2)),qstart(2)+(((qgoal(2)-qstart(2))/costideal)*(costideal/2))]; 27 | %define OBSTACLE 28 | 29 | %OBSTACLE 1 30 | obs1.x=[200 600]; 31 | obs1.y=[350 650]; 32 | 33 | %OBSTACLE 2 34 | %obs2.x=[]; 35 | %obs2.y=[]; 36 | obs2.x = [0 0]; 37 | obs2.y = [0 0]; 38 | 39 | %create obstacle1 40 | x1box=obs1.x([1 1 2 2 1]); 41 | y1box=obs1.y([1 2 2 1 1]); 42 | 43 | %create obstacle2 44 | x2box=obs2.x([1 1 2 2 1]); 45 | y2box=obs2.y([1 2 2 1 1]); 46 | 47 | obs1area = abs(obs1.x(1)-obs1.x(2))*abs(obs1.y(1)-obs1.y(2)); 48 | obs2area = abs(obs2.x(1)-obs2.x(2))*abs(obs2.y(1)-obs2.y(2)); 49 | 50 | totalarea = height*width; 51 | maxfree = abs(totalarea - (obs1area+obs2area)); 52 | ddimension = 2; 53 | unitballvolume = 2*pi; 54 | 55 | % define tree edge, node and vertice 56 | % 57 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 58 | % order 59 | 60 | vertecies = qstart; 61 | vert_count = 1; 62 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 63 | sol_tree_count = 1; 64 | sol_tree_log(sol_tree_count,:) =num2cell(sol_tree_count,[1 2]); 65 | sol_tree_cost(sol_tree_count,:) = 0; 66 | %vertecies.cost = 0; 67 | 68 | eps=(height*3)/100; 69 | eps1=(height*5)/100; 70 | val = 0; 71 | cmin(vert_count,:)=0; 72 | solcost = 0; 73 | soltree = qstart; 74 | qnew_cost = 0; 75 | cmax = 0; 76 | % 77 | % 78 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 79 | % because vertecies(1) is the original point 80 | 81 | edges.x = zeros(iterations,2); 82 | edges.y = zeros(iterations,2); 83 | edge_count = 0; 84 | %% 85 | % 86 | % 87 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 88 | % closetest vertex to vertecies(i+1) 89 | %% 90 | ind_nearest = zeros(iterations,1); 91 | qnew = [0 0]; 92 | colli = 0; 93 | i=1; 94 | randcount=1; 95 | optimumtolarance = 0; 96 | %% 97 | % define variables for optimization 98 | 99 | 100 | %% 101 | % define figure and hold 102 | %% 103 | hold on; 104 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 105 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 106 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 107 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 108 | %plot(qstart(1),qstart(2),'r*'); 109 | %plot(qgoal(1),qgoal(2),'r*'); 110 | 111 | %figure('name', 'RRT growing'); 112 | tic; 113 | %% 114 | while i <= iterations 115 | 116 | [x_rand,y_rand] = samplepoint(height,width,cmax,costideal,slop,qcenter); 117 | % x_rand = width*rand(); % random point generation 118 | % y_rand = height*rand(); 119 | temp.rand= [x_rand,y_rand]; %save temporary random point 120 | randcount=randcount+1; 121 | % drawnow 122 | %scatter(x_rand,y_rand,30,'filled'); 123 | %if random point is goal point this condition is not consider in this 124 | %programme yet 125 | 126 | % find nearest point using KD-Tree method 127 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 128 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 129 | 130 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 131 | qnew_goal_cost = eudist(qgoal,qnearest); 132 | %find new point using steer funcrtion or BVP 133 | %find new point towards goal if no obstacle eps1>eps 134 | %if there is obstacle eps>eps1 135 | %else use only eps 136 | %A goabbiasing function 137 | colli_random = collision(x1box,y1box,x2box,y2box,[x_rand,y_rand],qnearest); 138 | if colli_random == 0 139 | %this is tem new point 140 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps,eps1); 141 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 142 | if colli == 1 143 | % valus of eps and eps1 are inter change to not stay in local minima 144 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps1,eps); 145 | colli1 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 146 | if colli1 == 1 147 | %use normal steer function 148 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 149 | colli2 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 150 | if colli2 == 1 151 | continue 152 | end 153 | end 154 | end 155 | else 156 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 157 | colli3 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 158 | if colli3 == 1 159 | continue 160 | end 161 | end 162 | 163 | 164 | 165 | %check collision 166 | %colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 167 | 168 | % if colli_check == 1 169 | % continue 170 | % end 171 | 172 | %calculate radius for rangesearch 173 | Rrrt = 2*((1+(1/ddimension))^(1/ddimension))*((maxfree/unitballvolume)^(1/ddimension)); 174 | dynamicradius = Rrrt*((log(vert_count)/vert_count)^(1/ddimension)); 175 | %optimization in edge developement 176 | 177 | qnew_cost = eudist(qnearest,qnew); 178 | try1 = abs((dynamicradius-eps)/2); 179 | qrange1=rangesearch(vertecies,qnew,max(dynamicradius,eps)); 180 | %qrange1=rangesearch(vertecies,qnew,try1); 181 | qrange=cell2mat(qrange1); 182 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 183 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 184 | qmin1 = qnearest; 185 | txx = length(qrange); 186 | %rewire process 187 | for k = 1:length(qrange) 188 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 189 | % tzxzz = qrange(k); 190 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 191 | if cnew < cmin1 192 | tzz = 1; 193 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 194 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 195 | cmin1 = cnew; 196 | ind_nearest(i) = qrange(k); 197 | end 198 | end 199 | end 200 | 201 | 202 | 203 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 204 | vertecies(vert_count+1,:) = qnew; 205 | 206 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 207 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 208 | edge_count = edge_count + 1; 209 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 210 | cmin(vert_count+1,:) = cmin1; 211 | 212 | for j = 1:length(qrange) 213 | cnear = cmin(qrange(j)); 214 | cmin2 = cmin1 + eudist(qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]); 215 | if cmin2 < cnear 216 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]) 217 | % ind_nearest(i) = vert_count+1; 218 | qnewrewire = [vertecies(qrange(j),1),vertecies(qrange(j),2)]; 219 | %vert_count_rewire = qrange(j); 220 | qtree(qrange(j),:) = addtreenode(qrange(j)-1,qtree,vert_count+1); 221 | cmin(qrange(j),:) = cmin2; 222 | % rw = cell2mat(qtree(qrange(j))); 223 | %rw1 = cell2mat(qtree(vert_count+1)); 224 | %rw1(length(rw1)+1) = qrange(j); 225 | end 226 | end 227 | end 228 | 229 | 230 | 231 | % find final path 232 | 233 | goalcost = eudist (qnew,qgoal); 234 | 235 | 236 | if goalcost <= eps 237 | solcost = cmin(vert_count+1,:) + goalcost; 238 | soltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 239 | %final_tree_count = 1; 240 | sol_tree_log(sol_tree_count,:) = num2cell(soltree,[1 2]); 241 | sol_tree_cost(sol_tree_count,:) = solcost; 242 | sol_tree_count = sol_tree_count + 1; 243 | 244 | [min_cost_tree,min_cost_index] = min(sol_tree_cost); 245 | soltree = cell2mat(sol_tree_log(min_cost_index)); 246 | cmax= min_cost_tree; 247 | 248 | optimumtolarance = ((costideal*15)/100)+costideal; 249 | if cmax <= optimumtolarance 250 | %disp(optimumtolarance) 251 | break; 252 | end 253 | end 254 | 255 | 256 | % hold on 257 | % drawnow 258 | % line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','g'); 259 | % scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 260 | % plot(edges.x', edges.y'); 261 | 262 | 263 | 264 | 265 | vert_count = vert_count + 1; 266 | i=i+1; 267 | end 268 | 269 | 270 | %if not find final route 271 | 272 | if cmax == 0 273 | ind_nearest1 = knnsearch(vertecies,qgoal); 274 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 275 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 276 | if finalcolli == 0 277 | 278 | goalcost = eudist (qnearest,qgoal); 279 | cmax = cmin(ind_nearest1,:) + goalcost; 280 | soltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 281 | else 282 | disp('no nearby points') 283 | end 284 | end 285 | 286 | % path finishing 287 | %if cmax >=1 288 | % soltree = pathfinsh(soltree); 289 | %end 290 | 291 | disp(cmax) 292 | 293 | toc; 294 | clear i x_rand y_rand edge_rand 295 | disp(randcount) 296 | disp(vert_count) 297 | %disp(cmax) 298 | hold on 299 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 300 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 301 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 302 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 303 | scatter(vertecies(:,1), vertecies(:,2), 1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 304 | plot(edges.x', edges.y','LineWidth',0.01); 305 | line(soltree(:,1),soltree(:,2),'LineWidth',2,'Color','r'); 306 | 307 | -------------------------------------------------------------------------------- /project code/RRTtest/RRT_final.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [150,150]; 21 | qgoal = [850,950]; 22 | iterations = 2000; 23 | 24 | %define OBSTACLE 25 | 26 | %OBSTACLE 1 27 | obs1.x=[0 0]; 28 | obs1.y=[0 0]; 29 | 30 | %OBSTACLE 2 31 | %obs2.x=[]; 32 | %obs2.y=[]; 33 | obs2.x = [0 0]; 34 | obs2.y = [0 0]; 35 | 36 | %create obstacle1 37 | x1box=obs1.x([1 1 2 2 1]); 38 | y1box=obs1.y([1 2 2 1 1]); 39 | 40 | %create obstacle2 41 | x2box=obs2.x([1 1 2 2 1]); 42 | y2box=obs2.y([1 2 2 1 1]); 43 | 44 | 45 | % define tree edge, node and vertice 46 | % 47 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 48 | % order 49 | 50 | vertecies = qstart; 51 | vert_count = 1; 52 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 53 | %vertecies.cost = 0; 54 | format longG 55 | eps=30; 56 | val = 0; 57 | cmin(vert_count,:)=0; 58 | finalcost = 0; 59 | finaltree = qstart; 60 | qnew_cost = 0; 61 | % 62 | % 63 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 64 | % because vertecies(1) is the original point 65 | 66 | edges.x = zeros(iterations,2); 67 | edges.y = zeros(iterations,2); 68 | edge_count = 0; 69 | %% 70 | % 71 | % 72 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 73 | % closetest vertex to vertecies(i+1) 74 | %% 75 | ind_nearest = zeros(iterations,1); 76 | qnew = [0 0]; 77 | colli = 0; 78 | i=1; 79 | randcount=1; 80 | %% 81 | % define variables for optimization 82 | 83 | 84 | %% 85 | % define figure and hold 86 | %% 87 | hold on; 88 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 89 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 90 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 91 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 92 | %plot(qstart(1),qstart(2),'r*'); 93 | %plot(qgoal(1),qgoal(2),'r*'); 94 | 95 | %figure('name', 'RRT growing'); 96 | tic; 97 | %% 98 | while i <= iterations 99 | 100 | x_rand = width*rand(); % random point generation 101 | y_rand = height*rand(); 102 | temp.rand= [x_rand,y_rand]; %save temporary random point 103 | randcount=randcount+1; 104 | 105 | %if random point is goal point this condition is not consider in this 106 | %programme yet 107 | 108 | % find nearest point using KD-Tree method 109 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 110 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 111 | 112 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 113 | %find new point using steer funcrtion or BVP 114 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps); %this is tem new point 115 | 116 | %check collision 117 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 118 | 119 | if colli == 1 120 | continue 121 | end 122 | %optimization in edge developement 123 | qnew_cost = eudist(qnearest,qnew); 124 | qrange1=rangesearch(vertecies,qnew,50); 125 | qrange=cell2mat(qrange1); 126 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 127 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 128 | qmin1 = qnearest; 129 | txx = length(qrange); 130 | %rewire process 131 | for k = 1:length(qrange) 132 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 133 | % tzxzz = qrange(k); 134 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 135 | if cnew < cmin1 136 | tzz = 1; 137 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 138 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 139 | cmin1 = cnew; 140 | ind_nearest(i) = qrange(k); 141 | end 142 | end 143 | end 144 | 145 | 146 | 147 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 148 | vertecies(vert_count+1,:) = qnew; 149 | 150 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 151 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 152 | edge_count = edge_count + 1; 153 | 154 | %rewire process in RRT 155 | 156 | 157 | 158 | 159 | 160 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 161 | cmin(vert_count+1,:) = cmin1; 162 | 163 | % find final path 164 | 165 | goalcost = eudist (qnew,qgoal); 166 | 167 | if goalcost <= eps 168 | finalcost = cmin(vert_count+1,:) + goalcost; 169 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 170 | %break; 171 | end 172 | 173 | hold on 174 | 175 | scatter(vertecies(vert_count+1,1),vertecies(vert_count+1,2),3,'filled'); 176 | plot([vertecies(ind_nearest(i),1),vertecies(vert_count+1,1)],[vertecies(ind_nearest(i),2),vertecies(vert_count+1,2)],'LineWidth',0.1,'Color','y'); 177 | line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','g'); 178 | %scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 179 | %plot(edges.x', edges.y'); 180 | vert_count = vert_count + 1; 181 | i=i+1; 182 | 183 | end 184 | disp(randcount) 185 | 186 | 187 | %if not find final route 188 | if finalcost == 0 189 | ind_nearest1 = knnsearch(vertecies,qgoal); 190 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 191 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 192 | if finalcolli == 0 193 | 194 | goalcost = eudist (qnearest,qgoal); 195 | finalcost = cmin(ind_nearest1,:) + goalcost; 196 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 197 | else 198 | disp('no nearby points') 199 | end 200 | end 201 | disp(finalcost) 202 | 203 | toc; 204 | clear i x_rand y_rand edge_rand 205 | testidealdist = eudist(qstart,qgoal); 206 | disp(testidealdist) 207 | hold on 208 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 209 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 210 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 211 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 212 | scatter(vertecies(:,1), vertecies(:,2), 0.1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 213 | plot(edges.x', edges.y'); 214 | line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','r'); 215 | 216 | -------------------------------------------------------------------------------- /project code/RRTtest/RRT_nocollision.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | axis ([0 width 0 height]); 14 | %% 15 | % 16 | % 17 | % define starting and end point and iteration 18 | %% 19 | qstart = [50,50]; 20 | qgoal = [750,850]; 21 | iterations = 2000; 22 | 23 | %define OBSTACLE 24 | 25 | %OBSTACLE 1 26 | obs1.x=[0 0]; 27 | obs1.y=[0 0]; 28 | 29 | %OBSTACLE 2 30 | %obs2.x=[]; 31 | %obs2.y=[]; 32 | obs2.x = [0 0]; 33 | obs2.y = [0 0]; 34 | 35 | %create obstacle1 36 | x1box=obs1.x([1 1 2 2 1]); 37 | y1box=obs1.y([1 2 2 1 1]); 38 | 39 | %create obstacle2 40 | x2box=obs2.x([1 1 2 2 1]); 41 | y2box=obs2.y([1 2 2 1 1]); 42 | 43 | 44 | % define tree edge, node and vertice 45 | % 46 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 47 | % order 48 | 49 | vertecies = qstart; 50 | vert_count = 1; 51 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 52 | %vertecies.cost = 0; 53 | format longG 54 | eps=15; 55 | val = 0; 56 | cmin(vert_count,:)=0; 57 | finalcost = 0; 58 | finaltree = qstart; 59 | qnew_cost = 0; 60 | % 61 | % 62 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 63 | % because vertecies(1) is the original point 64 | 65 | edges.x = zeros(iterations,2); 66 | edges.y = zeros(iterations,2); 67 | edge_count = 0; 68 | %% 69 | % 70 | % 71 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 72 | % closetest vertex to vertecies(i+1) 73 | %% 74 | ind_nearest = zeros(iterations,1); 75 | qnew = [0 0]; 76 | colli = 0; 77 | i=1; 78 | randcount=1; 79 | %% 80 | % define variables for optimization 81 | 82 | 83 | %% 84 | % define figure and hold 85 | %% 86 | hold on; 87 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 88 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 89 | scatter(qstart(1), qstart(2), 45, '*','r','LineWidth',1); hold on; 90 | scatter(qgoal(1), qgoal(2), 45, '*','r','LineWidth',1); hold on; 91 | 92 | %figure('name', 'RRT growing'); 93 | tic; 94 | %% 95 | while i <= iterations 96 | 97 | x_rand = width*rand(); % random point generation 98 | y_rand = height*rand(); 99 | temp.rand= [x_rand,y_rand]; %save temporary random point 100 | randcount=randcount+1; 101 | 102 | 103 | % find nearest point using KD-Tree method 104 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 105 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 106 | 107 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 108 | %find new point using steer funcrtion or BVP 109 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps); %this is tem new point 110 | 111 | %check collision 112 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 113 | 114 | if colli == 1 115 | continue 116 | end 117 | 118 | 119 | %optimization in edge developement 120 | 121 | qnew_cost = eudist(qnearest,qnew); 122 | 123 | %qrange1=rangesearch(vertecies,qnew,eps+20); 124 | %qrange=cell2mat(qrange1); 125 | 126 | 127 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 128 | vertecies(vert_count+1,:) = qnew; 129 | 130 | edges.x(edge_count+1,:) = [vertecies(ind_nearest(i),1), qnew(1)]; 131 | edges.y(edge_count+1,:) = [vertecies(ind_nearest(i),2), qnew(2)]; 132 | edge_count = edge_count + 1; 133 | 134 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 135 | cmin(vert_count+1,:) = cmin(ind_nearest(i))+qnew_cost; 136 | 137 | % find final path 138 | 139 | goalcost = eudist (qnew,qgoal); 140 | 141 | if goalcost <= eps 142 | finalcost = cmin(vert_count+1,:) + goalcost; 143 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 144 | end 145 | 146 | 147 | %scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 148 | %plot(edges.x', edges.y'); 149 | vert_count = vert_count + 1; 150 | i=i+1; 151 | 152 | end 153 | disp(randcount) 154 | 155 | 156 | %if not find final route 157 | if finalcost == 0 158 | ind_nearest1 = knnsearch(vertecies,qgoal); 159 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 160 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 161 | if colli == 1 162 | disp('no nearby points') 163 | end 164 | goalcost = eudist (qnearest,qgoal); 165 | finalcost = cmin(ind_nearest1,:) + goalcost; 166 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1,qgoal); 167 | end 168 | disp(finalcost) 169 | 170 | toc; 171 | clear i x_rand y_rand edge_rand 172 | testidealdist = eudist(qstart,qgoal); 173 | disp(testidealdist) 174 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 175 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 176 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 177 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 178 | scatter(vertecies(:,1), vertecies(:,2), 5,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 179 | plot(edges.x', edges.y'); 180 | line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','r'); 181 | 182 | %plot(finaltree(:,1),finaltree(:,2),'MarkerEdgeColor','k','MarkerFaceColor',[0,0,0]); -------------------------------------------------------------------------------- /project code/RRTtest/RRT_nocollision_twotree.m: -------------------------------------------------------------------------------- 1 | 2 | %clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | axis ([0 width 0 height]); 14 | %% 15 | % 16 | % 17 | % define starting and end point and iteration 18 | %% 19 | qstart = [150,150]; 20 | qgoal = [950,450]; 21 | iterations = 2500; 22 | 23 | %define OBSTACLE 24 | 25 | %OBSTACLE 1 26 | obs1.x=[450 750]; 27 | obs1.y=[250 850]; 28 | 29 | %OBSTACLE 2 30 | %obs2.x=[]; 31 | %obs2.y=[]; 32 | obs2.x = [0 0]; 33 | obs2.y = [0 0]; 34 | 35 | %create obstacle1 36 | x1box=obs1.x([1 1 2 2 1]); 37 | y1box=obs1.y([1 2 2 1 1]); 38 | 39 | %create obstacle2 40 | x2box=obs2.x([1 1 2 2 1]); 41 | y2box=obs2.y([1 2 2 1 1]); 42 | 43 | 44 | % define tree edge, node and vertice 45 | % 46 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 47 | % order 48 | 49 | vertecies = [qstart;qgoal]; 50 | vert_count = 2; 51 | eps=10; 52 | val = 0; 53 | % 54 | % 55 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 56 | % because vertecies(1) is the original point 57 | 58 | edges.x = zeros(iterations,2); 59 | edges.y = zeros(iterations,2); 60 | edge_count = 0; 61 | %% 62 | % 63 | % 64 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 65 | % closetest vertex to vertecies(i+1) 66 | %% 67 | ind_nearest = zeros(iterations,1); 68 | qnear = [0 0]; 69 | colli = 0; 70 | i=1; 71 | randcount=1; 72 | %% 73 | % 74 | % 75 | % define figure and hold 76 | %% 77 | hold on; 78 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 79 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 80 | scatter(qstart(1), qstart(2), 45, '*','r','LineWidth',1); hold on; 81 | scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 82 | 83 | %figure('name', 'RRT growing'); 84 | tic; 85 | %% 86 | while i <= iterations 87 | 88 | x_rand = width*rand(); % random point generation 89 | y_rand = height*rand(); 90 | temp.rand= [x_rand,y_rand]; %save temporary random point 91 | randcount=randcount+1; 92 | 93 | 94 | % find nearest point using KD-Tree method 95 | [ind_nearest(i),val] = knnsearch(vertecies, [x_rand,y_rand]); 96 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 97 | 98 | 99 | %find new point using steer funcrtion or BVP 100 | qnear = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),val,eps); %this is tem new point 101 | 102 | %check collision 103 | colli = collision(x1box,y1box,x2box,y2box,qnear,qnearest); 104 | 105 | if colli == 1 106 | continue 107 | end 108 | 109 | 110 | 111 | 112 | 113 | 114 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 115 | vertecies(vert_count+1,:) = qnear; 116 | vert_count = vert_count + 1; 117 | edges.x(edge_count+1,:) = [vertecies(ind_nearest(i),1), qnear(1)]; 118 | edges.y(edge_count+1,:) = [vertecies(ind_nearest(i),2), qnear(2)]; 119 | edge_count = edge_count + 1; 120 | 121 | 122 | 123 | %rewire to find path 124 | 125 | 126 | 127 | 128 | %scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 129 | %plot(edges.x', edges.y'); 130 | i=i+1; 131 | 132 | end 133 | disp(randcount) 134 | toc; 135 | clear i x_rand y_rand edge_rand 136 | 137 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 138 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 139 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 140 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 141 | scatter(vertecies(:,1), vertecies(:,2), 5,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 142 | plot(edges.x', edges.y'); 143 | 144 | -------------------------------------------------------------------------------- /project code/RRTtest/RRT_rewire.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/RRT_rewire.mlx -------------------------------------------------------------------------------- /project code/RRTtest/RRT_star_final.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [100,100]; 21 | qgoal = [900,700]; 22 | iterations = 1500; 23 | 24 | %define OBSTACLE 25 | 26 | %OBSTACLE 1 27 | obs1.x=[0 0]; 28 | obs1.y=[0 0]; 29 | 30 | %OBSTACLE 2 31 | %obs2.x=[]; 32 | %obs2.y=[]; 33 | obs2.x = [0 0]; 34 | obs2.y = [0 0]; 35 | 36 | %create obstacle1 37 | x1box=obs1.x([1 1 2 2 1]); 38 | y1box=obs1.y([1 2 2 1 1]); 39 | 40 | %create obstacle2 41 | x2box=obs2.x([1 1 2 2 1]); 42 | y2box=obs2.y([1 2 2 1 1]); 43 | 44 | obs1area = abs(obs1.x(1)-obs1.x(2))*abs(obs1.y(1)-obs1.y(2)); 45 | obs2area = abs(obs2.x(1)-obs2.x(2))*abs(obs2.y(1)-obs2.y(2)); 46 | 47 | totalarea = height*width; 48 | maxfree = abs(totalarea - (obs1area+obs2area)); 49 | ddimension = 2; 50 | unitballvolume = 2*pi; 51 | 52 | % define tree edge, node and vertice 53 | % 54 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 55 | % order 56 | 57 | vertecies = qstart; 58 | vert_count = 1; 59 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 60 | %vertecies.cost = 0; 61 | format longG 62 | eps=(height*3)/100; 63 | val = 0; 64 | cmin(vert_count,:)=0; 65 | finalcost = 0; 66 | finaltree = qstart; 67 | qnew_cost = 0; 68 | % 69 | % 70 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 71 | % because vertecies(1) is the original point 72 | 73 | edges.x = zeros(iterations,2); 74 | edges.y = zeros(iterations,2); 75 | edge_count = 0; 76 | %% 77 | % 78 | % 79 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 80 | % closetest vertex to vertecies(i+1) 81 | %% 82 | ind_nearest = zeros(iterations,1); 83 | qnew = [0 0]; 84 | colli = 0; 85 | i=1; 86 | randcount=1; 87 | %% 88 | % define variables for optimization 89 | 90 | 91 | %% 92 | % define figure and hold 93 | %% 94 | hold on; 95 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 96 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 97 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 98 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 99 | %plot(qstart(1),qstart(2),'r*'); 100 | %plot(qgoal(1),qgoal(2),'r*'); 101 | 102 | %figure('name', 'RRT growing'); 103 | tic; 104 | %% 105 | while i <= iterations 106 | 107 | x_rand = width*rand(); % random point generation 108 | y_rand = height*rand(); 109 | temp.rand= [x_rand,y_rand]; %save temporary random point 110 | randcount=randcount+1; 111 | 112 | %if random point is goal point this condition is not consider in this 113 | %programme yet 114 | 115 | % find nearest point using KD-Tree method 116 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 117 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 118 | 119 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 120 | %find new point using steer funcrtion or BVP 121 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps); %this is tem new point 122 | 123 | %check collision 124 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 125 | 126 | if colli == 1 127 | continue 128 | end 129 | 130 | %calculate radius for rangesearch 131 | Rrrt = 2*((1+(1/ddimension))^(1/ddimension))*((maxfree/unitballvolume)^(1/ddimension)); 132 | dynamicradius = Rrrt*((log(vert_count)/vert_count)^(1/ddimension)); 133 | %optimization in edge developement 134 | 135 | qnew_cost = eudist(qnearest,qnew); 136 | qrange1=rangesearch(vertecies,qnew,max(dynamicradius,eps)); 137 | qrange=cell2mat(qrange1); 138 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 139 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 140 | qmin1 = qnearest; 141 | txx = length(qrange); 142 | %rewire process 143 | for k = 1:length(qrange) 144 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 145 | % tzxzz = qrange(k); 146 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 147 | if cnew < cmin1 148 | tzz = 1; 149 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 150 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 151 | cmin1 = cnew; 152 | ind_nearest(i) = qrange(k); 153 | end 154 | end 155 | end 156 | 157 | 158 | 159 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 160 | vertecies(vert_count+1,:) = qnew; 161 | 162 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 163 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 164 | edge_count = edge_count + 1; 165 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 166 | cmin(vert_count+1,:) = cmin1; 167 | 168 | for j = 1:length(qrange) 169 | cnear = cmin(qrange(j)); 170 | cmin2 = cmin1 + eudist(qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]); 171 | if cmin2 < cnear 172 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]) 173 | % ind_nearest(i) = vert_count+1; 174 | qnewrewire = [vertecies(qrange(j),1),vertecies(qrange(j),2)]; 175 | %vert_count_rewire = qrange(j); 176 | qtree(qrange(j),:) = addtreenode(qrange(j)-1,qtree,vert_count+1); 177 | cmin(qrange(j),:) = cmin2; 178 | % rw = cell2mat(qtree(qrange(j))); 179 | %rw1 = cell2mat(qtree(vert_count+1)); 180 | %rw1(length(rw1)+1) = qrange(j); 181 | end 182 | end 183 | end 184 | 185 | 186 | 187 | % find final path 188 | 189 | goalcost = eudist (qnew,qgoal); 190 | 191 | if goalcost <= eps 192 | finalcost = cmin(vert_count+1,:) + goalcost; 193 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 194 | % break; 195 | end 196 | 197 | hold on 198 | %drawnow 199 | % scatter(vertecies(vert_count+1,1),vertecies(vert_count+1,2),3,'filled'); 200 | % plot([vertecies(ind_nearest(i),1),vertecies(vert_count+1,1)],[vertecies(ind_nearest(i),2),vertecies(vert_count+1,2)],'LineWidth',0.1,'Color','y'); 201 | line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','g'); 202 | scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 203 | plot(edges.x', edges.y'); 204 | vert_count = vert_count + 1; 205 | i=i+1; 206 | 207 | end 208 | disp(randcount) 209 | 210 | 211 | %if not find final route 212 | if finalcost == 0 213 | ind_nearest1 = knnsearch(vertecies,qgoal); 214 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 215 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 216 | if finalcolli == 0 217 | 218 | goalcost = eudist (qnearest,qgoal); 219 | finalcost = cmin(ind_nearest1,:) + goalcost; 220 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 221 | else 222 | disp('no nearby points') 223 | end 224 | end 225 | disp(finalcost) 226 | 227 | toc; 228 | clear i x_rand y_rand edge_rand 229 | testidealdist = eudist(qstart,qgoal); 230 | disp(testidealdist) 231 | hold on 232 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 233 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 234 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 235 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 236 | %scatter(vertecies(:,1), vertecies(:,2), 0.1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 237 | %plot(edges.x', edges.y','Color','k'); 238 | %line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','r'); 239 | 240 | -------------------------------------------------------------------------------- /project code/RRTtest/RRT_star_final_test.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | %clc; 4 | close(findobj('type','figure','name','RRT basic')); 5 | close(findobj('type','figure','name','RRT growing')); 6 | 7 | %% 8 | % define boundry and area. 9 | 10 | height = 1000; 11 | width = 1000; 12 | figure('name', 'RRT star'); 13 | hold on 14 | axis ([0 width 0 height]); 15 | %% 16 | % 17 | % 18 | % define starting and end point and iteration 19 | %% 20 | qstart = [100,100]; 21 | qgoal = [900,700]; 22 | iterations = 1500; 23 | testidealdist = eudist(qstart,qgoal); 24 | disp(testidealdist) 25 | %define OBSTACLE 26 | 27 | %OBSTACLE 1 28 | obs1.x=[0 0]; 29 | obs1.y=[0 0]; 30 | 31 | %OBSTACLE 2 32 | %obs2.x=[]; 33 | %obs2.y=[]; 34 | obs2.x = [0 0]; 35 | obs2.y = [0 0]; 36 | 37 | %create obstacle1 38 | x1box=obs1.x([1 1 2 2 1]); 39 | y1box=obs1.y([1 2 2 1 1]); 40 | 41 | %create obstacle2 42 | x2box=obs2.x([1 1 2 2 1]); 43 | y2box=obs2.y([1 2 2 1 1]); 44 | 45 | obs1area = abs(obs1.x(1)-obs1.x(2))*abs(obs1.y(1)-obs1.y(2)); 46 | obs2area = abs(obs2.x(1)-obs2.x(2))*abs(obs2.y(1)-obs2.y(2)); 47 | 48 | totalarea = height*width; 49 | maxfree = abs(totalarea - (obs1area+obs2area)); 50 | ddimension = 2; 51 | unitballvolume = 2*pi; 52 | 53 | % define tree edge, node and vertice 54 | % 55 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated 56 | % order 57 | 58 | vertecies = qstart; 59 | vert_count = 1; 60 | qtree(vert_count,:)=num2cell(vert_count,[1 2]); 61 | %vertecies.cost = 0; 62 | format longG 63 | eps=(height*3)/100; 64 | eps1=(height*6)/100; 65 | val = 0; 66 | cmin(vert_count,:)=0; 67 | finalcost = 0; 68 | finaltree = qstart; 69 | qnew_cost = 0; 70 | % 71 | % 72 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1), 73 | % because vertecies(1) is the original point 74 | 75 | edges.x = zeros(iterations,2); 76 | edges.y = zeros(iterations,2); 77 | edge_count = 0; 78 | %% 79 | % 80 | % 81 | % ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the 82 | % closetest vertex to vertecies(i+1) 83 | %% 84 | ind_nearest = zeros(iterations,1); 85 | qnew = [0 0]; 86 | colli = 0; 87 | i=1; 88 | randcount=1; 89 | %% 90 | % define variables for optimization 91 | 92 | 93 | %% 94 | % define figure and hold 95 | %% 96 | hold on; 97 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 98 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none'); 99 | scatter(qstart(1), qstart(2), 45, 'o','r','filled'); hold on; 100 | scatter(qgoal(1), qgoal(2), 45, 'o','r','filled'); hold on; 101 | %plot(qstart(1),qstart(2),'r*'); 102 | %plot(qgoal(1),qgoal(2),'r*'); 103 | 104 | %figure('name', 'RRT growing'); 105 | tic; 106 | %% 107 | while i <= iterations 108 | 109 | x_rand = width*rand(); % random point generation 110 | y_rand = height*rand(); 111 | temp.rand= [x_rand,y_rand]; %save temporary random point 112 | randcount=randcount+1; 113 | % drawnow 114 | %scatter(x_rand,y_rand,30,'filled'); 115 | %if random point is goal point this condition is not consider in this 116 | %programme yet 117 | 118 | % find nearest point using KD-Tree method 119 | ind_nearest(i) = knnsearch(vertecies, [x_rand,y_rand]); 120 | qnearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)]; 121 | 122 | qnew_cost1 = eudist(qnearest,[x_rand,y_rand]); 123 | qnew_goal_cost = eudist(qgoal,qnearest); 124 | %find new point using steer funcrtion or BVP 125 | %find new point towards goal if no obstacle eps1>eps 126 | %if there is obstacle eps>eps1 127 | %else use only eps 128 | %A goabbiasing function 129 | colli_random = collision(x1box,y1box,x2box,y2box,[x_rand,y_rand],qnearest); 130 | if colli_random == 0 131 | %this is tem new point 132 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps,eps1); 133 | colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 134 | if colli == 1 135 | % valus of eps and eps1 are inter change to not stay in local minima 136 | qnew = gbsteerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,qnew_goal_cost,qgoal,eps1,eps); 137 | colli1 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 138 | if colli1 == 1 139 | %use normal steer function 140 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 141 | colli2 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 142 | if colli2 == 1 143 | continue 144 | end 145 | end 146 | end 147 | else 148 | qnew = steerfn(temp.rand(1),temp.rand(2),qnearest(1),qnearest(2),qnew_cost1,eps1); 149 | colli3 = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 150 | if colli3 == 1 151 | continue 152 | end 153 | end 154 | 155 | 156 | 157 | %check collision 158 | %colli = collision(x1box,y1box,x2box,y2box,qnew,qnearest); 159 | 160 | % if colli_check == 1 161 | % continue 162 | % end 163 | 164 | %calculate radius for rangesearch 165 | Rrrt = 2*((1+(1/ddimension))^(1/ddimension))*((maxfree/unitballvolume)^(1/ddimension)); 166 | dynamicradius = Rrrt*((log(vert_count)/vert_count)^(1/ddimension)); 167 | %optimization in edge developement 168 | 169 | qnew_cost = eudist(qnearest,qnew); 170 | try1 = abs((dynamicradius-eps)/2); 171 | qrange1=rangesearch(vertecies,qnew,max(dynamicradius,eps)); 172 | %qrange1=rangesearch(vertecies,qnew,try1); 173 | qrange=cell2mat(qrange1); 174 | txxx=[vertecies(qrange,1),vertecies(qrange,2)]; 175 | cmin1 = cmin(ind_nearest(i)) + qnew_cost; 176 | qmin1 = qnearest; 177 | txx = length(qrange); 178 | %rewire process 179 | for k = 1:length(qrange) 180 | % txxz = eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 181 | % tzxzz = qrange(k); 182 | cnew = cmin(qrange(k)) + eudist([vertecies(qrange(k),1),vertecies(qrange(k),2)],qnew); 183 | if cnew < cmin1 184 | tzz = 1; 185 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(k),1),vertecies(qrange(k),2)]) 186 | qmin1 = [vertecies(qrange(k),1),vertecies(qrange(k),2)]; 187 | cmin1 = cnew; 188 | ind_nearest(i) = qrange(k); 189 | end 190 | end 191 | end 192 | 193 | 194 | 195 | %vertecies(vert_count+1,:) = [x_rand, y_rand]; 196 | vertecies(vert_count+1,:) = qnew; 197 | 198 | edges.x(edge_count+1,:) = [qmin1(1), qnew(1)]; 199 | edges.y(edge_count+1,:) = [qmin1(2), qnew(2)]; 200 | edge_count = edge_count + 1; 201 | qtree(vert_count+1,:) = addtreenode(vert_count,qtree,ind_nearest(i)); 202 | cmin(vert_count+1,:) = cmin1; 203 | 204 | for j = 1:length(qrange) 205 | cnear = cmin(qrange(j)); 206 | cmin2 = cmin1 + eudist(qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]); 207 | if cmin2 < cnear 208 | if 0 == collision(x1box,y1box,x2box,y2box,qnew,[vertecies(qrange(j),1),vertecies(qrange(j),2)]) 209 | % ind_nearest(i) = vert_count+1; 210 | qnewrewire = [vertecies(qrange(j),1),vertecies(qrange(j),2)]; 211 | %vert_count_rewire = qrange(j); 212 | qtree(qrange(j),:) = addtreenode(qrange(j)-1,qtree,vert_count+1); 213 | cmin(qrange(j),:) = cmin2; 214 | % rw = cell2mat(qtree(qrange(j))); 215 | %rw1 = cell2mat(qtree(vert_count+1)); 216 | %rw1(length(rw1)+1) = qrange(j); 217 | end 218 | end 219 | end 220 | 221 | 222 | 223 | % find final path 224 | 225 | goalcost = eudist (qnew,qgoal); 226 | 227 | 228 | if goalcost <= eps 229 | finalcost = cmin(vert_count+1,:) + goalcost; 230 | finaltree = createfinalpath(vertecies,qtree,vert_count,qgoal); 231 | optimumtolarance = ((testidealdist*1)/100)+testidealdist; 232 | 233 | if finalcost <= optimumtolarance 234 | disp(optimumtolarance) 235 | break; 236 | end 237 | end 238 | 239 | hold on 240 | drawnow 241 | % scatter(vertecies(vert_count+1,1),vertecies(vert_count+1,2),3,'filled'); 242 | % plot([vertecies(ind_nearest(i),1),vertecies(vert_count+1,1)],[vertecies(ind_nearest(i),2),vertecies(vert_count+1,2)],'LineWidth',0.1,'Color','y'); 243 | line(finaltree(:,1),finaltree(:,2),'LineWidth',1,'Color','g'); 244 | scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on; 245 | plot(edges.x', edges.y'); 246 | vert_count = vert_count + 1; 247 | i=i+1; 248 | 249 | end 250 | 251 | 252 | %if not find final route 253 | if finalcost == 0 254 | ind_nearest1 = knnsearch(vertecies,qgoal); 255 | qnearest = [vertecies(ind_nearest1,1),vertecies(ind_nearest1,2)]; 256 | finalcolli = collision(x1box,y1box,x2box,y2box,qgoal,qnearest); 257 | if finalcolli == 0 258 | 259 | goalcost = eudist (qnearest,qgoal); 260 | finalcost = cmin(ind_nearest1,:) + goalcost; 261 | finaltree = createfinalpath(vertecies,qtree,ind_nearest1-1,qgoal); 262 | else 263 | disp('no nearby points') 264 | end 265 | end 266 | disp(finalcost) 267 | 268 | toc; 269 | clear i x_rand y_rand edge_rand 270 | disp(randcount) 271 | disp(vert_count) 272 | hold on 273 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none'); 274 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none') 275 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on; 276 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on; 277 | scatter(vertecies(:,1), vertecies(:,2), 0.1,linspace(1,10,length(vertecies(:,1))),'filled'); hold on; 278 | plot(edges.x', edges.y','LineWidth',0.01); 279 | line(finaltree(:,1),finaltree(:,2),'LineWidth',2,'Color','r'); 280 | 281 | -------------------------------------------------------------------------------- /project code/RRTtest/addtreenode.m: -------------------------------------------------------------------------------- 1 | function qtreeout = addtreenode( vc,qt,in ) 2 | % add new nodes in TREE with minimum cost 3 | qt1=cell2mat(qt(in)); 4 | 5 | qt1(length(qt1)+1)=vc+1; 6 | 7 | qtreeout=num2cell(qt1,[1 2]); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /project code/RRTtest/addxyz.m: -------------------------------------------------------------------------------- 1 | function [ e,f ] = addxyz(a,b,c,d ) 2 | %UNTITLED3 Summary of this function goes here 3 | % Detailed explanation goes here 4 | e=a+b; 5 | f=c+d; 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /project code/RRTtest/collision.m: -------------------------------------------------------------------------------- 1 | function co = collision( x1box,y1box,x2box,y2box,qnear,nearest ) 2 | %collision check 3 | % create temporary edge between nearest point in vertecies and new point 4 | tempedgex = [nearest(1),qnear(1)]; 5 | tempedgey = [nearest(2),qnear(2)]; 6 | %plot(tempedgex',tempedgey'); 7 | %check insersection point between edges of the obstacle and new point to 8 | %nearest point edge 9 | 10 | [x1i,y1i] = polyxpoly(tempedgex,tempedgey,x1box,y1box); 11 | [x2i,y2i] = polyxpoly(tempedgex,tempedgey,x2box,y2box); 12 | 13 | csi1 = size([x1i,y1i]); 14 | csi2 = size ([x2i,y2i]); 15 | %a=csi1(1); 16 | %b=csi2(1); 17 | if (csi1(1)==0 && csi2(1) ==0) 18 | co = 0; 19 | else 20 | co= 1; 21 | end 22 | 23 | end 24 | 25 | -------------------------------------------------------------------------------- /project code/RRTtest/createfinalpath.m: -------------------------------------------------------------------------------- 1 | function ftree = createfinalpath(ver,qt,vc,qg ) 2 | %create final path 3 | qt1=cell2mat(qt(vc+1)); 4 | %fedx1 = zeros(length(qt1)+1,2); 5 | %fedx1 = zeros(length(qt1)+1,2); 6 | 7 | 8 | for j=1:length(qt1) 9 | qt2(j,:) = [ver(qt1(j),1),ver(qt1(j),2)]; 10 | 11 | end 12 | qt2(length(qt2)+1,:) = qg; 13 | 14 | ftree = qt2; 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /project code/RRTtest/eudist.m: -------------------------------------------------------------------------------- 1 | function d1 = eudist( dist1,dist2 ) 2 | % Find Euclidean Distance between two points in cartesian system 3 | 4 | d0=[dist1;dist2]; 5 | d1=pdist(d0); 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /project code/RRTtest/gaussian.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/gaussian.mlx -------------------------------------------------------------------------------- /project code/RRTtest/gaussian_test.m: -------------------------------------------------------------------------------- 1 | clear; 2 | height = 100; 3 | width = 100; 4 | hold on 5 | qstart = [10 50]; 6 | qgoal = [90 50]; 7 | %axis ([0 width 0 height]); 8 | b=eudist(qstart,qgoal); 9 | 10 | slop = abs(qgoal(2)-qstart(2))/(qgoal(1)-qstart(1)); 11 | qcenter = [qstart(1)+(((qgoal(1)-qstart(1))/b)*(b/2)),qstart(2)+(((qgoal(2)-qstart(2))/b)*(b/2))]; 12 | rotation = [cos(slop) sin(slop);sin(slop) cos(slop)]; 13 | m = 1000; 14 | for i=1:5000 15 | x1 = randn(1,2); 16 | x2=x1*[b^(1/2) 0.5;0.5 1]; 17 | 18 | 19 | 20 | x3=x2*rotation; 21 | x(i,:) = x3+qcenter; 22 | end 23 | %figure; 24 | %plot([qstart(1),qgoal(1)],[qstart(2),qgoal(2)]) 25 | y=[cos(slop) -sin(slop)]; 26 | a1=qstart*rotation; 27 | a2=qgoal*rotation; 28 | %plot([a1(1),a2(1)],[a1(2),a2(2)]) 29 | plot(x(:,1),x(:,2),'k.','MarkerSize',3) 30 | -------------------------------------------------------------------------------- /project code/RRTtest/gaussianwiki.m: -------------------------------------------------------------------------------- 1 | X = -5:0.1:5; 2 | Y = -5:0.1:5; 3 | 4 | %2-d Mean and covariance matrix 5 | MeanVec = [0 0]; 6 | CovMatrix = [1 0.6; 0.6 2]; 7 | 8 | %Get the 1-d PDFs for the "walls" 9 | Z_x = normpdf(X,MeanVec(1), sqrt(CovMatrix(1,1))); 10 | Z_y = normpdf(Y,MeanVec(2), sqrt(CovMatrix(2,2))); 11 | 12 | %Get the 2-d samples for the "floor" 13 | Samples = mvnrnd(MeanVec, CovMatrix, 5000); 14 | 15 | %Get the sigma ellipses by transform a circle by the cholesky decomp 16 | L = chol(CovMatrix,'lower'); 17 | t = linspace(0,2*pi,100); %Our ellipse will have 100 points on it 18 | C = [cos(t) ; sin(t)]; %A unit circle 19 | E1 = 1*L*C; E2 = 2*L*C; E3 = 3*L*C; %Get the 1,2, and 3-sigma ellipses 20 | 21 | figure; hold on; 22 | %Plot the samples on the "floor" 23 | plot3(Samples(:,1),Samples(:,2),zeros(size(Samples,1),1),'k.','MarkerSize',3) -------------------------------------------------------------------------------- /project code/RRTtest/gbsteerfn.m: -------------------------------------------------------------------------------- 1 | function a1=gbsteerfn(x_rand,y_rand,qn1,qn2,qnc,qngc,xgoal,ep,ep1) 2 | N = [0 0]; 3 | xgoal_x = xgoal(1); 4 | xgoal_y = xgoal(2); 5 | % N1=[temp.rand;temp.nearest 6 | %find step size towards qgoal point 7 | if qnc>=eps 8 | %temp1= dist(temp.rand(1),temp.nearest(1)); 9 | % a = x_rand-qn1; 10 | % a2 = y_rand-qn2; 11 | N(1) = abs(qn1 + (((x_rand-qn1)/qnc)*ep)+(((xgoal_x-qn1)/qngc)*ep1)); 12 | N(2) = abs(qn2 + (((y_rand-qn2)/qnc)*ep)+(((xgoal_y-qn2)/qngc)*ep1)); 13 | 14 | else 15 | N(1)=x_rand; 16 | N(2)=y_rand; 17 | end 18 | a1= [N(1),N(2)]; 19 | end 20 | 21 | -------------------------------------------------------------------------------- /project code/RRTtest/html/RRT_nocollision.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | RRT_nocollision
%clear;
 70 | %clc;
 71 | close(findobj('type','figure','name','RRT basic'));
 72 | close(findobj('type','figure','name','RRT growing'));
 73 |

define boundry and area.

height = 1000;
 74 | width = 1000;
 75 | figure('name', 'RRT star');
 76 | axis ([0 width 0 height]);
 77 |

define starting and end point and iteration

qstart = [150,150];
 78 | qgoal = [950,450];
 79 | iterations = 2000;
 80 | 
 81 | %define OBSTACLE
 82 | 
 83 | %OBSTACLE 1
 84 | obs1.x=[450 750];
 85 | obs1.y=[250 850];
 86 | 
 87 | %OBSTACLE 2
 88 | %obs2.x=[];
 89 | %obs2.y=[];
 90 | obs2.x = [0 0];
 91 | obs2.y = [0 0];
 92 | 
 93 | %create obstacle1
 94 | x1box=obs1.x([1 1 2 2 1]);
 95 | y1box=obs1.y([1 2 2 1 1]);
 96 | 
 97 | %create obstacle2
 98 | x2box=obs2.x([1 1 2 2 1]);
 99 | y2box=obs2.y([1 2 2 1 1]);
100 | 
101 | 
102 | % define tree edge, node and vertice
103 | %
104 | % vertecies: An array of Vertrcies' coordinates, sorted by the generated
105 | % order
106 | 
107 | vertecies = qstart;
108 | vert_count = 1;
109 | eps=13;
110 | val = 0;
111 | %
112 | %
113 | % Edges:starting and ending of each edges, note edges(i) corresponds to vertecies(i+1),
114 | % because vertecies(1) is the original point
115 | 
116 | edges.x = zeros(iterations,2);
117 | edges.y = zeros(iterations,2);
118 | edge_count = 0;
119 |

ind_nearest: index to the nearest point. vertecies(ind_nearest(i)) is the closetest vertex to vertecies(i+1)

ind_nearest = zeros(iterations,1);
120 | qnear = [0 0];
121 | colli = 0;
122 | i=1;
123 | randcount=1;
124 |

define figure and hold

hold on;
125 | mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none');
126 | mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none');
127 | scatter(qstart(1), qstart(2), 45, '*','r','LineWidth',1); hold on;
128 | scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on;
129 | 
130 | %figure('name', 'RRT growing');
131 | tic;
132 | 
while i <= iterations
133 | 
134 |     x_rand = width*rand();    % random point generation
135 |     y_rand = height*rand();
136 |     temp.rand= [x_rand,y_rand]; %save temporary random point
137 |     randcount=randcount+1;
138 | 
139 | 
140 |     % find nearest point using KD-Tree method
141 |     [ind_nearest(i),val] = knnsearch(vertecies, [x_rand,y_rand]);
142 |     temp.nearest = [vertecies(ind_nearest(i),1),vertecies(ind_nearest(i),2)];
143 | 
144 | 
145 |     %find new point using steer funcrtion or BVP
146 |     qnear =  steerfn(temp.rand(1),temp.rand(2),temp.nearest(1),temp.nearest(2),val,eps); %this is tem new point
147 | 
148 |     %check collision
149 |     colli = collision(x1box,y1box,x2box,y2box,qnear,temp);
150 | 
151 |     if colli == 1
152 |         continue
153 |     end
154 | 
155 | 
156 |    % end
157 | 
158 | 
159 |     %vertecies(vert_count+1,:) = [x_rand, y_rand];
160 |     vertecies(vert_count+1,:) = qnear;
161 |     vert_count = vert_count + 1;
162 |     edges.x(edge_count+1,:) = [vertecies(ind_nearest(i),1), qnear(1)];
163 |     edges.y(edge_count+1,:) = [vertecies(ind_nearest(i),2), qnear(2)];
164 |     edge_count = edge_count + 1;
165 | 
166 | 
167 | 
168 |     %rewire to find path
169 | 
170 | 
171 | 
172 | 
173 | %scatter(vertecies(i,1), vertecies(i,2), 5,linspace(1,10,length(vertecies(i,1))),'filled'); hold on;
174 | %plot(edges.x', edges.y');
175 |    i=i+1;
176 | 
177 | end
178 | disp(randcount)
179 | toc;
180 | clear i x_rand y_rand edge_rand
181 | 
182 | %mapshow(x1box,y1box,'DisplayType','polygon','LineStyle','none');
183 | %mapshow(x2box,y2box,'DisplayType','polygon','LineStyle','none')
184 | %scatter(qstart(1), qstart(2), 45, 'o','r','LineWidth',1); hold on;
185 | %scatter(qgoal(1), qgoal(2), 45, 'o','r','LineWidth',1); hold on;
186 | scatter(vertecies(:,1), vertecies(:,2), 5,linspace(1,10,length(vertecies(:,1))),'filled'); hold on;
187 | plot(edges.x', edges.y');
188 | 
        2445
189 | 
190 | Elapsed time is 5.564359 seconds.
191 |


Published with MATLAB® R2017a

-------------------------------------------------------------------------------- /project code/RRTtest/html/RRT_nocollision.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/html/RRT_nocollision.png -------------------------------------------------------------------------------- /project code/RRTtest/html/RRT_nocollision_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/html/RRT_nocollision_01.png -------------------------------------------------------------------------------- /project code/RRTtest/html/RRT_nocollision_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/html/RRT_nocollision_02.png -------------------------------------------------------------------------------- /project code/RRTtest/html/RRT_nocollision_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/html/RRT_nocollision_03.png -------------------------------------------------------------------------------- /project code/RRTtest/nocollision.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/nocollision.mlx -------------------------------------------------------------------------------- /project code/RRTtest/num2cell_cell2mat.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/num2cell_cell2mat.mlx -------------------------------------------------------------------------------- /project code/RRTtest/pathfinish.m: -------------------------------------------------------------------------------- 1 | function [ soltree ] = pathfinish( soltree1 ) 2 | % path finishing so that minimum cost path can acheive 3 | soltree(1,:) = 4 | for l= 1:length(soltree) 5 | soltree = 6 | end 7 | 8 | end 9 | 10 | -------------------------------------------------------------------------------- /project code/RRTtest/samplepoint.m: -------------------------------------------------------------------------------- 1 | function [xran,yran,eps,eps1 ] = samplepoint(hi,wi,cmax,cid,slop,qcen,eps,eps1) 2 | %create sampling point based on the first path 3 | % use the initial path value to generate normal distribution(gaussian distribution) 4 | % so that limit the state space which help to get more cost effective path 5 | % in less time 6 | 7 | 8 | 9 | 10 | 11 | if cmax >= 1 12 | eps=(cmax*3)/100; 13 | eps1=(cmax*5)/100; 14 | rotation = [cos(slop) sin(slop);sin(slop) cos(slop)]; 15 | trandia = cmax/2; 16 | conjudia = (((((cmax)^2)-((cid)^2))^(1/2))/2)/2; 17 | ran = randn(1,2); %create random point 18 | ran1 = ran*[trandia 0;0 conjudia]; %calculate with 19 | 20 | ran2 = ran1*rotation; 21 | ran3 = ran2+qcen; 22 | xran = ran3(1); 23 | yran = ran3(2); 24 | 25 | else 26 | xran = wi*rand(); % random point generation 27 | yran = hi*rand(); 28 | end 29 | 30 | 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /project code/RRTtest/startup.m: -------------------------------------------------------------------------------- 1 | set(0,'DefaultFigureWindowStyle','docked') -------------------------------------------------------------------------------- /project code/RRTtest/steerfn.m: -------------------------------------------------------------------------------- 1 | function a1=steerfn(x_rand,y_rand,qn1,qn2,qnc,eps) 2 | N = [0 0]; 3 | % N1=[temp.rand;temp.nearest 4 | %find step size towards qrandom point 5 | if qnc>=eps 6 | %temp1= dist(temp.rand(1),temp.nearest(1)); 7 | N(1) = abs(qn1 + ((x_rand-qn1)*eps)/qnc); 8 | N(2) = abs(qn2 + ((y_rand-qn2)*eps)/qnc); 9 | else 10 | N(1)=x_rand; 11 | N(2)=y_rand; 12 | end 13 | a1= [N(1),N(2)]; 14 | end 15 | 16 | -------------------------------------------------------------------------------- /project code/RRTtest/test.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/test.mlx -------------------------------------------------------------------------------- /project code/RRTtest/test2.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/test2.mlx -------------------------------------------------------------------------------- /project code/RRTtest/test3.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/test3.mlx -------------------------------------------------------------------------------- /project code/RRTtest/test4.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/test4.mlx -------------------------------------------------------------------------------- /project code/RRTtest/test5.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/test5.mlx -------------------------------------------------------------------------------- /project code/RRTtest/test_graph.m: -------------------------------------------------------------------------------- 1 | G := Graph([a, b, c, d], [[a, b], [a, c], [b, c], [c, d]], EdgeWeights = [2, 1, 3, 2],EdgeCosts = [1, 3,1, 2],Directed): -------------------------------------------------------------------------------- /project code/RRTtest/testwith_1500_20_nerrowpassage.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/testwith_1500_20_nerrowpassage.fig -------------------------------------------------------------------------------- /project code/RRTtest/whileloop.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/project code/RRTtest/whileloop.m -------------------------------------------------------------------------------- /project code/readme.txt: -------------------------------------------------------------------------------- 1 | RRT* project code files with test 2 | -------------------------------------------------------------------------------- /update on thesis with sign page.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jparth1593/RRT-using-Matlab/3e890c859b62778c661dabfb807280e8cb70dcd6/update on thesis with sign page.pdf --------------------------------------------------------------------------------