75 lines
1.7 KiB
Matlab
75 lines
1.7 KiB
Matlab
function path = buildPRM(L1, L2, point1, point2)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% function path = buildRRT(L1, L2, point1, point2)
|
|
% Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters
|
|
%
|
|
% Inputs:
|
|
% - L1: Arm n°1 length
|
|
% - L2: Arm n°2 length
|
|
% - point1: [x1,y1] starting point of x1 and y1 coordinates
|
|
% - point2: [x2,y2] goal point of x2 and y2 coordinates
|
|
%
|
|
% Output:
|
|
% -path: array of points
|
|
%
|
|
% author: Nicolas Traglia
|
|
% date: 01/12/2023
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
q1q2_valid = [];
|
|
counter = 0;
|
|
x1 = point1(1);
|
|
x2 = point2(1);
|
|
y1 = point1(2);
|
|
y2 = point2(2);
|
|
amountOfSample = 1000;
|
|
arraySamples = [];
|
|
sampleUpBoundX=L1+L2;
|
|
sampleLowBoundX=-L1-L2;
|
|
sampleUpBoundY=L1;
|
|
sampleLowBoundY=-L1;
|
|
while (length(arraySamples)<amountOfSample)
|
|
xSample = sampleLowBoundX + (sampleUpBoundX - sampleLowBoundX) * rand;
|
|
ySample = sampleLowBoundY + ((sampleUpBoundY) - (sampleLowBoundY)) * rand;
|
|
if (xSample.^2 +ySample.^2 <= (L1+L2).^2) && !(((xSample<(L1-L2))&&(xSample>(-L1+L2)))&&((ySample<(L1-L2))&&(ySample>(-L1+L2))))
|
|
arraySamples = [arraySamples; xSample, ySample];
|
|
endif
|
|
endwhile
|
|
|
|
plot([-L2; L2], [L2; L2], 'r');
|
|
hold on;
|
|
plot([L2; L2], [L2; -L2], 'r');
|
|
hold on;
|
|
plot([L2; -L2], [-L2; -L2], 'r');
|
|
hold on;
|
|
plot([-L2; -L2], [-L2; L2], 'r');
|
|
hold on;
|
|
|
|
plot([-(L1+L2); L1+L2], [L1 ; L1], 'r');
|
|
hold on;
|
|
plot([-(L1+L2); L1+L2], [-L1 ; -L1], 'r');
|
|
hold on;
|
|
|
|
drawCircle(0,0, L1+L2);
|
|
hold on;
|
|
drawCircle(0,0, L1-L2);
|
|
hold on;
|
|
|
|
plot(x1,y1,"b");
|
|
hold on;
|
|
plot(x2,y2,"g");
|
|
hold on;
|
|
|
|
for i =1:length(arraySamples)
|
|
plot(arraySamples(i,1),arraySamples(i,2),"black");
|
|
hold on;
|
|
endfor
|
|
|
|
xlim([-(L1+L2) (L1+L2)]);
|
|
ylim([-(L1+L2) (L1+L2)]);
|
|
|
|
xlabel('x(mm)');
|
|
ylabel('y(mm)');
|
|
title('Cartesian space');
|
|
|