diff --git a/DrawObstacles.m b/DrawObstacles.m
index c764d34..c04c445 100644
--- a/DrawObstacles.m
+++ b/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];
diff --git a/buildPRM.m b/buildPRM.m
index 571cf3b..7c69d36 100644
--- a/buildPRM.m
+++ b/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(n,j) = checkingLine(GapValue, L1, L2, n, j, points); % that is designed as a double entry table
+        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, 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];
diff --git a/buildRRT.m b/buildRRT.m
index 8338d79..630e5ff 100644
--- a/buildRRT.m
+++ b/buildRRT.m
@@ -25,12 +25,17 @@ 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
-  [Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04    104.48
+  [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);
   
+
   q1_1st = randVmin + (randVmax - randVmin)* rand(); % first q1 random value
   q2_1st = randVmin + (randVmax - randVmin)* rand(); % first q2 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
diff --git a/checkingLine.m b/checkingLine.m
index 92e623f..465bdd9 100644
--- a/checkingLine.m
+++ b/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
         
diff --git a/planPathPRM.m b/planPathPRM.m
index d501ef0..d6f7024 100644
--- a/planPathPRM.m
+++ b/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]; 
diff --git a/planPathRRT.m b/planPathRRT.m
index fabaaf5..029fa1b 100644
--- a/planPathRRT.m
+++ b/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)
-    if nodeTrajectory(i) != 1
+  for i=2:columns(shortcutPath)-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);
diff --git a/setParams.m b/setParams.m
index 34c734e..d03aeb4 100644
--- a/setParams.m
+++ b/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
\ No newline at end of file