function planPathPRM()
  [nbNode, visGraph, points] = buildPRM();
  [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
  visGraph
  addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
  Q1plot = [];
  Q2plot = [];
  X_plot = [];
  Y_plot = [];
  GapValue = 1;
  nodeTrajectory = [columns(points) nodeTrajectory];

  for i=1:columns(nodeTrajectory)
    Q1plot = [Q1plot points(1, nodeTrajectory(i))];
    Q2plot = [Q2plot points(2, nodeTrajectory(i))];
  
    if i>1
      
          
      A = (Q2plot(i-1)- Q2plot(i))/(Q1plot(i-1) - Q1plot(i));
      B = Q2plot(i) - A * Q1plot(i);

      Q2 = @(Q1) A*Q1+B;
          
      if Q1plot(i) > Q1plot(i-1)
        gap = -GapValue;
      else
        gap = GapValue;
      endif
      for g=Q1plot(i):gap:Q1plot(i-1)
        Q1test = g;
        Q2test = Q2(g);
        
        %Is the end effector colliding with obstacle
        [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
        
        X_plot = [X_plot Xtest];
        Y_plot = [Y_plot Ytest]; 
      endfor
    endif
    
  endfor
  
  figure 1
  axis([-180 180 -180 180]);
  title('q1 q2 Joint Space');
  hold on
  plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5)
  
  figure 2
  title('X-Y Cartesian Space');
  axis([-3 3 -3 3]);
  hold all
  text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');
  
  for i=2:columns(nodeTrajectory)-1
    text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20);
  endfor
  x = 3*cos(0:0.01*pi:2*pi);
  y = 3*sin(0:0.01*pi:2*pi);
  plot(x,y, 'Color', 'k');
  
  x = cos(0:0.01*pi:2*pi);
  y = sin(0:0.01*pi:2*pi);
  plot(x,y, 'Color', 'k');

endfunction