From 9565a01512e92e83fda4148cadfd09797ccad972 Mon Sep 17 00:00:00 2001 From: Adrien Lasserre Date: Thu, 19 Jan 2023 11:26:33 +0100 Subject: [PATCH] finished RRT --- buildRRT.m | 73 ++++++++++++++++++++++++++++++++++++++++++++++++++- mapTest.mat | 26 +++++++++--------- planPathPRM.m | 40 +++++++++++++++++++++++++--- 3 files changed, 122 insertions(+), 17 deletions(-) diff --git a/buildRRT.m b/buildRRT.m index ec75b29..0380f83 100644 --- a/buildRRT.m +++ b/buildRRT.m @@ -28,7 +28,7 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal) i = 1; %while nbPoints=nbPoints+2;%adding the starting point %Points=zeros(nbPoints, 2); - MatrixOfLinks=zeros(nbPoints, nbPoints); + %MatrixOfLinks=zeros(nbPoints, nbPoints); alpha=[0;0]; d=[0;0]; a=[L1;L2]; @@ -108,6 +108,10 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal) intersect=1; else drawEdge(L); + MatrixOfLinks(i+1, i+1)=1; + MatrixOfLinks(i+1,i)=1; + MatrixOfLinks(i,i+1)=1; + i=i+1; break; endif @@ -115,4 +119,71 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal) endif endif endwhile + +## Write the path planning here + +## Start from goal, towards start, going to the previous point each time +## check everytime if the start is not already accessible, and if so draw a line +## prendre le goal, trouver le point auquel il est attach, y aller + + index_goal = columns(MatrixOfLinks); + index = index_goal; + +##Draw the clean figure + figure 2; hold on; + b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles + hold on; + c=drawCircle(0, 0, L2-L1); + hold on; + %creates the lines defining the prohibited areas + top_line = createLine([0,L1,1,0]); + bottom_line = createLine([0,-L1,1,0]); + drawLine(top_line); + hold on; + drawLine(bottom_line); + center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]; + drawPolygon(center_box); + hold on; + + poly_a=circleToPolygon([0 0 L2-L1], 32);%create a polygon for matgeom with the circle info (smaller one) + poly_b=circleToPolygon([0 0 L1+L2], 32);%bigger one radius=3 + + %set the starting point + %Points(1,1:2) = start; + + %draw the start and goal points + %drawPoint(start(1, 1), start(1,2)); + Points_2(1, 1:2)=goal; + drawPoint(Points_2(1, 1:2), 'ro'); + z=1; + MatrixOfLinks + while Points_2(z, 1:2)!=Points(1, 1:2) +## Start the path planning +##Check if we can reach the start + L = createEdge(start, Points_2(z, 1:2)); + if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1) + intersect=1; + else + drawEdge(L, 'r'); + break; + endif + ##If not, then check the next point + for i=1:index-1 + if MatrixOfLinks(index, i)==1 + disp('found a point connected'); + Points_2(z+1, 1:2)=Points(i, 1:2); + disp('new point added, and drawn'); + drawPoint(Points_2(z+1, 1:2), 'r'); + L=createEdge(Points_2(z+1, 1:2), Points_2(z, 1:2)); + drawEdge(L, 'r'); + index=i; + break; + endif + endfor + z=z+1; + disp('next step') + endwhile + + drawPoint(start, 'ro'); + endfunction diff --git a/mapTest.mat b/mapTest.mat index e21f3bf..13caa76 100644 --- a/mapTest.mat +++ b/mapTest.mat @@ -1,25 +1,25 @@ -# Created by Octave 7.3.0, Wed Jan 11 15:52:59 2023 GMT +# Created by Octave 7.3.0, Thu Jan 19 11:20:48 2023 GMT # name: Points # type: matrix # rows: 2 # columns: 10 - 1.0102396615914184 0.52999250891267136 -1.4938105820093814 0.39973309803001944 1.6575303500294156 2.1811052817711225 2.3728058316176091 -1.7930511997776755 -1.1301884102529285 0.37332095642010499 - -0.38102794441973026 -1.4202013308006278 0.272919986694101 -1.7257148318842865 1.9757520956131371 0.5058894523007601 1.417055067995548 -1.7814659113455531 1.964795369645167 -1.5680163207760147 + 0.19697057332778345 -1.1771718145368819 -2.887110614207359 -1.4285411653803914 -1.6523013408997613 -1.363706683365784 1.4748850427562932 -1.1404559732519735 -2.1736190854336215 2.7723194289404676 + 1.745837315672909 0.75220366971764996 0.5218447246367357 -0.15692440100160332 1.3102147601497778 0.29066752826903497 -0.013557097054121003 -1.4995957486596576 -1.718447568352325 -0.91198013843207804 # name: MatrixOfLinks # type: matrix # rows: 10 # columns: 10 - 1 0 0 0 1 1 1 0 0 0 - 0 1 0 1 0 0 0 1 0 1 - 0 0 1 0 0 0 0 1 1 0 - 0 1 0 1 0 0 0 1 0 1 - 1 0 0 0 1 1 1 0 1 0 - 1 0 0 0 1 1 1 0 1 0 - 1 0 0 0 1 1 1 0 1 0 - 0 1 1 1 0 0 0 1 1 1 - 0 0 1 0 1 1 1 1 1 0 - 0 1 0 1 0 0 0 1 0 1 + 1 0 1 0 1 0 0 0 0 0 + 0 1 1 1 1 1 0 1 1 0 + 1 1 1 1 1 1 0 1 1 0 + 0 1 1 1 1 1 0 1 1 0 + 1 1 1 1 1 1 0 1 1 0 + 0 1 1 1 1 1 0 1 1 0 + 0 0 0 0 0 0 1 0 0 1 + 0 1 1 1 1 1 0 1 1 1 + 0 1 1 1 1 1 0 1 1 1 + 0 0 0 0 0 0 1 1 1 1 diff --git a/planPathPRM.m b/planPathPRM.m index 7e903e3..5b551e0 100644 --- a/planPathPRM.m +++ b/planPathPRM.m @@ -2,7 +2,7 @@ ## Created: 2023-01-08 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%function planPathPRM(mapFilePath, startPoint, endPoint) +%function planPathPRM(mapFilePath, startPoint, endPoint, L1, L2) % % Task: Establish the path from the starting point to the goal, using the points defined with the PRM method % and using the Dijkstra method. @@ -20,7 +20,7 @@ % 08/01/2023 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function planPathPRM (mapFilePath, startPoint, endPoint) +function planPathPRM (mapFilePath, startPoint, endPoint, L1, L2) mapFile = load(mapFilePath); @@ -49,6 +49,40 @@ function planPathPRM (mapFilePath, startPoint, endPoint) [numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) %create the visibilityGraph - dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning + [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning + + figure 3; hold on; + b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles + hold on; + c=drawCircle(0, 0, L2-L1); + hold on; + %creates the lines defining the prohibited areas + top_line = createLine([0,L1,1,0]); + bottom_line = createLine([0,-L1,1,0]); + drawLine(top_line); + hold on; + drawLine(bottom_line); + center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]; + drawPolygon(center_box); + hold on; + + + Points_path=zeros(size(nodeTrajectory(1,2))+1,2); + drawPoint(S, 'ro'); + + for i=1:size(nodeTrajectory, 2) + drawPoint(points2D(nodeTrajectory(1, i), 1:2), 'r'); + Points_path(i, 1:2)=points2D(nodeTrajectory(1, i), 1:2); + endfor + + drawPoint(G, 'ro'); + + for j=2:size(Points_path, 1) + L=createEdge(Points_path(j-1, 1:2), Points_path(j, 1:2)); + drawEdge(L, 'r'); + endfor + indexClosestPointFinal = findClosestPoint(G,Points_path); + L=createEdge(Points_path(indexClosestPointFinal, 1:2), G); + drawEdge(L, 'r'); endfunction