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

documenting

This commit is contained in:
Adrien LASSERRE 2023-01-12 14:00:46 +01:00
parent 8607da8776
commit 019b6f3b62
4 changed files with 72 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
buildPRM.m
View File

@ -1,10 +1,16 @@
## Author: adril <adril@LAPTOP-EJ1AIJHT>
## Created: 2022-12-06
## For more info:
## Check library matgeom: https://octave.sourceforge.io/matgeom/overview.html
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
%
% Task:
%The goal of this function is to use the Probabilistic RoadMaps procedure,
%to build a map of both the Cartesian space and the C-space representations,
%with the number of points chosen by the user to establish the different
%options for the path to follow, to get to a specific goal (cartesian point)
%
% Inputs:
% - rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2
@ -13,19 +19,18 @@
% - MapFilename : the name of the file to be saved for the map
%
% Outputs:
% - None
% - .mat file in the workspace
%
% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
% 06/12/2022
%
% Check library matgeom: https://octave.sourceforge.io/matgeom/overview.html
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
hold off;
i = 1;
Points=zeros(2, nbPoints);
i = 1; %initialize
Points=zeros(2, nbPoints); %matrix of 2xnbPoints
MatrixOfLinks=zeros(nbPoints, nbPoints);
alpha=[0;0];
d=[0;0];
@ -34,6 +39,7 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
figure 1; hold on;
b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
## used for drawing the robot definition
hold on;
c=drawCircle(0, 0, L2-L1);
hold on;
@ -43,11 +49,11 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
drawLine(top_line);
hold on;
drawLine(bottom_line);
%creates the central box that's prohibited
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
@ -109,7 +115,7 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
endif
end
MatrixOfLinks
MatrixOfLinks;
Points %display the points vector to check on the graph
qGraph (Q_storage, nbPoints, MatrixOfLinks)

26
mapTest.mat
View File

@ -1,25 +1,25 @@
# Created by Octave 6.2.0, Tue Jan 10 16:37:33 2023 CET <gwenn-durpoix@gwenns-macbook-air.home>
# Created by Octave 7.3.0, Wed Jan 11 15:52:59 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
# name: Points
# type: matrix
# rows: 2
# columns: 10
-2.9278270545629583 -1.6390603492396598 0.54333480331087836 0.74523888813119699 1.0407433467701765 2.8666794638359794 -0.17021771427410731 1.6710330436515206 1.6120313889005971 -1.6706558687217048
0.6489436540224216 -0.27473874500310402 -1.1849051429926958 -1.8204567844796986 -0.75288499470420012 0.051534548934427216 1.0909084733393528 0.26900756558602867 0.60178471895213748 -0.57476808418975023
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
# name: MatrixOfLinks
# type: matrix
# rows: 10
# columns: 10
1 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 1
0 0 1 1 0 0 0 0 0 0
0 0 1 1 0 1 0 1 1 0
0 0 0 0 1 1 0 1 1 0
0 0 0 1 1 1 0 1 1 0
0 0 0 0 0 0 1 0 0 0
0 0 0 1 1 1 0 1 1 0
0 0 0 1 1 1 0 1 1 0
0 1 0 0 0 0 0 0 0 1
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

40
planPathPRM.m
View File

@ -2,17 +2,21 @@
## Created: 2023-01-08
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function planPathPRM()
%function planPathPRM(mapFilePath, startPoint, endPoint)
%
% Task:
% Task: Establish the path from the starting point to the goal, using the points defined with the PRM method
% and using the Dijkstra method.
%
% Inputs:
% -
% - mapFilePath : the path to get the .mat file
% - startPoint : our starting point for the path planning
% - endPoint : our final goal
%
% Outputs:
% -
% - None
%
% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
% Adrien Lasserre (adrien.lasserre@ecam.fr) &
% Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
% 08/01/2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -20,31 +24,31 @@ function planPathPRM (mapFilePath, startPoint, endPoint)
mapFile = load(mapFilePath);
S = startPoint;
G = endPoint;
S = startPoint; %define the start as S
G = endPoint; %define the goal as G
n = columns(mapFile.MatrixOfLinks);
connectionMatrix = zeros(n+2,n+2);
connectionMatrix(2:n+1,2:n+1) = mapFile.MatrixOfLinks;
n = columns(mapFile.MatrixOfLinks); %get the size of the MatrixOfLinks
connectionMatrix = zeros(n+2,n+2); %initialize a new connection matrix, with size n+2 to enter the S and G
connectionMatrix(2:n+1,2:n+1) = mapFile.MatrixOfLinks;%put the MatrixOfLinks inside
points2Dmap = mapFile.Points';
points2Dmap = mapFile.Points'; %get the Points, but transposed (to use the findClosestPoint function)
indexClosestPointS = findClosestPoint(S,points2Dmap) + 1
indexClosestPointG = findClosestPoint(G,points2Dmap) + 1
indexClosestPointS = findClosestPoint(S,points2Dmap) + 1 %+1 as in the first indexes of the connectionMatrix we put the S
indexClosestPointG = findClosestPoint(G,points2Dmap) + 1 %display those results
connectionMatrix(1, indexClosestPointS) = 1;
connectionMatrix(1, indexClosestPointS) = 1; %use the previous results to update the connectionMatrix
connectionMatrix(indexClosestPointS, 1) = 1;
connectionMatrix(1, 1) = 1;
connectionMatrix(n+2, indexClosestPointG) = 1;
connectionMatrix(indexClosestPointG, n+2) = 1;
connectionMatrix(n+2, n+2) = 1;
points2D = [S; points2Dmap; G]
connectionMatrix
points2D = [S; points2Dmap; G] %store the points (with S and G)
connectionMatrix %debugging
[numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D)
[numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) %create the visibilityGraph
dijkstra(numberOfNodes,visibilityGraph);
dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning
endfunction

24
qGraph.m
View File

@ -4,10 +4,14 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
%
% Task:
% Task: Create the C-Space graph for the PRM method, with the selected joint
% values, that weren't outside of bounds.
%
% Inputs:
% -
% - Q : matrix that stores the different joint values for each point.
% - nbPoints : number of points required
% - mat : the Matrix of links, that stores the information on which point is
% linked to which other one.
%
% Outputs:
% - None
@ -20,16 +24,16 @@
function qGraph (Q, nbPoints, mat)
hold off;
figure 2
for i=1:nbPoints
figure 2 %launch a new figure
for i=1:nbPoints %double for loop to check every point for every link
for j=1:nbPoints
drawPoint(Q(1,i), Q(2,i));
current_point=[Q(1, i), Q(2, i)];
previous_point=[Q(1, j), Q(2, j)];
if mat(i, j)==1 & mat(j, i)==1
L = createEdge(current_point, previous_point);
drawPoint(Q(1,i), Q(2,i)); %draw the point in C-space
current_point=[Q(1, i), Q(2, i)]; %change the current point
previous_point=[Q(1, j), Q(2, j)]; %change the previous point
if mat(i, j)==1 & mat(j, i)==1 %if there is a link between the two
L = createEdge(current_point, previous_point); %then create a line between both
hold on;
drawEdge(L);
drawEdge(L); %and draw it on the figure.
endif
endfor
endfor