53 lines
1.5 KiB
Matlab
53 lines
1.5 KiB
Matlab
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] = 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)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2)) % 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;
|
|
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
|