From 3ed014ca0a046ead0f45595f1007269242fc11f0 Mon Sep 17 00:00:00 2001
From: Adrien Lasserre <adrien.lasserre@ecam.fr>
Date: Thu, 12 Jan 2023 15:24:46 +0100
Subject: [PATCH] corrected the error for RRT - finished it

---
 buildRRT.m | 95 +++++++++++++++++++++++++-----------------------------
 1 file changed, 44 insertions(+), 51 deletions(-)

diff --git a/buildRRT.m b/buildRRT.m
index e18a44e..ec75b29 100644
--- a/buildRRT.m
+++ b/buildRRT.m
@@ -4,15 +4,21 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %function buildRRT (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
 %
-% Task:
+% Task: Use the RRT method to reach a desired goal on the cartesian space
 %
 % Inputs:
-%	-
+%	- rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2
+% - nbPoints : number of points required
+% - L1, L2 : lengths of the links (in m)
+% - fixedLength : length of the short link
+% - start : starting point
+% - goal : ending point
 %
 % Outputs:
-%	-
+%	- None
 %
-% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
+% Adrien Lasserre (adrien.lasserre@ecam.fr) &
+% Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
 % 08/01/2023
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
@@ -26,7 +32,7 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
   alpha=[0;0];
   d=[0;0];
   a=[L1;L2];
-  jointNumber=[1;2];
+  jointNumber=1;
 
   figure 1; hold on;
   b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
@@ -48,7 +54,7 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
 
   %set the starting point
   Points(1,1:2) = start;
-  
+
   %draw the start and goal points
   drawPoint(start(1, 1), start(1,2));
   drawPoint(goal(1, 1), goal(1,2));
@@ -64,10 +70,29 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
     bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
     jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values
     jTee=jTee';
-    
-    L = createEdge(goal, jTee);
-    
-    if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1 | isempty(intersectEdgePolygon(L, top_line))!=1 | isempty(intersectEdgePolygon(L, bottom_line))!=1)
+
+    index=findClosestPoint(jTee, Points);
+    dx=jTee(1,1)-Points(index, 1);
+    dy=jTee(1,2)-Points(index, 2);
+    E=sqrt(dx^2+dy^2);
+    dx=(dx)*fixedLength/E;
+    dy=(dy)*fixedLength/E;
+    L = createEdge(Points(index, :), [Points(index, 1)+dx, Points(index,2)+dy]);
+
+    if ((Points(index,2)+dy)>=L1)
+      OutOfRange=1; %is not valid if in that area
+    elseif ((Points(index,2)+dy)<=-L1)
+      OutOfRange=1;
+    elseif (abs((Points(index,1)+dx)) <= L2 && abs((Points(index,2)+dy)) <=L2)
+      OutOfRange=1;
+    endif
+
+    if (OutOfRange==0)
+
+      if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
+        intersect=1; % intersection happenned
+        disp('Intersect')
+      else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
         hold on;%plotting the line
         drawEdge(L);
         MatrixOfLinks(i, i)=1;
@@ -76,50 +101,18 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
         Points(i, 1:2)=[Points(index, 1)+dx, Points(index,2)+dy];
         hold on;
         drawPoint(Points(i, 1), Points(i,2)); %draw the point
-        i=i+1;
-        break;
-    else
-      index=findClosestPoint(jTee, Points)
-      dx=jTee(1,1)-Points(index, 1);
-      dy=jTee(1,2)-Points(index, 2);
-  ##    D = distancePoints(jTee, Points(index, :));
-      E=sqrt(dx^2+dy^2);
-      dx=(dx)*fixedLength/E;
-      dy=(dy)*fixedLength/E;
-      L = createEdge(Points(index, :), [Points(index, 1)+dx, Points(index,2)+dy]);
+        intersect=0;
+        L = createEdge(goal, Points(i, 1:2));
 
-      if ((Points(index,2)+dy)>=L1)
-        OutOfRange=1; %is not valid if in that area
-      elseif ((Points(index,2)+dy)<=-L1)
-        OutOfRange=1;
-      elseif (abs((Points(index,1)+dx)) <= L2 && abs((Points(index,2)+dy)) <=L2)
-        OutOfRange=1;
-      endif
-
-      if (OutOfRange==0)
-
-        if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1 | isempty(intersectEdgePolygon(L, top_line))!=1 | isempty(intersectEdgePolygon(L, bottom_line))!=1)
-          intersect=1; % intersection happenned
-        else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
-          hold on;%plotting the line
+        if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
+          intersect=1;
+        else
           drawEdge(L);
-          MatrixOfLinks(i, i)=1;
-          MatrixOfLinks(i, index)=1;
-          MatrixOfLinks(index, i)=1;
-          Points(i, 1:2)=[Points(index, 1)+dx, Points(index,2)+dy];
-          hold on;
-          drawPoint(Points(i, 1), Points(i,2)); %draw the point
-          i=i+1;
+          break;
         endif
-       endif
+
+        i=i+1;
+      endif
     endif
   endwhile
-
-  ## findClosestPoint(Point, POINTARRAY) Nx2
-  ## compute dx and dy
-  ## create edge fixed length, with closestpoint value and dx dy
-  ## draw edge
-  ## draw point
-  ## if line intersects do not draw neither points nor edge
-  qGraph (Q_storage, nbPoints, MatrixOfLinks)
 endfunction