corresction for points in CSPACE

buildRRT.m

@@ -1,4 +1,4 @@
-function path = buildRRT(L1, L2, pt1, pt2)
+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
@@ -6,13 +6,13 @@ function path = buildRRT(L1, L2, pt1, pt2)
   % Inputs:
   %	- L1: first length
   %	- L2: second length
-  % - x1: start point x
+  % - pt1: start xy
-  % - y1: first point y
+  % - pt2: end xy
-  % - x2: end point x
-  % - y2: end point y
   %
   % 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
@@ -26,19 +26,37 @@ function path = buildRRT(L1, L2, pt1, pt2)
   x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
   y2 = [-L1 L1 L1 -L1];
   xy_valid = []
-  xy_valid(end+1,:) = pt1
+  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 = 0.1;
+  distanceBetweenPoints = 3;
 
   fill(x1, y1, 'r');
 
   hold on;
 
   done = 1;
-  if ~(IsIntersecting (L1, L2, pt1, pt2))
+
-      xy_valid(end+1,:) = pt2;
+  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
@@ -55,7 +73,8 @@ function path = buildRRT(L1, L2, pt1, pt2)
   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)
@@ -79,22 +98,30 @@ function path = buildRRT(L1, L2, pt1, pt2)
     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;
+      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 = [position_ee(1)-closestPoint(1,1) position_ee(2)-closestPoint(1,2)];
+    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;
-    newPoint = closestPoint+vectorForce*distanceBetweenPoints;
+    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
@@ -113,12 +140,13 @@ function path = buildRRT(L1, L2, pt1, pt2)
     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;
+      q1q2_valid(end+1,:) = newPointC;
     endif
     %no more obstacles
-    if ~(IsIntersecting (L1, L2, newPoint, pt2) || eeHittingObstacle == 1)
+    if ~(IsIntersecting (L1, L2, newPoint, pt2Card) || eeHittingObstacle == 1)
       done = 0
-      xy_valid(end+1,:) = pt2;
+      q1q2_valid(end+1,:) = pt2;
+      xy_valid(end+1,:) = pt2Card;
       validLinks(end+1,:) = [closestPointIdx length(xy_valid)];
     endif
 
@@ -126,12 +154,12 @@ function path = buildRRT(L1, L2, pt1, pt2)
 
   visibilityGraph = zeros(length(xy_valid));
 
-  % Add edges to visibility graph based on valid links
+  % 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(xy_valid(i, :) - xy_valid(j, :));
+              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
@@ -150,6 +178,8 @@ function path = buildRRT(L1, L2, pt1, pt2)
   endfor
 
   path = nodeTrajectory;
+  q1q2Path = q1q2_valid;
+  xyPath = xy_valid;
   end
 
 

test.m

@@ -1,5 +1,4 @@
-path = buildRRT(2, 1, [-1.5 -1.5], [1.5 1.5])
+[path, q1q2Path, xyPath] = buildRRT(2, 1, [180 40], [0 40])
-
 %test for intersections
 intersect = IsIntersecting (2, 1, [-1.5 1.5], [1.5 1.5])
 intersect = IsIntersecting (2, 1, [-1.5 -1.5], [1.5 1.5])