function DrawObstacles() %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %function DrawObstacles % % Task: Creates 10 000 random points in the x y cartesian space that are part of the known obstacles % Computes their corresponding q1 q2 values thanks to Inverse Kinematics % adds the obstacles in a red color to the figure that should have been previously drawned using PRM or RRT algorithms % % % Inputs: % % Outputs: % % % Thomas OLIVE (thomas.olive@ecam.fr) % 18/12/2021 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [S, G, randVmin, randVmax, L1, L2, threshold] = setParams(); qi = []; q1_plot = []; q2_plot = []; x_plot = []; y_plot = []; for i=1:10000 x = -(L1+L2) + rand()*2*(L1+L2); %creating random x and y values y = -(L1+L2) + rand()*2*(L1+L2); if(y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))% keeping them if they are inside the obstacles [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y);% computing their corresponding q1 q2 coordinates for j=1:nbSol % saving the computed coordinates q1 = qi(j,1); q2 = qi(j,2); if q2 > 180 q2 = q2-360; %the IK function is returning q2 values between 0 and 360 endif q1_plot = [q1_plot q1]; q2_plot = [q2_plot q2]; endfor x_plot = [x_plot x]; y_plot = [y_plot y]; endif endfor %plotting the points in the two different spaces, red color figure 1 plot(q1_plot, q2_plot, '*', 'Color', 'r') figure 2 plot(x_plot, y_plot, '*', 'Color', 'r') endfunction