Merge branch 'Gwenn'

buildPRM.m

@@ -1,10 +1,16 @@
 ## Author: adril <adril@LAPTOP-EJ1AIJHT>
 ## 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;
+  i = 1; %initialize
-  Points=zeros(2, nbPoints);
+  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)
 

buildRRT.m

@@ -0,0 +1,189 @@
+## Author: adril <adril@LAPTOP-EJ1AIJHT>
+## Created: 2023-01-08
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%function buildRRT (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
+%
+% Task: Use the RRT method to reach a desired goal on the cartesian space
+%
+% Inputs:
+%	- rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2
+% - nbPoints : number of points required
+% - L1, L2 : lengths of the links (in m)
+% - fixedLength : length of the short link
+% - start : starting point
+% - goal : ending point
+%
+% Outputs:
+%	- None
+%
+% Adrien Lasserre (adrien.lasserre@ecam.fr) &
+% Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
+% 08/01/2023
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
+
+  hold off;
+  i = 1; %while
+  nbPoints=nbPoints+2;%adding the starting point
+  %Points=zeros(nbPoints, 2);
+  %MatrixOfLinks=zeros(nbPoints, nbPoints);
+  alpha=[0;0];
+  d=[0;0];
+  a=[L1;L2];
+  jointNumber=1;
+
+  figure 1; hold on;
+  b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
+  hold on;
+  c=drawCircle(0, 0, L2-L1);
+  hold on;
+  %creates the lines defining the prohibited areas
+  top_line = createLine([0,L1,1,0]);
+  bottom_line = createLine([0,-L1,1,0]);
+  drawLine(top_line);
+  hold on;
+  drawLine(bottom_line);
+  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
+
+  %set the starting point
+  Points(1,1:2) = start;
+
+  %draw the start and goal points
+  drawPoint(start(1, 1), start(1,2));
+  drawPoint(goal(1, 1), goal(1,2));
+
+  while i <= nbPoints
+
+    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)];
+    Q_storage(1:2, i)=Q;
+    OutOfRange=0; %set the boolean
+    intersect=0;
+
+    bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
+    jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values
+    jTee=jTee';
+
+    index=findClosestPoint(jTee, Points);
+    dx=jTee(1,1)-Points(index, 1);
+    dy=jTee(1,2)-Points(index, 2);
+    E=sqrt(dx^2+dy^2);
+    dx=(dx)*fixedLength/E;
+    dy=(dy)*fixedLength/E;
+    L = createEdge(Points(index, :), [Points(index, 1)+dx, Points(index,2)+dy]);
+
+    if ((Points(index,2)+dy)>=L1)
+      OutOfRange=1; %is not valid if in that area
+    elseif ((Points(index,2)+dy)<=-L1)
+      OutOfRange=1;
+    elseif (abs((Points(index,1)+dx)) <= L2 && abs((Points(index,2)+dy)) <=L2)
+      OutOfRange=1;
+    endif
+
+    if (OutOfRange==0)
+
+      if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
+        intersect=1; % intersection happenned
+        disp('Intersect')
+      else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
+        hold on;%plotting the line
+        drawEdge(L);
+        MatrixOfLinks(i, i)=1;
+        MatrixOfLinks(i, index)=1;
+        MatrixOfLinks(index, i)=1;
+        Points(i, 1:2)=[Points(index, 1)+dx, Points(index,2)+dy];
+        hold on;
+        drawPoint(Points(i, 1), Points(i,2)); %draw the point
+        intersect=0;
+        L = createEdge(goal, Points(i, 1:2));
+
+        if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
+          intersect=1;
+        else
+          drawEdge(L);
+          MatrixOfLinks(i+1, i+1)=1;
+          MatrixOfLinks(i+1,i)=1;
+          MatrixOfLinks(i,i+1)=1;
+          i=i+1;
+          break;
+        endif
+
+        i=i+1;
+      endif
+    endif
+  endwhile
+
+##  Write the path planning here
+
+##  Start from goal, towards start, going to the previous point each time
+##  check everytime if the start is not already accessible, and if so draw a line
+##  prendre le goal, trouver le point auquel il est attach, y aller
+
+  index_goal = columns(MatrixOfLinks);
+  index = index_goal;
+
+##Draw the clean figure
+  figure 2; hold on;
+  b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
+  hold on;
+  c=drawCircle(0, 0, L2-L1);
+  hold on;
+  %creates the lines defining the prohibited areas
+  top_line = createLine([0,L1,1,0]);
+  bottom_line = createLine([0,-L1,1,0]);
+  drawLine(top_line);
+  hold on;
+  drawLine(bottom_line);
+  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
+
+  %set the starting point
+  %Points(1,1:2) = start;
+
+  %draw the start and goal points
+  %drawPoint(start(1, 1), start(1,2));
+  Points_2(1, 1:2)=goal;
+  drawPoint(Points_2(1, 1:2), 'ro');
+  z=1;
+  MatrixOfLinks
+  while Points_2(z, 1:2)!=Points(1, 1:2)
+##  Start the path planning
+##Check if we can reach the start
+    L = createEdge(start, Points_2(z, 1:2));
+    if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
+      intersect=1;
+    else
+      drawEdge(L, 'r');
+      break;
+    endif
+  ##If not, then check the next point
+    for i=1:index-1
+      if MatrixOfLinks(index, i)==1
+        disp('found a point connected');
+        Points_2(z+1, 1:2)=Points(i, 1:2);
+        disp('new point added, and drawn');
+        drawPoint(Points_2(z+1, 1:2), 'r');
+        L=createEdge(Points_2(z+1, 1:2), Points_2(z, 1:2));
+        drawEdge(L, 'r');
+        index=i;
+        break;
+      endif
+    endfor
+    z=z+1;
+    disp('next step')
+  endwhile
+
+  drawPoint(start, 'ro');
+
+endfunction

