buildPRM function

src/motion_planning/buildPRM.m

@@ -0,0 +1,96 @@
+function [q1q2_valid, position_valid] = buildPRM(L1, L2, nbPoints)
+%%%%%%%%%%%%%%%
+% function buildPRM(L1, L2, nbPoints)
+% ex. buildPRM(2000, 1000, 10)
+%
+% Inputs :
+% -L1: length of the first link (in mm)
+% -L2: length of the second link (in mm)
+% -nbPoints: the number of valid points in the roadmap
+%
+% Outputs :
+% -q1q2_valid: joints stored values
+% -position_valid: cartesian values
+%
+% Author : Adrien COMBE
+% Date : 27/11/2023
+% Last updated date : 01/12/2023
+%%%%%%%%%%%%%%%
+
+    q1q2_valid = [];
+    position_valid = [];
+
+    while (size(q1q2_valid, 1) < 2 * nbPoints)
+        % Samples randomly the joint space
+        q1 = rand() * 360;
+        q2 = rand() * 360;
+
+        % 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);
+        % Determine the position of the end-effector
+        position_ee = wTee(1:2, end);
+
+        % Checks if the end-effector is not hitting any obstacle
+        eeHittingObstacle = 0;
+        if (position_ee(2) >= L1)
+            eeHittingObstacle = 1;
+        endif
+
+        if (position_ee(2) <= -L1)
+            eeHittingObstacle = 1;
+        endif
+
+        if (position_ee(1) >= -L2 && position_ee(1) <= L2 && position_ee(2) >= -L2 && position_ee(2) <= L2)
+            eeHittingObstacle = 1;
+        endif
+
+        % Stores these random values
+        if (eeHittingObstacle == 0)
+            q1q2_valid = [q1q2_valid; theta];
+            position_valid = [position_valid; position_ee'];
+        endif
+
+    endwhile
+
+    % Plot the points
+    x = position_valid(:, 1);
+    y = position_valid(:, 2);
+    scatter(x, y, 'b', 'filled');
+    hold on;
+
+    % Plot the circle
+    t = linspace(0, 2 * pi, 100);
+    x_circle = (L1 + L2) * cos(t);
+    y_circle = (L1 + L2) * sin(t);
+    plot(x_circle, y_circle, 'g--');
+
+    % Plot the obstacle (centered rectangle)
+    x_square = [-L2, L2, L2, -L2, -L2];
+    y_square = [-L2, -L2, L2, L2, -L2];
+    plot(x_square, y_square, 'r--');
+
+    % Plot the borders
+    x_border = [-L1 - L2, L1 + L2, L1 + L2, -L1 - L2, -L1 - L2];
+    y_border1 = [-L1, -L1, -L1 - L2, -L1 - L2, -L1];
+    y_border2 = [L1, L1, L1 + L2, L1 + L2, L1];
+    plot(x_border, y_border1, 'r--');
+    plot(x_border, y_border2, 'r--');
+
+    % Plot the lines without going through the obstacle
+    for i = 1:length(x)
+        for j = 1:length(x)
+            if i ~= j
+                % Check if the line intersects with the obstacle
+                if segmentsIntersect(x(i), y(i), x(j), y(j), L1, L2) == false
+                    plot([x(i), x(j)], [y(i), y(j)], 'b-');
+                endif
+            endif
+        endfor
+    endfor
+end

src/motion_planning/segmentsIntersect.m

@@ -0,0 +1,60 @@
+function isIntersecting = segmentsIntersect(xi, yi, xj, yj, L1, L2)
+%%%%%%%%%%%
+% Check if two line segments intersect
+% Input:
+%   - segment: xi,yi,xj,yj coordinates of the first segment
+%   - L1 and L2 lengths for obstacle
+% Output:
+%   - isIntersecting: true if segments intersect, false otherwise
+% Author : Adrien COMBE
+% Date : 27/11/2023
+% Last updated date : 01/12/2023
+%%%%%%%%%%%
+    
+    segment1 = [xi, yi, xj, yj];
+    
+    for c = 1:4
+      if c == 1
+        segment2 = [-L2, -L2, L2, -L2];
+      endif
+      if c == 2
+        segment2 = [L2, -L2, L2, L2];
+      endif
+      if c == 3
+        segment2 = [L2, L2, -L2, L2];
+      endif
+      if c == 4
+        segment2 = [-L2, L2, -L2, -L2];
+      endif
+      
+      function orientation = findOrientation(p, q, r)
+        val = (q(2) - p(2)) * (r(1) - q(1)) - (q(1) - p(1)) * (r(2) - q(2));
+        if val == 0
+            orientation = 0; % Collinear
+        else
+            orientation = (val > 0) - (val < 0); % 1 for clockwise, -1 for counterclockwise
+        endif
+      end
+
+    % Find orientations of the four endpoint pairs
+    orient1 = findOrientation(segment1(1:2), segment1(3:4), segment2(1:2));
+    orient2 = findOrientation(segment1(1:2), segment1(3:4), segment2(3:4));
+    orient3 = findOrientation(segment2(1:2), segment2(3:4), segment1(1:2));
+    orient4 = findOrientation(segment2(1:2), segment2(3:4), segment1(3:4));
+
+    % General case: segments are not collinear
+    if orient1 ~= orient2 && orient3 ~= orient4
+        isIntersecting = true;
+        break
+    % Special cases: segments are collinear and overlap
+    elseif orient1 == 0 && onSegment(segment1(1:2), segment2(1:2), segment1(3:4)) ...
+        || orient2 == 0 && onSegment(segment1(1:2), segment2(3:4), segment1(3:4)) ...
+        || orient3 == 0 && onSegment(segment2(1:2), segment1(1:2), segment2(3:4)) ...
+        || orient4 == 0 && onSegment(segment2(1:2), segment1(3:4), segment2(3:4))
+        isIntersecting = true;
+        break
+    else
+    isIntersecting = false;
+    end
+    endfor
+end