97 lines
2.7 KiB
Matlab
97 lines
2.7 KiB
Matlab
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
|