mapTest.mat

@@ -1,25 +1,25 @@
-# Created by Octave 7.3.0, Tue Jan 10 14:36:10 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
+# Created by Octave 7.3.0, Thu Jan 19 11:20:48 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.19697057332778345 -1.1771718145368819 -2.887110614207359 -1.4285411653803914 -1.6523013408997613 -1.363706683365784 1.4748850427562932 -1.1404559732519735 -2.1736190854336215 2.7723194289404676
- 0.047890118375208762 -0.42713698315704518 -0.30925089631194735 -1.216804326954257 1.3865824550438368 1.3067789455813377 -1.2708472666201218 1.1717992231942889 -0.02269206049426975 1.84133215165838
+ 1.745837315672909 0.75220366971764996 0.5218447246367357 -0.15692440100160332 1.3102147601497778 0.29066752826903497 -0.013557097054121003 -1.4995957486596576 -1.718447568352325 -0.91198013843207804
 
 
 # name: MatrixOfLinks
 # type: matrix
 # rows: 10
 # columns: 10
- 1 0 1 1 0 0 0 1 0 1
+ 1 0 1 0 1 0 0 0 0 0
- 0 1 0 0 1 0 0 0 1 0
+ 0 1 1 1 1 1 0 1 1 0
- 1 0 1 1 0 0 0 0 0 1
+ 1 1 1 1 1 1 0 1 1 0
- 1 0 1 1 0 0 1 0 0 0
+ 0 1 1 1 1 1 0 1 1 0
- 0 1 0 0 1 1 0 1 1 1
+ 1 1 1 1 1 1 0 1 1 0
- 0 0 0 0 1 1 0 1 0 1
+ 0 1 1 1 1 1 0 1 1 0
- 0 0 0 1 0 0 1 0 0 0
+ 0 0 0 0 0 0 1 0 0 1
- 1 0 0 0 1 1 0 1 0 1
+ 0 1 1 1 1 1 0 1 1 1
- 0 1 0 0 1 0 0 0 1 0
+ 0 1 1 1 1 1 0 1 1 1
- 1 0 1 0 1 1 0 1 0 1
+ 0 0 0 0 0 0 1 1 1 1
 
 

planPathPRM.m

@@ -2,21 +2,87 @@
 ## Created: 2023-01-08
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
+%function planPathPRM(mapFilePath, startPoint, endPoint, L1, L2)
 %
-% 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 ()
+function planPathPRM (mapFilePath, startPoint, endPoint, L1, L2)
 
+  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]                            %store the points (with S and G)
+  connectionMatrix                                          %debugging
+
+  [numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) %create the visibilityGraph
+
+
+  [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning
+
+  figure 3; hold on;
+  b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
+  hold on;
+  c=drawCircle(0, 0, L2-L1);
+  hold on;
+  %creates the lines defining the prohibited areas
+  top_line = createLine([0,L1,1,0]);
+  bottom_line = createLine([0,-L1,1,0]);
+  drawLine(top_line);
+  hold on;
+  drawLine(bottom_line);
+  center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2];
+  drawPolygon(center_box);
+  hold on;
+
+
+  Points_path=zeros(size(nodeTrajectory(1,2))+1,2);
+  drawPoint(S, 'ro');
+
+  for i=1:size(nodeTrajectory, 2)
+    drawPoint(points2D(nodeTrajectory(1, i), 1:2), 'r');
+    Points_path(i, 1:2)=points2D(nodeTrajectory(1, i), 1:2);
+  endfor
+
+  drawPoint(G, 'ro');
+
+  for j=2:size(Points_path, 1)
+    L=createEdge(Points_path(j-1, 1:2), Points_path(j, 1:2));
+    drawEdge(L, 'r');
+  endfor
+  indexClosestPointFinal = findClosestPoint(G,Points_path);
+  L=createEdge(Points_path(indexClosestPointFinal, 1:2), G);
+  drawEdge(L, 'r');
 
 endfunction

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
+  figure 2 %launch a new figure
-  for i=1:nbPoints
+  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));
+      drawPoint(Q(1,i), Q(2,i)); %draw the point in C-space
-      current_point=[Q(1, i), Q(2, i)];
+      current_point=[Q(1, i), Q(2, i)]; %change the current point
-      previous_point=[Q(1, j), Q(2, j)];
+      previous_point=[Q(1, j), Q(2, j)]; %change the previous point
-      if mat(i, j)==1 & mat(j, i)==1
+      if mat(i, j)==1 & mat(j, i)==1 %if there is a link between the two
-        L = createEdge(current_point, previous_point);
+        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