adrien.lasserre
/
motion_planning_td5
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

finished RRT

This commit is contained in:
Adrien LASSERRE 2023-01-19 11:26:33 +01:00
parent 3ed014ca0a
commit 9565a01512
3 changed files with 122 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
buildRRT.m
View File

@ -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

26
mapTest.mat
View File

@ -1,25 +1,25 @@
# Created by Octave 7.3.0, Wed Jan 11 15:52:59 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
# Created by Octave 7.3.0, Thu Jan 19 11:20:48 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
# 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

40
planPathPRM.m
View File

@ -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