From 019b6f3b623834d366cf725906520deaa44f3ab4 Mon Sep 17 00:00:00 2001 From: Adrien Lasserre Date: Thu, 12 Jan 2023 14:00:46 +0100 Subject: [PATCH] documenting --- buildPRM.m | 18 ++++++++++----- mapTest.mat | 26 ++++++++++----------- planPathPRM.m | 62 +++++++++++++++++++++++++++------------------------ qGraph.m | 24 +++++++++++--------- 4 files changed, 72 insertions(+), 58 deletions(-) diff --git a/buildPRM.m b/buildPRM.m index d8afab3..9ebfd5e 100644 --- a/buildPRM.m +++ b/buildPRM.m @@ -1,10 +1,16 @@ ## Author: adril ## 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) diff --git a/mapTest.mat b/mapTest.mat index fc15146..e21f3bf 100644 --- a/mapTest.mat +++ b/mapTest.mat @@ -1,25 +1,25 @@ -# Created by Octave 6.2.0, Tue Jan 10 16:37:33 2023 CET +# Created by Octave 7.3.0, Wed Jan 11 15:52:59 2023 GMT # 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 diff --git a/planPathPRM.m b/planPathPRM.m index a4e7f08..7e903e3 100644 --- a/planPathPRM.m +++ b/planPathPRM.m @@ -2,49 +2,53 @@ ## 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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function planPathPRM (mapFilePath, startPoint, endPoint) - - mapFile = load(mapFilePath); - - S = startPoint; - G = endPoint; - - n = columns(mapFile.MatrixOfLinks); - connectionMatrix = zeros(n+2,n+2); - connectionMatrix(2:n+1,2:n+1) = mapFile.MatrixOfLinks; - - points2Dmap = mapFile.Points'; - indexClosestPointS = findClosestPoint(S,points2Dmap) + 1 - indexClosestPointG = findClosestPoint(G,points2Dmap) + 1 - - connectionMatrix(1, indexClosestPointS) = 1; + mapFile = load(mapFilePath); + + S = startPoint; %define the start as S + G = endPoint; %define the goal as G + + 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'; %get the Points, but transposed (to use the findClosestPoint function) + + 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; %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 - - [numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) - - - dijkstra(numberOfNodes,visibilityGraph); - + + points2D = [S; points2Dmap; G] %store the points (with S and G) + connectionMatrix %debugging + + [numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) %create the visibilityGraph + + + dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning + endfunction diff --git a/qGraph.m b/qGraph.m index ec03b8d..d9c7924 100644 --- a/qGraph.m +++ b/qGraph.m @@ -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