MotionPlanning/buildRRT.m

function [nbNodes, obstacle, points] = buildRRT()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [nbNodes, obstacle, points] = buildPRM()
%
% Task: Implement a code that creates a tree of points that are drawn in a random way in the q1 q2 joint space
%       The tree progresses by a predefined length towards random direction, starting from the S point,
%       until it finds a way to connect the start and goal points
%       that algorithm is called Rapidly-exploring Random Trees algorithm
%       
%
% Inputs: 
%
% Outputs:
%	-nbNodes: number of nodes of the graph excluding the starting and goal points
%	-obstacle: a matrix containing the distance between connected nodes 
%		(NaN refers to not connected nodes)
%		The matrix has a size of (nbNodes+2)x(nbNodes+2)
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
%         the 1st column describing the 1st (start) point
%		
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  clc
  close all
  
  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
  
  [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
  
  
  %[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;
  distArr = [];
  pdefL = 50;
  
  figure 1
  axis([-180 180 -180 180])
  hold on
  plot(points(1, 1), points(2, 1))
  text(points(1, 1), points(2, 1), 'S', 'FontSize', 20); %drawing a 'S' letter at q1 q2
  
  minTable = [];
  
  n=0; % represents the number of validated points
  WhileCond = 15; % number of desired points in the tree
  
  while n<WhileCond
    distArr = []; %resets the distArr array for each point (each iteration)
    
    q1_r = randVmin + (randVmax - randVmin)* rand(); % setting a new random point r
    q2_r = randVmin + (randVmax - randVmin)* rand();
    
    
    %[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);
    
    minTable(1, n) = inf;
    obstacle(n,n) = NaN; % increases the size of obstacle matrix
    if n==WhileCond % if we have enough points
      q1_r = Gq1; % we do not link a p point with a random one but with the Goal one
      q2_r = Gq2;
      for j=1:n
          q1_p = points(1,j);
          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, 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
      else
        pdefL = min(distArr); % we do not use the predefined length but the one separing the closest and goal points
      endif
    elseif n > 1 % if is not the first iteration
      for j=1:n % for each point in the tree
          q1_p = points(1,j);
          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, 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, 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
      
    if ~isnan(minDist) % if the line between p and r can be drawn (does not intersect an obstacle in X Y cartesian space)
      q1_p = points(1, minIndex);
      q2_p = points(2, minIndex);
      
      q1_pdefL = q1_p + (pdefL * (q1_r - q1_p) / minDist); % thanks to some theory coming from the Thales theorem 
      q2_pdefL = q2_p + (pdefL * (q2_r - q2_p) / minDist); % we compute the q1 and q2 values of the point that is on the line between p and r, away of pdefL from p 
      
      points(1, n+1) = q1_pdefL; %we note those two values in the points (= tree) matrix since we consider them as 'validated'
      points(2, n+1) = q2_pdefL;
      
      if n+1!=minIndex
        obstacle(n+1, minIndex) = pdefL; % we note the distance between p and r in the obstacle matrix
        obstacle(minIndex, n+1) = pdefL; % which is built as a double entry table (going from S to G)
      endif
      
      figure 1
      hold on
      plot(points(1, n), points(2, n)) % we plot the new values and add a description to it
      if pdefL == min(distArr) % this condition is valid only for the last iteration
        text(points(1, n+1), points(2, n+1), 'G', 'FontSize', 20);
      endif
      if n>1 % valid for every iteration except the first  and last ones
        text(points(1, n), points(2, n), int2str(n-1), 'FontSize', 20);
      endif
      Xplot = [q1_p, q1_pdefL];
      Yplot = [q2_p, q2_pdefL];
      plot(Xplot, Yplot, 'Color', 'b')
      drawnow
    endif
  endwhile
  nbNodes = WhileCond-1;
endfunction