added a threshold in the obstacle detection

DrawObstacles.m

@@ -32,6 +32,9 @@ function DrawObstacles()
         % saving the computed coordinates
         q1 = qi(j,1);
         q2 = qi(j,2);
+        if q2 > 180
+          q2 = q2-360;
+        endif
         q1_plot = [q1_plot q1];
         
         q2_plot = [q2_plot q2];

buildPRM.m

@@ -28,10 +28,19 @@ function [nbNodes, obstacle, points] = buildPRM()
   close all
   addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
   
-  [S, G, randVmin, randVmax, L1, L2] = setParams();
+  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
+  
+  
+  threshold = 0.000001;
+  
+  [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
+  Sq1 = qi(1,1)
+  Sq2 = qi(1,2)
+  
+  [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
+  Gq1 = qi(1,1)
+  Gq2 = qi(1,2)
   
-  [Sq1, Sq2] = MyIK(2,1,S(1),S(2)); %-28.995   104.48
-  [Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04    104.48
   points = [Sq1 Sq2 S(1) S(2)]';
   obstacle = [];
   GapValue = 10;
@@ -50,7 +59,7 @@ function [nbNodes, obstacle, points] = buildPRM()
     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))
+    if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))
     
       n=columns(points);
       
@@ -105,8 +114,8 @@ function [nbNodes, obstacle, points] = buildPRM()
         q2_stored = points(2,j);
         
         
-        obstacle(j,n) = checkingLine(GapValue, L1, L2, n, j, points); % we store the distance (NaN if invalid) between the points in a matrix
+        obstacle(j,n) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % we store the distance (NaN if invalid) between the points in a matrix
-        obstacle(n,j) = checkingLine(GapValue, L1, L2, n, j, points); % that is designed as a double entry table
+        obstacle(n,j) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % that is designed as a double entry table
         
         if ~isnan(obstacle(j, n)) && n!=j % if the points of index n and j can be linked
             Xplot = [q1, q1_stored];

buildRRT.m

@@ -25,10 +25,15 @@ function [nbNodes, obstacle, points] = buildRRT()
   clc
   close all
   
-  [S, G, randVmin, randVmax, L1, L2] = setParams();
+  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
   
-  [Sq1, Sq2] = MyIK(2,1,S(1),S(2)); %-28.995   104.48
+  [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
-  [Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04    104.48
+  Sq1 = qi(1,1);
+  Sq2 = qi(1,2);
+  
+  [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
+  Gq1 = qi(1,1);
+  Gq2 = qi(1,2);
   
 
   q1_1st = randVmin + (randVmax - randVmin)* rand(); % first q1 random value
@@ -54,7 +59,7 @@ function [nbNodes, obstacle, points] = buildRRT()
   minTable = [];
   
   n=0; % represents the number of validated points
-  WhileCond = 20; % number of desired points in the tree
+  WhileCond = 15; % number of desired points in the tree
   
   while n<WhileCond
     distArr = []; %resets the distArr array for each point (each iteration)
@@ -81,7 +86,7 @@ function [nbNodes, obstacle, points] = buildRRT()
           q2_p = points(2,j);
        
           pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp
-          distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)];
+          distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp, threshold)];
       endfor
       if isnan(min(distArr)) % if the line can not be drawn between the closest and goal point
         WhileCond++; % we keep adding more points
@@ -94,14 +99,14 @@ function [nbNodes, obstacle, points] = buildRRT()
           q2_p = points(2,j);
        
           pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp
