finished RRT

buildRRT.m

@@ -28,7 +28,7 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
   i = 1; %while
   nbPoints=nbPoints+2;%adding the starting point
   %Points=zeros(nbPoints, 2);
-  MatrixOfLinks=zeros(nbPoints, nbPoints);
+  %MatrixOfLinks=zeros(nbPoints, nbPoints);
   alpha=[0;0];
   d=[0;0];
   a=[L1;L2];
@@ -108,6 +108,10 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
           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
 
@@ -115,4 +119,71 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
       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, Wed Jan 11 15:52:59 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
- 1.0102396615914184 0.52999250891267136 -1.4938105820093814 0.39973309803001944 1.6575303500294156 2.1811052817711225 2.3728058316176091 -1.7930511997776755 -1.1301884102529285 0.37332095642010499
+ 0.19697057332778345 -1.1771718145368819 -2.887110614207359 -1.4285411653803914 -1.6523013408997613 -1.363706683365784 1.4748850427562932 -1.1404559732519735 -2.1736190854336215 2.7723194289404676
- -0.38102794441973026 -1.4202013308006278 0.272919986694101 -1.7257148318842865 1.9757520956131371 0.5058894523007601 1.417055067995548 -1.7814659113455531 1.964795369645167 -1.5680163207760147
+ 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 0 0 1 1 1 0 0 0
+ 1 0 1 0 1 0 0 0 0 0
- 0 1 0 1 0 0 0 1 0 1
+ 0 1 1 1 1 1 0 1 1 0
- 0 0 1 0 0 0 0 1 1 0
+ 1 1 1 1 1 1 0 1 1 0
- 0 1 0 1 0 0 0 1 0 1
+ 0 1 1 1 1 1 0 1 1 0
- 1 0 0 0 1 1 1 0 1 0
+ 1 1 1 1 1 1 0 1 1 0
- 1 0 0 0 1 1 1 0 1 0
+ 0 1 1 1 1 1 0 1 1 0
- 1 0 0 0 1 1 1 0 1 0
+ 0 0 0 0 0 0 1 0 0 1
- 0 1 1 1 0 0 0 1 1 1
+ 0 1 1 1 1 1 0 1 1 1
- 0 0 1 0 1 1 1 1 1 0
+ 0 1 1 1 1 1 0 1 1 1
- 0 1 0 1 0 0 0 1 0 1
+ 0 0 0 0 0 0 1 1 1 1
 
 

planPathPRM.m

@@ -2,7 +2,7 @@
 ## Created: 2023-01-08
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%function planPathPRM(mapFilePath, startPoint, endPoint)
+%function planPathPRM(mapFilePath, startPoint, endPoint, L1, L2)
 %
 % Task: Establish the path from the starting point to the goal, using the points defined with the PRM method
 %       and using the Dijkstra method.
@@ -20,7 +20,7 @@
 % 08/01/2023
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-function planPathPRM (mapFilePath, startPoint, endPoint)
+function planPathPRM (mapFilePath, startPoint, endPoint, L1, L2)
 
   mapFile = load(mapFilePath);
 
@@ -49,6 +49,40 @@ function planPathPRM (mapFilePath, startPoint, endPoint)
   [numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) %create the visibilityGraph
 
 
-  dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning
+  [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