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

planPathPRM working properly

This commit is contained in:
Gwenn DURPOIX-ESPINASSON 2023-01-10 16:39:56 +01:00
parent f1421e292d
commit 94d114001d
3 changed files with 44 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored Normal file
View File

Binary file not shown.

26
mapTest.mat
View File

@ -1,25 +1,25 @@
# Created by Octave 7.3.0, Tue Jan 10 14:36:10 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
# Created by Octave 6.2.0, Tue Jan 10 16:37:33 2023 CET <gwenn-durpoix@gwenns-macbook-air.home>
# name: Points
# type: matrix
# rows: 2
# columns: 10
-2.9722694997088999 2.2027074585576791 -2.4163314018998832 -1.563641777935366 1.8247189104154553 -0.17308723248350155 0.24577290552178044 -0.80469834936605633 2.0135592026997866 -0.36560396489119051
0.047890118375208762 -0.42713698315704518 -0.30925089631194735 -1.216804326954257 1.3865824550438368 1.3067789455813377 -1.2708472666201218 1.1717992231942889 -0.02269206049426975 1.84133215165838
-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
# name: MatrixOfLinks
# type: matrix
# rows: 10
# columns: 10
1 0 1 1 0 0 0 1 0 1
0 1 0 0 1 0 0 0 1 0
1 0 1 1 0 0 0 0 0 1
1 0 1 1 0 0 1 0 0 0
0 1 0 0 1 1 0 1 1 1
0 0 0 0 1 1 0 1 0 1
0 0 0 1 0 0 1 0 0 0
1 0 0 0 1 1 0 1 0 1
0 1 0 0 1 0 0 0 1 0
1 0 1 0 1 1 0 1 0 1
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

34
planPathPRM.m
View File

@ -2,7 +2,7 @@
## Created: 2023-01-08
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
%function planPathPRM()
%
% Task:
%
@ -16,7 +16,35 @@
% 08/01/2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function planPathPRM ()
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;
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);
endfunction