89 lines
2.9 KiB
Matlab
89 lines
2.9 KiB
Matlab
## Author: adril <adril@LAPTOP-EJ1AIJHT>
|
|
## Created: 2022-12-06
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
|
|
%
|
|
% Task:
|
|
%
|
|
% Inputs:
|
|
% - rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2
|
|
% - nbPoints : number of points required
|
|
% - L1, L2 : lengths of the links (in m)
|
|
% - MapFilename : the name of the file to be saved for the map
|
|
%
|
|
% Outputs:
|
|
% - None
|
|
%
|
|
% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
|
|
% 06/12/2022
|
|
%
|
|
% Check library matgeom: https://octave.sourceforge.io/matgeom/overview.html
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
|
|
Points=zeros(2, nbPoints);
|
|
MatrixOfLinks=zeros(nbPoints, nbPoints);
|
|
alpha=[0;0];
|
|
d=[0;0];
|
|
a=[L1;L2];
|
|
jointNumber=[1;2];
|
|
|
|
figure 1
|
|
b=drawCircle(0, 0, L2);
|
|
c=drawCircle(0, 0, L1);%teacher's functions for drawing circles
|
|
|
|
poly_a=circleToPolygon([0 0 L2], 32);%create a polygon for matgeom with the circle info (smaller one)
|
|
poly_b=circleToPolygon([0 0 L1], 32);%bigger one radius=2
|
|
|
|
for i=1:nbPoints % won't give 10 points if intersection happens
|
|
|
|
Q=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
|
|
|
|
theta=[Q(1,1);Q(2,1)];
|
|
OutOfRange=0; %set the boolean
|
|
|
|
bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
|
|
jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values
|
|
|
|
if (jTee(2,1)>=L1)
|
|
OutOfRange=1; %is out if in that area
|
|
elseif (jTee(2,1)<=-L1)
|
|
OutOfRange=1;
|
|
elseif (abs(jTee(1,1)) <= L2 && abs(jTee(2,1)) <=L2)
|
|
OutOfRange=1;
|
|
endif
|
|
|
|
if (OutOfRange==0)
|
|
Points(1:2, i)=jTee; %assign the current random point to the Points matrix
|
|
MatrixOfLinks(i, i)=1; %it doesnt intersect with the obstacles when compared to itself
|
|
if i>=2 %i=1 useless
|
|
for j=1:i %cmare with the other points if there is an intersection
|
|
intersect=0; %set no intersection to start with
|
|
|
|
dx=Points(1,i)-Points(1, j); %dx for creating the line - dif between x actual and x at n-1
|
|
dy=Points(2,i)-Points(2, j);
|
|
LINE=[Points(1, i), Points(2, i), dx, dy]; % creation of the vector LINE
|
|
|
|
L = createLine(LINE); %creation of the line with matgeom
|
|
hold on %plotting the line
|
|
drawLine(L);
|
|
|
|
if (isempty(intersectLinePolygon(L, poly_a))!=1)%intersect returns a vector of points of intersection
|
|
%meaning that if it is empty, there is no intersection
|
|
intersect=1; % intersection happenned
|
|
else
|
|
MatrixOfLinks(i, j)=1;
|
|
MatrixOfLinks(j,i)=1;
|
|
hold on
|
|
drawPoint(jTee(1,1), jTee(2,1)); %draw the point
|
|
endif
|
|
endfor
|
|
endif
|
|
endif
|
|
endfor
|
|
|
|
Points %display the points vector to check on the graph
|
|
|
|
end
|