nicolas.traglia
/
S7AdvancedRoboticsT5
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added basic frame for buildPRM

This commit is contained in:
NicolasTraglia 2023-12-02 18:05:05 +01:00
parent 48209ad309
commit ee02d5d229
2 changed files with 97 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
IsIntersecting.m Normal file
View File

@ -0,0 +1,23 @@
function intersect = IsIntersecting (closestPoint, newPoint)
x3 = [-L2 -L2 L2 L2];
y3 = [-L2 L2 L2 -L2];
x1 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
y1 = [-L1-L2 L1+L2 L1+L2 -L1-L2];
x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
y2 = [-L1 L1 L1 -L1];
% checks if the path is crossed by an obstacle
crossesObstacle = false;
for i = 1:length(x1)
edge1 = [x1(i), y1(i), x1(mod(i, 4) + 1), y1(mod(i, 4) + 1)];
edge2 = [x2(i), y2(i), x2(mod(i, 4) + 1), y2(mod(i, 4) + 1)];
edge3 = [x3(i), y3(i), x3(mod(i, 4) + 1), y3(mod(i, 4) + 1)];
% Check if the line intersects with any obstacle edge
if doIntersect(closestPoint, newPoint, edge1(1:2), edge1(3:4)) || ...
doIntersect(closestPoint, newPoint, edge2(1:2), edge2(3:4)) || ...
doIntersect(closestPoint, newPoint, edge3(1:2), edge3(3:4))
crossesObstacle = true;
break;
end
end
endfunction

74
buildPRM.m Normal file
View File

@ -0,0 +1,74 @@
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');