TD5 finished

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
+      no_collision = true;
-			%connectionMap(l,m) = 1;
+      % crosses x =-1000 ???
-			% else
+      if (min(A(1), B(1)) <= -1000 && max(A(1), B(1)) >= -1000)
-			%connectionMap(l,m) = 0;
+        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');

clear.m

@@ -1,4 +0,0 @@
-clear
-clear all
-clc
-

connectionMapFile.mat

@@ -0,0 +1,17 @@
+# Created by Octave 8.4.0, Sun Dec 03 22:49:46 2023 GMT <unknown@LAPTOP-T>
+# 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
+
+

dataFile.mat

@@ -0,0 +1,25 @@
+# Created by Octave 8.4.0, Sun Dec 03 23:07:59 2023 GMT <unknown@LAPTOP-T>
+# 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
+
+

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');
+

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