120 lines
3.0 KiB
Matlab
120 lines
3.0 KiB
Matlab
function BUILDPRM(L1, L2, nbPoints)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% FUNCTION BUILDPRM
|
|
%
|
|
% INPUTS :
|
|
% - L1 : length of first link in mm
|
|
% - L2 : length of first link in mm
|
|
% - nbPoints : number of valid points in the RoadMap
|
|
%
|
|
% OUTPUTS :
|
|
% - None
|
|
%
|
|
% Author : Rémi Bussiere, remi.bussiere@ecam.fr
|
|
% Date : 03/12/2023
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
q1q2_valid = [];
|
|
counter = 0;
|
|
points = [];
|
|
valid = zeros(12, 12);
|
|
points = [ points ; [2, 0.01]];
|
|
|
|
while(size(q1q2_valid) < nbPoints)
|
|
% chooses random values for q1 and q2
|
|
q1 = rand()*360.0;
|
|
q2 = rand()*360.0;
|
|
|
|
% Create D-H table
|
|
theta = [q1, q2];
|
|
d = [0, 0];
|
|
a = [L1, L2];
|
|
alpha = [0, 0];
|
|
|
|
% computes the Forward Kinematics
|
|
wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
|
|
|
|
% determines position of end-effector
|
|
position_ee = wTee(1:2, end);
|
|
|
|
% check if it hits obstacles
|
|
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)
|
|
eeHittingObstacle = 1;
|
|
endif
|
|
if (position_ee(2) >= -L2 && position_ee(2) <= L2)
|
|
eeHittingObstacle = 1;
|
|
endif
|
|
|
|
% stores the position of the end-effector
|
|
if(eeHittingObstacle == 0)
|
|
q1q2_valid = [q1q2_valid; theta];
|
|
x = position_ee(1);
|
|
y = position_ee(2);
|
|
points = [ points ; [x, y]];
|
|
endif
|
|
|
|
endwhile
|
|
points = [ points ; [-2, 0.01]];
|
|
disp(points);
|
|
for i = 1:length(points)
|
|
x_old = points(i, 1);
|
|
y_old = points(i, 2);
|
|
for j = 1:length(points)
|
|
x = points(j, 1);
|
|
y = points(j, 2);
|
|
a_eq = (y_old - y)/(x_old - x);
|
|
b_eq = y - a_eq * x;
|
|
intersectest = 0;
|
|
|
|
if ( a_eq * L2 + b_eq >= - L2 && a_eq * L2 + b_eq <= L2)
|
|
intersectest = 1;
|
|
endif
|
|
if ( a_eq * (-L2) + b_eq >= - L2 && a_eq * (-L2) + b_eq <= L2 )
|
|
intersectest = 1;
|
|
endif
|
|
if ( ( L2 - b_eq )/a_eq >= -L2 && ( L2 - b_eq )/a_eq <= L2)
|
|
intersectest = 1;
|
|
endif
|
|
if ( ( (-L2) - b_eq )/a_eq >= -L2 && ( (-L2) - b_eq )/a_eq <= L2)
|
|
intersectest = 1;
|
|
endif
|
|
|
|
if( intersectest == 0 )
|
|
valid(i, j) = 1;
|
|
x_old = x;
|
|
y_old = y;
|
|
|
|
endif
|
|
endfor
|
|
endfor
|
|
%compute the djisktra algorithm
|
|
[nbNodes, visibilityGraph] = createVisibilityGraph(valid, points);
|
|
dijkstra(nbNodes, visibilityGraph);
|
|
|
|
% Plot the points with text labels
|
|
figure;
|
|
plot(points(:, 1), points(:, 2), 'o', 'MarkerSize', 2);
|
|
hold on;
|
|
title('Set of Points');
|
|
xlabel('X-axis');
|
|
ylabel('Y-axis');
|
|
grid on;
|
|
|
|
% Add text labels next to each point
|
|
for i = 1:size(points, 1)
|
|
text(points(i, 1), points(i, 2) + 0.1, num2str(i), 'HorizontalAlignment', 'center', 'FontSize', 12);
|
|
end
|
|
|
|
side_length = L2;
|
|
half_side = side_length;
|
|
x = [-half_side, -half_side, half_side, half_side, -half_side];
|
|
y = [-half_side, half_side, half_side, -half_side, -half_side];
|
|
plot(x, y, '-o');
|
|
endfunction |