44 lines
1.2 KiB
Matlab
44 lines
1.2 KiB
Matlab
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% function retval = buildPRM()
|
|
% Task:
|
|
%
|
|
% Inputs:
|
|
% - AL1: Length of first link
|
|
% - AL2: Length of second link
|
|
% Outputs:
|
|
% -
|
|
% author: Traglia Nicolas, nicolas.traglia@ecam.fr
|
|
% date: 22/11/2023
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
function retval = buildPRM (AL1, AL2, nOfPoints)
|
|
q1q2_valid = []; #Joint values q1 and q2 (q1 in ref of axis x, q2 in ref of arm1
|
|
counter=0;
|
|
while (size(q1q2_valid,1)<nOfPoints)
|
|
q1 = rand()*360.0; #Random angle 1 (dg)
|
|
q2 = rand()*360.0; #Random angle 2 (dg)
|
|
theta = [q1 q2];
|
|
d = [0 0];
|
|
a = [AL1 AL2];
|
|
alpha = [0 0];
|
|
wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
|
|
position_ee = wTee(1:2,end);
|
|
eeHittingObstacle=0;
|
|
% verify if point is possible
|
|
if (position_ee(2)>=AL1)
|
|
eeHittingObstacle=1;
|
|
elseif (position_ee(2)<=-AL1)
|
|
eeHittingObstacle=1;
|
|
elseif (position_ee(1)<=AL2 && position_ee(1)>=-AL2 && position_ee(2)<=AL2 && position_ee(2)>=-AL2)
|
|
eeHittingObstacle=1;
|
|
endif
|
|
% Store point
|
|
if(eeHittingObstacle==0)
|
|
q1q2_valid=[q1q2_valid; theta]
|
|
x = cos(q1)*AL1+cos(q1+q2)*AL2
|
|
y = sin(q1)*AL1+sin(q1+q2)*AL2
|
|
counter
|
|
endif
|
|
counter=counter+1;
|
|
endwhile
|
|
endfunction
|