106 lines
3.0 KiB
Matlab
106 lines
3.0 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
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
%TODO: Fix given function dijkstra ; Add draw shortest path once dijkstra is fixed (can't test/debug if no input is given)
|
|
q1q2_valid = [];
|
|
counter = 0;
|
|
x1 = point1(1);
|
|
x2 = point2(1);
|
|
y1 = point1(2);
|
|
y2 = point2(2);
|
|
|
|
%sampling points
|
|
amountOfSample = 10; %amount of samples ; don't forget to make this variable later
|
|
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
|
|
|
|
%getting matrix of distance between points
|
|
%TODO: add = NaN if obstacle (deadzone)
|
|
display(point1);
|
|
display(arraySamples);
|
|
pointsDijkstra=[point1;arraySamples];
|
|
pointsDijkstra=[pointsDijkstra;point2];
|
|
matrixDijkstra=cell(length(pointsDijkstra),length(pointsDijkstra));
|
|
for i=1:1:length(pointsDijkstra)
|
|
pt1=[pointsDijkstra(i,1), pointsDijkstra(i,2)];
|
|
for j=1:length(pointsDijkstra)
|
|
pt2=[pointsDijkstra(j,1), pointsDijkstra(j,2)];
|
|
matrixDijkstra{i,j}=distBetweenPts(pt1,pt2);
|
|
endfor
|
|
endfor
|
|
matrixDijkstra{1,length(matrixDijkstra)}="NaN";
|
|
matrixDijkstra{length(matrixDijkstra),1}="NaN";
|
|
|
|
display(matrixDijkstra);
|
|
DijkstraResults=[0, 0, 0];
|
|
%DijkstraResults = dijkstra(length(matrixDijkstra{1}), matrixDijkstra);
|
|
%WARNING: function dijkstra currently does not return a correct value (example: minimum distance between [-2,0] and [2,0] = 1.71323)
|
|
%More debugging of given function required ATM
|
|
|
|
%draw inner deadzone
|
|
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;
|
|
|
|
% draw upper+lower deadzone
|
|
plot([-(L1+L2); L1+L2], [L1 ; L1], 'r');
|
|
hold on;
|
|
plot([-(L1+L2); L1+L2], [-L1 ; -L1], 'r');
|
|
hold on;
|
|
|
|
%draw circles
|
|
drawCircle(0,0, L1+L2);
|
|
hold on;
|
|
drawCircle(0,0, L1-L2);
|
|
hold on;
|
|
|
|
%draw start and end points
|
|
plot(x1,y1,"b");
|
|
hold on;
|
|
plot(x2,y2,"g");
|
|
hold on;
|
|
|
|
%draw all sampled points
|
|
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');
|
|
endfunction
|