From 2e4611af0d826ac24645a865dba46468c77c284f Mon Sep 17 00:00:00 2001 From: Thomas_P Date: Sun, 3 Dec 2023 23:40:43 +0100 Subject: [PATCH] TD5 finished --- buildPRM.m | 50 +++++++++++++++++++++++++++--- clear.m | 4 --- connectionMapFile.mat | 17 +++++++++++ dataFile.mat | 25 +++++++++++++++ dijkstra.m | 71 +++++++++++++++++++++++++++++++++++++++++++ planPathPRM.m | 44 +++++++++++++++++++++++++++ 6 files changed, 202 insertions(+), 9 deletions(-) delete mode 100644 clear.m create mode 100644 connectionMapFile.mat create mode 100644 dataFile.mat create mode 100644 dijkstra.m create mode 100644 planPathPRM.m diff --git a/buildPRM.m b/buildPRM.m index f078a1d..2965f08 100644 --- a/buildPRM.m +++ b/buildPRM.m @@ -9,6 +9,7 @@ function buildPRM(L1, L2, nbPoints) % -nbVP : Number of Valid Points % %Outputs: +% - .mat file with the connectionMap % %Author : Thomas Périn, thomas.perin@ecam.fr %Date/ 22/11/2023 @@ -102,10 +103,11 @@ subplot(1,2,2); % creates a connection map connectionMap = zeros(nbPoints, nbPoints); +xy_valid = []; for l=1:size(q1q2_valid,2) for m=1:size(q1q2_valid,2) - if (l ~=m) + if (l ~= m) % creates the DH table theta = q1q2_valid(:,l); d = [0; 0]; @@ -125,11 +127,49 @@ for l=1:size(q1q2_valid,2) B = wTee(1:2,end); % computes y =ax + b + a = (A(2)-B(2))/(A(1)-B(1)); + b = A(2) - a * A(1); - % if no collision - %connectionMap(l,m) = 1; - % else - %connectionMap(l,m) = 0; + no_collision = true; + % crosses x =-1000 ??? + if (min(A(1), B(1)) <= -1000 && max(A(1), B(1)) >= -1000) + if ((a * (-1000) + b ) < -1000) || ((a * (-1000) + b ) > 1000) + no_collision = true; + else + no_collision = false; + end + end + % crosses x =1000 ??? + if (min(A(1), B(1)) <= 1000 && max(A(1), B(1)) >= 1000 && no_collision) + if ((a * (1000) + b ) < -1000) || ((a * (1000) + b ) > 1000) + no_collision = true; + else + no_collision = false; + end + end + % crosses y =-1000 ??? + if (min(A(2), B(2)) <= -1000 && max(A(2), B(2)) >= -1000 && no_collision) + if (((-1000-b)/a ) < -1000) || (((-1000-b)/a ) > 1000) + no_collision = true; + else + no_collision = false; + end + end + + if no_collision + connectionMap(l,m) = 1; + subplot(1,2,1); + plot([q1q2_valid(1,l) q1q2_valid(1,m)],[q1q2_valid(2,l) q1q2_valid(2,m)], '-g'); hold on; + subplot(1,2,2); + plot([A(1) B(1)],[A(2) B(2)], '-g'); hold on; + else + connectionMap(l,m) = 0; + end end end + xy_valid = [xy_valid [A(1); A(2)]]; end +connectionMap +xy_valid +% Save the matrix to a .mat file +save('dataFile.mat', 'connectionMap', 'xy_valid'); diff --git a/clear.m b/clear.m deleted file mode 100644 index 5d5bca3..0000000 --- a/clear.m +++ /dev/null @@ -1,4 +0,0 @@ -clear -clear all -clc - diff --git a/connectionMapFile.mat b/connectionMapFile.mat new file mode 100644 index 0000000..1fe85d8 --- /dev/null +++ b/connectionMapFile.mat @@ -0,0 +1,17 @@ +# Created by Octave 8.4.0, Sun Dec 03 22:49:46 2023 GMT +# name: connectionMap +# type: matrix +# rows: 10 +# columns: 10 + 0 0 0 0 0 0 0 0 1 1 + 0 0 0 1 1 0 0 1 0 0 + 0 0 0 0 0 1 1 0 1 0 + 0 1 0 0 1 0 1 1 0 0 + 0 1 0 1 0 0 0 1 0 0 + 0 0 1 0 0 0 1 0 1 0 + 0 0 1 1 0 1 0 0 1 0 + 0 1 0 1 1 0 0 0 0 0 + 1 0 1 0 0 1 1 0 0 1 + 1 0 0 0 0 0 0 0 1 0 + + diff --git a/dataFile.mat b/dataFile.mat new file mode 100644 index 0000000..9469934 --- /dev/null +++ b/dataFile.mat @@ -0,0 +1,25 @@ +# Created by Octave 8.4.0, Sun Dec 03 23:07:59 2023 GMT +# name: connectionMap +# type: matrix +# rows: 10 +# columns: 10 + 0 0 1 0 1 0 1 1 1 1 + 0 0 0 1 0 1 1 0 0 0 + 1 0 0 0 1 0 1 1 0 0 + 0 1 0 0 1 1 1 0 0 0 + 1 0 1 1 0 0 1 1 0 0 + 0 1 0 1 0 0 1 0 1 0 + 1 1 1 1 1 1 0 1 0 0 + 1 0 1 0 1 0 1 0 1 1 + 1 0 0 0 0 1 0 1 0 1 + 1 0 0 0 0 0 0 1 1 0 + + +# name: xy_valid +# type: matrix +# rows: 2 +# columns: 10 + -1183.1144222084818 1264.4708348085574 -334.39822518231665 2171.4653844739678 655.97776462668855 1073.0376457230295 2337.6632188169524 -2724.1855429400352 -1270.0317481226446 -1301.9713130947187 + 1782.3056782605233 122.44277855170276 1340.3707372349479 245.01665496817861 1581.186555640114 -1823.1319004730015 1824.6138407095095 1199.9716377405398 -1315.9879394455597 -147.11573260990281 + + diff --git a/dijkstra.m b/dijkstra.m new file mode 100644 index 0000000..e5afc87 --- /dev/null +++ b/dijkstra.m @@ -0,0 +1,71 @@ +function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph) +% +% Task: Perform the Dijkstra algorithm on a given visibility graph +% +% Inputs: +% -nbNodes: number of nodes of the graph excluding the starting and goal points +% -visibilityGraph: a matrix containing the distance between connected nodes +% (NaN refers to not connected nodes) +% The matrix has a size of (nbNodes+2)x(nbNodes+2) +% The first row/col corresponds to the Starting point, the last row/col to the Goal point. +% +% Outputs: +% - distanceToNode: distance between the current node and its parent +% - parentOfNode: index of the parent node for each node +% - nodeTrajectory: best trajectory +% +% Guillaume Gibert (guillaume.gibert@ecam.fr) +% 17/03/2021 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +constantLargeDitance=10000; + +visitedNodes = zeros(1, nbNodes+2); +distanceToNode = constantLargeDitance*ones(1, nbNodes+2); +distanceToNode(1) = 0; +parentOfNode = zeros(1, nbNodes+2); + +fprintf('##Starting Dijkstra''s algorithm...\n') + +while (sum(visitedNodes(:)==0)) + thresholdDistance = constantLargeDitance+1; + for l_node=1:nbNodes+2 + %l_node + if (visitedNodes(l_node)==0 && distanceToNode(l_node) < thresholdDistance) + minIndex = l_node; + thresholdDistance = distanceToNode(l_node); + end + end + + fprintf('-->Visiting N%d\n', minIndex-1) + + visitedNodes(minIndex) = 1; + for l_node=1:nbNodes+2 + %l_node + if (l_node~=minIndex && ~isnan(visibilityGraph(minIndex, l_node))) + distance = distanceToNode(minIndex) + visibilityGraph(minIndex,l_node); + if (distance < distanceToNode(l_node)) + distanceToNode(l_node) = distance; + parentOfNode(l_node) = minIndex; + end + end + end +end +fprintf('##Dijkstra''s algorithm is done!\n') +fprintf('##Results\n') +fprintf('Minimal distance to target: %d\n', distanceToNode(nbNodes+2)) +nodeIndex = nbNodes+2; +nodeTrajectory = []; +while(nodeIndex~=1) + nodeIndex = parentOfNode(nodeIndex); + nodeTrajectory = [nodeTrajectory nodeIndex]; +end +fprintf('S-->'); +for l_node=2:length(nodeTrajectory) + fprintf('N%d-->', nodeTrajectory(length(nodeTrajectory)-(l_node-1))-1); +end +fprintf('G\n'); +fprintf('########\n'); + diff --git a/planPathPRM.m b/planPathPRM.m new file mode 100644 index 0000000..14f3bf3 --- /dev/null +++ b/planPathPRM.m @@ -0,0 +1,44 @@ +function retval = planPathPRM () +%%%%%%%%%%%%%%%%%%% +%planPathPRM () +%ex. planPathPRM() +% +%Inputs: +% +% +%Outputs: +% +%Author : Thomas Périn, thomas.perin@ecam.fr +%Date/ 22/11/2023 +%%%%%%%%%%%%%%%%%%% + +load('dataFile.mat') + +start = [2000; 0]; +goal = [-2000; 0]; + +nbNodes = size(xy_valid,2); + +visibilityGraph = zeros(size(xy_valid,2)+2,size(xy_valid,2)+2); + +for l = 1:(size(visibilityGraph,2)-1) + for m = 1:(size(visibilityGraph,2)-1) + if l ~= m + if l == 1 + visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, m-1))^2 + xy_valid(2, m-1)^2); + elseif m == 1 + visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, l-1))^2 + xy_valid(2, l-1)^2); + elseif connectionMap(l-1, m-1) == 1 + visibilityGraph(l , m) = sqrt((xy_valid(1, m-1)-xy_valid(1, l-1))^2 + (xy_valid(2, m-1)-xy_valid(2, l-1))^2); + end + end + end +end +for s = 2:(size(visibilityGraph,2)-1) + visibilityGraph(s , 12) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2); + visibilityGraph(12 , s) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2); +end +visibilityGraph +[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph); + +endfunction