diff --git a/buildPRM.m b/buildPRM.m new file mode 100644 index 0000000..e7137cc --- /dev/null +++ b/buildPRM.m @@ -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)=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