TD5_RRT/buildRRT.m

function [path, q1q2Path, xyPath] = buildRRT(L1, L2, pt1, pt2)
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % function  path = buildRRT(L1, L2, start, finish)
  % Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters
  %
  % Inputs:
  %	- L1: first length
  %	- L2: second length
  % - pt1: start xy
  % - pt2: end xy
  %
  % Output:
  %	-path: Vector of points
  % - q1q2Path: List of all the points created in C-Space
  % - xyPath: List of all the points created in Cardinal space
  %
  % author: Marais Lucas
  % date: 22/11/2023
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


  x3 = [-L2 -L2 L2 L2];
  y3 = [-L2 L2 L2 -L2];
  x1 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
  y1 = [-L1-L2 L1+L2 L1+L2 -L1-L2];
  x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
  y2 = [-L1 L1 L1 -L1];
  xy_valid = []
  d = [0; 0];
  a = [L1; L2];
  alpha = [0; 0];

  % computes the FK
  wTee = dh2ForwardKinematics(pt1', d, a, alpha, 1);

  % determines the position of the end-effector
  position_ee = wTee(1:2,end);
  xy_valid(end+1,:) = position_ee;
  q1q2_valid = [];
  q1q2_valid(end+1,:) = pt1;
  validLinks = [];

  distanceBetweenPoints = 3;

  fill(x1, y1, 'r');

  hold on;

  done = 1;

  pt1Card = position_ee';
  % computes the FK
  wTee = dh2ForwardKinematics(pt2', d, a, alpha, 1);

  % determines the position of the end-effector
  pt2Card = wTee(1:2,end)';
  if ~(IsIntersecting (L1, L2, pt1Card, pt2Card))
      xy_valid(end+1,:) = pt2Card;
      q1q2_valid(end+1,:) = pt2;
      validLinks(end+1,:) = [1 2];
      done = 0
  endif

  t = linspace(0, 2*pi, 100)';
  r=L1+L2;
  circsx = r.*cos(t) + 0;
  circsy = r.*sin(t) + 0;
  hold on;
  fill(x2, y2, 'w');
  hold on;
  plot(circsx, circsy, 'b');

  hold on;
  fill(x3, y3, 'r');
  hold on;
  plot(pt1Card(1), pt1Card(2), 'g');
  plot(pt2Card(1), pt2Card(2), 'g');
  axis equal;

  while(done == 1)
    % samples randomly the joint space
    q1 = rand()*360.0;
    q2 = rand()*360.0;

    % creates the DH table
    theta = [q1; q2];
    d = [0; 0];
    a = [L1; L2];
    alpha = [0; 0];

    % computes the FK
    wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);

    % determines the position of the end-effector
    position_ee = wTee(1:2,end);
    %determine the closest point
    min = 12345678901234567890;
    closestPoint = [];
    closestPointIdx = 0;
    for i=1:size(xy_valid,1)
      dist = (theta(1)-q1q2_valid(i,1))^2+ (theta(2)-q1q2_valid(i,2))^2;
      if (dist < min)
        min = dist;
        closestPoint = xy_valid(i, :);
        closestPointC = q1q2_valid(i, :);
        closestPointIdx = i;
      endif
    endfor
    min = 12345678901234567890;

    %place the point at a given length
    vectorForce = [theta(1)-closestPointC(1,1) theta(2)-closestPointC(1,2)];
    % Calculate the Euclidean norm (length) of the vector
    vectorNorm = norm(vectorForce);
    % Normalize the vector
    vectorForce = vectorForce / vectorNorm;
    closestPointC
    newPointC = closestPointC+vectorForce*distanceBetweenPoints;

    % computes the FK
    wTee = dh2ForwardKinematics(newPointC', d, a, alpha, 1);

    % determines the position of the end-effector
    newPoint = wTee(1:2,end)';

    plot(newPoint(1), newPoint(2), 'b');
    % checks if the end-effector is not hitting any obstacle
    eeHittingObstacle = 0;
    if (newPoint(2) >= L1)
      eeHittingObstacle = 1;
    end
    if (newPoint(2) <= -L1)
      eeHittingObstacle = 1;
    end
    if (newPoint(1) >= -L2 && newPoint(1) <= L2 && newPoint(2) >= -L2 && newPoint(2) <= L2)
      eeHittingObstacle = 1;
    end

    % If the there is something wrong don't do
    if ~(IsIntersecting (L1, L2, closestPoint, newPoint) || eeHittingObstacle == 1)
      validLinks(end+1,:) = [closestPointIdx length(xy_valid)+1];
      xy_valid(end+1,:) = newPoint;
      q1q2_valid(end+1,:) = newPointC;
    endif
    %no more obstacles
    if ~(IsIntersecting (L1, L2, newPoint, pt2Card) || eeHittingObstacle == 1)
      done = 0
      q1q2_valid(end+1,:) = pt2;
      xy_valid(end+1,:) = pt2Card;
      validLinks(end+1,:) = [closestPointIdx length(xy_valid)];
    endif

  end

  visibilityGraph = zeros(length(xy_valid));

  % Add edges to visibility graph based on valid links // can be used with q1q2_valid or xy_valid
  for i = 1:length(xy_valid)
      for j = i+1:length(xy_valid)
          if ~IsIntersecting(L1, L2, xy_valid(i, :), xy_valid(j, :))
              % If the line segment between points i and j does not intersect with obstacles
              visibilityGraph(i, j) = norm(q1q2_valid(i, :) - q1q2_valid(j, :));% here can be change
              visibilityGraph(j, i) = visibilityGraph(i, j); % Assuming undirected graph
          else
              visibilityGraph(i, j) = NaN;% No links
              visibilityGraph(j, i) = visibilityGraph(i, j); % Assuming undirected graph
          end
      end
  end

  [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(length(xy_valid)-2, visibilityGraph);
  nodeTrajectory = [1 nodeTrajectory];
  nodeTrajectory(end) = length(xy_valid)
  for i=1:length(nodeTrajectory)-1
    x = [xy_valid(nodeTrajectory(i),1) xy_valid(nodeTrajectory(i+1),1)]
    y = [xy_valid(nodeTrajectory(i),2) xy_valid(nodeTrajectory(i+1),2)]
    plot(x, y)
  endfor

  path = nodeTrajectory;
  q1q2Path = q1q2_valid;
  xyPath = xy_valid;
  end