finished with original function instead of fast computing one

buildPRM.m

@@ -26,6 +26,7 @@ function [nbNodes, obstacle, points] = buildPRM()
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   clc
   close all
+  addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
   
   [S, G, randVmin, randVmax, L1, L2] = setParams();
   
@@ -44,8 +45,10 @@ function [nbNodes, obstacle, points] = buildPRM()
     q1 = randVmin + (randVmax - randVmin)* rand();% setting a new random point r
     q2 = randVmin + (randVmax - randVmin)* rand();
     
-    [x, y]=MyFK(L1,L2,q1,q2);
+    %[x, y]=MyFK(L1,L2,q1,q2); remove the '%' on that line and add it on the next ones for faster computation
-
+    jTee=dh2ForwardKinematics([q1; q2], [0; 0], [L1; L2], [0; 0], 1);
+    x= jTee(1,4);
+    y= jTee(2,4);
     %Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space
     if not(y >= L1 || y <= -L1 || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2))
     

buildRRT.m

@@ -35,7 +35,10 @@ function [nbNodes, obstacle, points] = buildRRT()
   q2_1st = randVmin + (randVmax - randVmin)* rand(); % first q2 random value
   
   
-  [x, y]=MyFK(L1,L2,q1_1st,q2_1st);
+  %[x, y]=MyFK(L1,L2,q1_1st,q2_1st); remove the % on that line and add it on the next ones for faster computation
+  jTee=dh2ForwardKinematics([q1_1st; q2_1st], [0; 0], [L1; L2], [0; 0], 1);
+  x= jTee(1,4);
+  y= jTee(2,4);
   points = [Sq1 Sq2 S(1) S(2)]'; %flling the first column of the point array with the start point data
   
   GapValue = 5;
@@ -51,7 +54,7 @@ function [nbNodes, obstacle, points] = buildRRT()
   minTable = [];
   
   n=0; % represents the number of validated points
-  WhileCond = 15; % number of desired points in the tree
+  WhileCond = 20; % number of desired points in the tree
   
   while n<WhileCond
     distArr = []; %resets the distArr array for each point (each iteration)
@@ -60,7 +63,11 @@ function [nbNodes, obstacle, points] = buildRRT()
     q2_r = randVmin + (randVmax - randVmin)* rand();
     
     
-    [x, y]=MyFK(L1,L2,q1_r,q2_r); % getting the X Y coordinates of r point
+    %[x, y]=MyFK(L1,L2,q1_r,q2_r); remove the % on that line and add it on the next ones for faster computation
+    
+    jTee=dh2ForwardKinematics([q1_r; q2_r], [0; 0], [L1; L2], [0; 0], 1); % getting the X Y coordinates of r point
+    x= jTee(1,4);
+    y= jTee(2,4);
     
     n = columns(points);
     

checkingLine.m

@@ -62,8 +62,11 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points)
         Q2test = Q2(g);
         
         %Is each sampled point from the line colliding with an obstacle in X Y cartesian space
-        [Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test);
         
+        %[Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test); remove the % on that line and add it on the next ones for faster computation
+        jTee=dh2ForwardKinematics([Q1test; Q2test], [0; 0], [L1; L2], [0; 0], 1);
+        Xtest = jTee(1,4);
+        Ytest = jTee(2,4);
         % if a sampled point is colliding and we haven't already noted it, we do
         if dist !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
           dist = NaN;

planPathRRT.m

@@ -11,7 +11,7 @@ function planPathRRT
 % Outputs:
 %
 % Thomas OLIVE (thomas.olive@ecam.fr)
-% 18/12/2021
+% 19/12/2021
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
   addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); % gets access to the scripts that are stored in that folder
@@ -19,7 +19,6 @@ function planPathRRT
   [S, G, randVmin, randVmax, L1, L2] = setParams();
   
   GapValue=1;
-  nodeTrajCut = [];
   [nbNode, visGraph, points] = buildRRT();
   
   for i=1:columns(visGraph)
@@ -38,19 +37,28 @@ function planPathRRT
   Y_plot = [];
   
   nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array
-
+  shortcutPath = [1]; % we create an array similar to nodeTrajectory but without the nodes that can be avoided with shortcuts
-  for i=columns(nodeTrajectory):-1:1 %runs through the nodeTrajectory array from the end to the beginning
+  loopAgain = 1; %boolean value indicating wether or not we keep running the loop looking for shortcuts
-    if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points)) % if a line can be drawn between the start point and the one of index i
+  prevMaxIndex = columns(nodeTrajectory); % the index of the previous shortcut node we have in memory, the start point being at the end of nodeTrajectory
-      nodeTrajCut = [nodeTrajCut nodeTrajectory(i)]; % we note the distance between those points
+  while loopAgain
-       maxIndex = i;
+    maxShortcut = 0; % we reset the shortcut point we have in memory
+    for i=prevMaxIndex:-1:1 % we loop from the last saved shortcut to the goal point
+      if ~isnan(checkingLine(GapValue, L1, L2, nodeTrajectory(prevMaxIndex), nodeTrajectory(i), points)) % if a line can be drawn between the last saved shortcut and the node of index i
+        maxShortcut = nodeTrajectory(i); % we update that point of index i as the maximum shortcut reachable
+        maxIndex = i; % we save the index of that point of index i that will be the next shortcut
+      endif
+    endfor
+    prevMaxIndex = maxIndex;
+    if shortcutPath(end) == columns(points) || maxShortcut == 0 % if we reached the goal point with the shortcuts
+      loopAgain = 0; % we stop looping
+    else
+      loopAgain = 1;
+      shortcutPath = [shortcutPath maxShortcut];
     endif
-  endfor
+  endwhile  
-  
+  for i=1:columns(shortcutPath) % running through the new nodeTrajectory array
-  nodeTrajectory = [nodeTrajectory(1:maxIndex-1) max(nodeTrajCut) 1]; % as we want to travel the maximal distance from the start point, withouth going through the first branches, we take the maximum value of the nodeTrajCut array
+    Q1plot = [Q1plot points(1, shortcutPath(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
-  
+    Q2plot = [Q2plot points(2, shortcutPath(i))];
-  for i=1:columns(nodeTrajectory) % running through the new nodeTrajectory array
-    Q1plot = [Q1plot points(1, nodeTrajectory(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
-    Q2plot = [Q2plot points(2, nodeTrajectory(i))];
     
     if i>1 % for the points that are not the start one
       
@@ -73,8 +81,8 @@ function planPathRRT
         %we compute the x y coordinates that are matching the sample points
         [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
         if g==Q1plot(i) % as the sampling starts from a node that condition means that we are computing about a node
-          points(3, nodeTrajectory(i)) = Xtest; % we note the x and y values of the nodes
+          points(3, shortcutPath(i)) = Xtest; % we note the x and y values of the nodes
-          points(4, nodeTrajectory(i)) = Ytest;
+          points(4, shortcutPath(i)) = Ytest;
         endif
         
         X_plot = [X_plot Xtest]; % we note the x and y values of the sample points
@@ -95,9 +103,9 @@ function planPathRRT
   hold all
   text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');% we plot the shortest path in the x y cartesian space
   
-  for i=2:columns(nodeTrajectory)
+  for i=2:columns(shortcutPath)
     if nodeTrajectory(i) != 1
-      text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
+      text(points(3, shortcutPath(i)), points(4, shortcutPath(i)), int2str(shortcutPath(i)-1), 'FontSize', 20); % we add the description of each node
     endif
  endfor