added RRT

buildRRT.m

@@ -0,0 +1,105 @@
+## Author: adril <adril@LAPTOP-EJ1AIJHT>
+## Created: 2023-01-08
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%function buildRRT (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
+%
+% Task:
+%
+% Inputs:
+%	-
+%
+% Outputs:
+%	-
+%
+% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
+% 08/01/2023
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
+
+  hold off;
+  i = 1; %while
+  nbPoints=nbPoints+2;%adding the starting point
+  %Points=zeros(nbPoints, 2);
+  MatrixOfLinks=zeros(nbPoints, nbPoints);
+  alpha=[0;0];
+  d=[0;0];
+  a=[L1;L2];
+  jointNumber=[1;2];
+
+  figure 1; hold on;
+  b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
+  hold on;
+  c=drawCircle(0, 0, L2-L1);
+  hold on;
+  %creates the lines defining the prohibited areas
+  top_line = createLine([0,L1,1,0]);
+  bottom_line = createLine([0,-L1,1,0]);
+  drawLine(top_line);
+  hold on;
+  drawLine(bottom_line);
+  center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2];
+  drawPolygon(center_box);
+  hold on;
+
+  poly_a=circleToPolygon([0 0 L2-L1], 32);%create a polygon for matgeom with the circle info (smaller one)
+  poly_b=circleToPolygon([0 0 L1+L2], 32);%bigger one radius=3
+
+  Points(1, 1:2)=[2,0];%starting point
+  drawPoint(Points(1, 1), Points(1,2));
+
+  while i <= nbPoints
+
+    Q=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
+    theta=[Q(1,1);Q(2,1)];
+    Q_storage(1:2, i)=Q;
+    OutOfRange=0; %set the boolean
+    intersect=0;
+
+    bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
+    jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values
+    jTee=jTee';
+    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]);
+
+    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
+        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;
+      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