Finished for real

DrawObstacles.m

@@ -16,8 +16,7 @@ function DrawObstacles()
 % 18/12/2021
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-  L1 = 2;
-  L2 = 1;
+  [S, G, randVmin, randVmax, L1, L2] = setParams();
   qi = [];
   q1_plot = [];
   q2_plot = [];
@@ -25,18 +24,20 @@ function DrawObstacles()
   y_plot = [];
   
   for i=1:10000
-    x = -(L1+L2) + rand()*2*(L1+L2);
+    x = -(L1+L2) + rand()*2*(L1+L2); %creating random x and y values
     y = -(L1+L2) + rand()*2*(L1+L2);
-    if((y >= L1)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2))
-      [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y);
+    if((y >= L1)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2)) % keeping them if they are inside the obstacles
+      [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y); % computing their corresponding q1 q2 coordinates
+      % saving the computed coordinates
       q1_plot = [q1_plot q1 q1_];
       
       q2_plot = [q2_plot q2 q2_];
       x_plot = [x_plot x];
       
-      y_plot = [y_plot y];
+      y_plot = [y_plot y]; 
     endif
   endfor
+  %plotting the points in the two different spaces, red color
   figure 1
   plot(q1_plot, q2_plot, '*', 'Color', 'r')
   figure 2

DrawObstaclesFct.m

@@ -1,39 +0,0 @@
-function M = DrawObstaclesFct(x,y,prevM)
-  L1 = 2;
-  L2 = 1;
-  qi = [];
-  q1_plot = [];
-  q2_plot = [];
-  x_plot = [];
-  y_plot = [];
-  prevM
-  if isempty(prevM)
-    prevM = [0;0;0;0];
-  endif
-  
-  M = prevM
-  
-  if (x^2+y^2 > L2^2)&&(x^2+y^2 < (L1+L2)^2)&&((y >= L1)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2))
-    disp('obstacle found');
-    [blc, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y);
-    M(1, columns(prevM)+1) = [qi(1,1)];
-    M(1, columns(prevM)+2) = [qi(1,2)];
-    
-    M(2, columns(prevM)+1) = [qi(2,1)];
-    M(2, columns(prevM)+2) = [qi(2,2)];
-    
-    M(3, columns(prevM)+1) = [x];
-    M(3, columns(prevM)+2) = [x];
-    
-    M(4, columns(prevM)+1) = [y];
-    M(4, columns(prevM)+2) = [y];
-  else
-    disp('not an obstacle');
-  endif
-  figure 2
-  hold on
-  plot(M(1,:), M(1,:), '*', 'Color', 'r')
-  figure 3
-  hold on
-  plot(M(1,:), M(1,:), '*', 'Color', 'k')
-endfunction

checkIV.m

@@ -1,19 +0,0 @@
-function Valid = checkIV(M)
-  LiCols = licols(M);
-  n=[]; %count how many times the Linear Independent Column is present in M
-  Valid = 0; %booleqn representing zhether or not a Linear Independent Column is present only once
-  for i=1:columns(LiCols)
-    n(i)=0;
-    for j=1:columns(M)
-      if M(j)==LiCols(i)
-        n(i)++;
-      endif
-    endfor
-  endfor
-  
-  for z=1:columns(n)
-    if n(i)==1
-      Valid = 1;
-    endif
-  endfor
-endfunction

licols.m

@@ -1,34 +0,0 @@
-function [Xsub,idx]=licols(X,tol)
-%Extract a linearly independent set of columns of a given matrix X
-%
-%    [Xsub,idx]=licols(X)
-%
-%in:
-%
-%  X: The given input matrix
-%  tol: A rank estimation tolerance. Default=1e-10
-%
-%out:
-%
-% Xsub: The extracted columns of X
-% idx:  The indices (into X) of the extracted columns
-   if ~nnz(X) %X has no non-zeros and hence no independent columns
-       
-       Xsub=[]; idx=[];
-       return
-   end
-   if nargin<2, tol=1e-10; end
-   
-           
-     [Q, R, E] = qr(X,0); 
-     
-     if ~isvector(R)
-      diagr = abs(diag(R));
-     else
-      diagr = abs(R(1));   
-     end
-     %Rank estimation
-     r = find(diagr >= tol*diagr(1), 1, 'last'); %rank estimation
-     idx=sort(E(1:r));
-     Xsub=X(:,idx);                      
-                           

mainPRM.m

@@ -1,18 +0,0 @@
-clear all
-close all
-
-points = []
-randV = 360;
-
-L1 = 2; %swap
-L2 = 1;
-
-for i=1:10
-  
-  q1 = randi(randV);
-  q2 = randi(randV);
-  [x, y] = buildPRM (q1, q2, L1, L2);
-
-  points = [points [x;y]];
-endfor
-points

planPathRRT.m

@@ -46,7 +46,7 @@ function planPathRRT
     endif
   endfor
   
-  nodeTrajectory = [nodeTrajectory(1:maxIndex) max(nodeTrajCut) 1]; % as we want to travel the maximal distance from the start point, withouth going through the first branches, we take the maximum value of the nodeTrajCut array
+  nodeTrajectory = [nodeTrajectory(1:maxIndex-1) max(nodeTrajCut) 1]; % as we want to travel the maximal distance from the start point, withouth going through the first branches, we take the maximum value of the nodeTrajCut array
   
   for i=1:columns(nodeTrajectory) % running through the new nodeTrajectory array
     Q1plot = [Q1plot points(1, nodeTrajectory(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
@@ -95,9 +95,11 @@ function planPathRRT
   hold all
   text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');% we plot the shortest path in the x y cartesian space
   
-  for i=2:columns(nodeTrajectory)-1
-    text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
-  endfor
+  for i=2:columns(nodeTrajectory)
+    if nodeTrajectory(i) != 1
+      text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
+    endif
+ endfor
   
   text(S(1), S(2), 'S', 'FontSize', 20);
   text(G(1), G(2), 'G', 'FontSize', 20);

test.m

@@ -1,37 +0,0 @@
-clc
-clear all
-close all
-L1 = 2;
-L2 = 1;
-randV = 360;
-a = [L1, L2];
-alpha = [0, 0];
-jointNumber = 1;
-d = [0, 0];
-petitcarre = 0;
-haut = 0;
-bas = 0;
-Bmatrix = [0; 0; 0; 1];
-bon = 0;
-for i = 1:1000
-
-  theta = [randi(randV), randi(randV)];
-  jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber);
-  b_P_ee = jTee*Bmatrix;
-  %Is the end effector colliding with obstacle
-  x=b_P_ee(1);
-  y=b_P_ee(2);
-  if(x<=-L2 && x<=L2 && y>=-L2 && y<=L2)
-    petitcarre++;
-  elseif(y >= L1)
-    haut++;
-  elseif(y <= -L1)
-    bas++;
-  else
-    bon++;
-  endif
-endfor
-bon
-petitcarre
-haut
-bas