diff --git a/buildRRT.m b/buildRRT.m
index 840fc1c..e18a44e 100644
--- a/buildRRT.m
+++ b/buildRRT.m
@@ -16,7 +16,7 @@
 % 08/01/2023
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
+function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
 
   hold off;
   i = 1; %while
@@ -46,8 +46,12 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
   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
 
-  Points(1, 1:2)=[2,0];%starting point
-  drawPoint(Points(1, 1), Points(1,2));
+  %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
 
@@ -60,28 +64,10 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
     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);
-##    D = distancePoints(jTee, Points(index, :));
-    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 | isempty(intersectEdgePolygon(L, top_line))!=1 | isempty(intersectEdgePolygon(L, bottom_line))!=1)
-        intersect=1; % intersection happenned
-      else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
+    
+    L = createEdge(goal, jTee);
+    
+    if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1 | isempty(intersectEdgePolygon(L, top_line))!=1 | isempty(intersectEdgePolygon(L, bottom_line))!=1)
         hold on;%plotting the line
         drawEdge(L);
         MatrixOfLinks(i, i)=1;
@@ -91,7 +77,41 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
         hold on;
         drawPoint(Points(i, 1), Points(i,2)); %draw the point
         i=i+1;
+        break;
+    else
+      index=findClosestPoint(jTee, Points)
+      dx=jTee(1,1)-Points(index, 1);
+      dy=jTee(1,2)-Points(index, 2);
+  ##    D = distancePoints(jTee, Points(index, :));
+      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 | isempty(intersectEdgePolygon(L, top_line))!=1 | isempty(intersectEdgePolygon(L, bottom_line))!=1)
+          intersect=1; % intersection happenned
+        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
+          i=i+1;
+        endif
+       endif
     endif
   endwhile