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

created buildPRM.m

This commit is contained in:
NicolasTraglia 2023-11-22 09:58:57 +01:00
parent 48209ad309
commit 554a3bb696
1 changed files with 43 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
buildPRM.m Normal file
View File

@ -0,0 +1,43 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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