buildPRM function
This commit is contained in:
parent
eebbd604c1
commit
270590a99d
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
Loading…
Reference in New Issue