73 lines
1.5 KiB
Matlab
73 lines
1.5 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
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
|
|
Points=zeros(1, nbPoints);
|
|
MatrixOfLinks=zeros(nbPoints, nbPoints);
|
|
alpha=[0;0];
|
|
d=[0;0];
|
|
a=[L1;L2];
|
|
jointNumber=[1;2];
|
|
|
|
for i=1:nbPoints
|
|
|
|
[q1;q2]=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
|
|
|
|
theta=[q1;q2];
|
|
OutOfRange=0;
|
|
|
|
bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber);
|
|
jTee=bTee(1:2, 4);
|
|
|
|
if (jTee(2,1)>=L1)
|
|
OutOfRange=1;
|
|
else if (jTee(2,1)<=-L1)
|
|
OutOfRange=1;
|
|
else if (abs(jTee(1,1)) <= L2 && abs(jTee(2,1)) <=L2)
|
|
OutOfRange=1;
|
|
endif
|
|
|
|
if (OutOfRange==0)
|
|
Points(i)=jTee;
|
|
MatrixOfLinks(i, i)=1;
|
|
for j=1:i
|
|
intersect=0;
|
|
if
|
|
|
|
intersect=1;
|
|
|
|
else
|
|
|
|
MatrixOfLinks(i, j)=1;
|
|
MatrixOfLinks(j,i)=1;
|
|
|
|
endif
|
|
|
|
|
|
|
|
endfor
|
|
endif
|
|
|
|
endfor
|
|
|
|
|
|
endfunction
|