TD5_RRT/buildRRT.m

function path = 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
  % - x1: start point x
  % - y1: first point y
  % - x2: end point x
  % - y2: end point y
  %
  % Output:
  %	-path: Vector of points
  %
  % 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 = []
  xy_valid(end+1,:) = pt1
  q1q2_valid = [];
  validLinks = [];

  distanceBetweenPoints = 0.1;

  fill(x1, y1, 'r');

  hold on;

  done = 1;
  if ~(IsIntersecting (L1, L2, pt1, pt2))
      xy_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;

  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 = (position_ee(1)-xy_valid(i,1))^2+ (position_ee(2)-xy_valid(i,2))^2;
      if (dist < min)
        min = dist;
        closestPoint = xy_valid(i, :);
        closestPointIdx = i;
      endif
    endfor
    min = 12345678901234567890;

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

    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,:) = theta;
    endif
    %no more obstacles
    if ~(IsIntersecting (L1, L2, newPoint, pt2) || eeHittingObstacle == 1)
      done = 0
      xy_valid(end+1,:) = pt2;
      validLinks(end+1,:) = [closestPointIdx length(xy_valid)];
    endif

  end

  visibilityGraph = zeros(length(xy_valid));

  % Add edges to visibility graph based on valid links
  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(xy_valid(i, :) - xy_valid(j, :));
              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;
  end