-          distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)]; % we compute the distance between each point in the tree (index j) and the new r one (index n+1)
+          distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp, threshold)]; % we compute the distance between each point in the tree (index j) and the new r one (index n+1)
       endfor
       
     else % if it is the first iteration
       q1_p = points(1,1);
       q2_p = points(2,1);
       pointsTemp = [points [q1_r q2_r 0 0]'];
-      distArr = [distArr checkingLine(GapValue, L1, L2, 1, 2, pointsTemp)]; % we compute the distance between the first point in the tree (index 1) and the new r one (index n+1)
+      distArr = [distArr checkingLine(GapValue, L1, L2, 1, 2, pointsTemp, threshold)]; % we compute the distance between the first point in the tree (index 1) and the new r one (index n+1)
     
     endif
     [minDist, minIndex] = min(distArr); % we save which point is the closest one from the new r point and we note the distance between them

checkingLine.m

@@ -1,4 +1,4 @@
-function dist = checkingLine(GapValue, L1, L2, n, j, points)
+function dist = checkingLine(GapValue, L1, L2, n, j, points, threshold)
   
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %function dist = checkingLine(GapValue, L1, L2, n, j, points)
@@ -36,7 +36,6 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points)
   q1_ = points(1,n);
   q2_ = points(2,n);
   
-  
   dist = 0; % 0 value, if not modified, will represent that we didn't note about an invalid link between the n and j points
   if n==j
     dist = NaN;
@@ -68,7 +67,7 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points)
         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
+        if dist !=NaN && (Ytest >= L1 - threshold || Ytest <= -L1 + threshold || (Xtest>=-L2 - threshold && Xtest<=L2 - threshold && Ytest>=-L2 - threshold && Ytest<=L2 + threshold)) %verifie les obstacles
           dist = NaN;
         endif
         

planPathPRM.m

@@ -13,6 +13,7 @@ function planPathPRM()
 % Thomas OLIVE (thomas.olive@ecam.fr)
 % 18/12/2021
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams()
   
   [nbNode, visGraph, points] = buildPRM();
   [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
@@ -48,7 +49,11 @@ function planPathPRM()
         Q2test = Q2(g);
         
         %we compute the x y coordinates that are matching the sample points
-        [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
+        % [Xtest, Ytest]=MyFK(2,1,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);
         
         X_plot = [X_plot Xtest];% we note the x and y values of the sample points
         Y_plot = [Y_plot Ytest]; 

planPathRRT.m

@@ -16,7 +16,7 @@ function planPathRRT
 
   addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); % gets access to the scripts that are stored in that folder
   
-  [S, G, randVmin, randVmax, L1, L2] = setParams();
+  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
   
   GapValue=1;
   [nbNode, visGraph, points] = buildRRT();
@@ -43,7 +43,7 @@ function planPathRRT
   while loopAgain
     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
+      if ~isnan(checkingLine(GapValue, L1, L2, nodeTrajectory(prevMaxIndex), nodeTrajectory(i), points, threshold)) % 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
@@ -79,7 +79,11 @@ function planPathRRT
         Q2test = Q2(g);
         
         %we compute the x y coordinates that are matching the sample points
-        [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
+        % [Xtest, Ytest]=MyFK(2,1,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 g==Q1plot(i) % as the sampling starts from a node that condition means that we are computing about a node
           points(3, shortcutPath(i)) = Xtest; % we note the x and y values of the nodes
           points(4, shortcutPath(i)) = Ytest;
@@ -103,10 +107,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(shortcutPath)
+  for i=2:columns(shortcutPath)-1
-    if nodeTrajectory(i) != 1
       text(points(3, shortcutPath(i)), points(4, shortcutPath(i)), int2str(shortcutPath(i)-1), 'FontSize', 20); % we add the description of each node
-    endif
+    %endif
  endfor
   
   text(S(1), S(2), 'S', 'FontSize', 20);

setParams.m

@@ -1,4 +1,4 @@
-function [S, G, randVmin, randVmax, L1, L2] = setParams()
+function [S, G, randVmin, randVmax, L1, L2, threshold] = setParams()
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %function [S, G, randVmin, randVmax, L1, L2] = setParams()
 %
@@ -28,4 +28,5 @@ function [S, G, randVmin, randVmax, L1, L2] = setParams()
   randVmax = 180;
   L1 = 2;
   L2 = 1;
+  threshold = 0.000001;
 endfunction