qGraph function added for C-space

buildPRM.m

@@ -22,7 +22,7 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
-  
+
   hold off;
   i = 1;
   Points=zeros(2, nbPoints);
@@ -43,11 +43,11 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
   drawLine(top_line);
   hold on;
   drawLine(bottom_line);
-  center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]
+  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
 
@@ -56,7 +56,7 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
     %creates random angle values for Q1 and Q2
     Q=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
     theta=[Q(1,1);Q(2,1)];
-    
+
     OutOfRange=0; %set the boolean
 
     bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
@@ -72,29 +72,29 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
     endif
 
     if (OutOfRange==0)
-      
+      Q_storage(1:2, i)=Q;
       Points(1:2, i)=jTee; %assign the current random point to the Points matrix
       MatrixOfLinks(i, i)=1; %it doesnt intersect with the obstacles when compared to itself
-      
+
       if i == 1
         drawPoint(jTee(1,1), jTee(2,1)); %draw the point
       endif
-      
+
       if i>=2 %i=1 useless
-        
+
         for j=1:i %compare with the other points to check if it can be connected to them
-          
+
           intersect=0; %set no intersection to start with
           %takes the coordinates of the current and past points
-          current_point=[Points(1, i), Points(2, i)]; 
+          current_point=[Points(1, i), Points(2, i)];
           previous_point=[Points(1, j), Points(2, j)];
-          
+
           %creates a line-segment between the points before checking if that line-segment instersects an obstacle
           L = createEdge(current_point, previous_point);
           %intersect returns a vector of points of intersection meaning that if it is empty, there is no intersection
           if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
             intersect=1; % intersection happenned
-            
+
           else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
             MatrixOfLinks(i, j)=1;
             MatrixOfLinks(j,i)=1;
@@ -108,10 +108,11 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
       i= i + 1;
     endif
   end
-  
+
   MatrixOfLinks
   Points %display the points vector to check on the graph
-  
+  qGraph (Q_storage, nbPoints, MatrixOfLinks)
+
   %saves the results into a .mat file
   save(strcat(MapFilename,'.mat'), 'Points', 'MatrixOfLinks')
 

mapTest.mat

@@ -0,0 +1,25 @@
+# Created by Octave 7.3.0, Tue Jan 10 14:36:10 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
+# 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
+
+
+# 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
+
+

qGraph.m

@@ -0,0 +1,36 @@
+## Author: adril <adril@LAPTOP-EJ1AIJHT>
+## Created: 2023-01-10
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
+%
+% Task:
+%
+% Inputs:
+%	-
+%
+% Outputs:
+%	- None
+%
+% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
+% 10/01/2023
+%
+% Check library matgeom: https://octave.sourceforge.io/matgeom/overview.html
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function qGraph (Q, nbPoints, mat)
+  hold off;
+  figure 2
+  for i=1:nbPoints
+    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);
+        hold on;
+        drawEdge(L);
+      endif
+    endfor
+  endfor
+endfunction

simpleGraph.txt

@@ -0,0 +1,16 @@
+# graph
+# nodes
+5 2
+10 10
+20 10
+10 20
+20 20
+27 15
+# edges
+6
+1 2
+1 3
+2 4
+2 5
+3 4
+4 5

test_file.m

@@ -52,7 +52,6 @@ intersectLinePolygon(L, poly_a)
 isempty(intersectLinePolygon(L, poly_a))!=1
 
 
-
 ## This should be how we apply the function:
 ####rangeQ1Q2=[-5, -5; 5, 5];
 ####nbPoints=10;