45 lines
1.1 KiB
Matlab
45 lines
1.1 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
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
L1 = 2;
|
|
L2 = 1;
|
|
qi = [];
|
|
q1_plot = [];
|
|
q2_plot = [];
|
|
x_plot = [];
|
|
y_plot = [];
|
|
|
|
for i=1:10000
|
|
x = -(L1+L2) + rand()*2*(L1+L2);
|
|
y = -(L1+L2) + rand()*2*(L1+L2);
|
|
if((y >= L1)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2))
|
|
[q1 q2 q1_ q2_] = MyIK(L1, L2, x, y);
|
|
q1_plot = [q1_plot q1 q1_];
|
|
|
|
q2_plot = [q2_plot q2 q2_];
|
|
x_plot = [x_plot x];
|
|
|
|
y_plot = [y_plot y];
|
|
endif
|
|
endfor
|
|
figure 1
|
|
plot(q1_plot, q2_plot, '*', 'Color', 'r')
|
|
figure 2
|
|
plot(x_plot, y_plot, '*', 'Color', 'r')
|
|
endfunction
|