37 lines
577 B
Matlab
37 lines
577 B
Matlab
clc
|
|
clear all
|
|
close all
|
|
L1 = 2;
|
|
L2 = 1;
|
|
randV = 360;
|
|
a = [L1, L2];
|
|
alpha = [0, 0];
|
|
jointNumber = 1;
|
|
d = [0, 0];
|
|
petitcarre = 0;
|
|
haut = 0;
|
|
bas = 0;
|
|
Bmatrix = [0; 0; 0; 1];
|
|
bon = 0;
|
|
for i = 1:1000
|
|
|
|
theta = [randi(randV), randi(randV)];
|
|
jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber);
|
|
b_P_ee = jTee*Bmatrix;
|
|
%Is the end effector colliding with obstacle
|
|
x=b_P_ee(1);
|
|
y=b_P_ee(2);
|
|
if(x<=-L2 && x<=L2 && y>=-L2 && y<=L2)
|
|
petitcarre++;
|
|
elseif(y >= L1)
|
|
haut++;
|
|
elseif(y <= -L1)
|
|
bas++;
|
|
else
|
|
bon++;
|
|
endif
|
|
endfor
|
|
bon
|
|
petitcarre
|
|
haut
|
|
bas |