1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40
| l1 = 120; % longueur bras 1
l2 = l1; % longueur bras 2
thetamax=180;
thetamax=thetamax*pi/180;
theta1 = 0:0.05:pi; % valeurs de theta1 pour le bras
theta2 = -thetamax/2:0.05:thetamax/2; % valeurs de l'angle theta2
[THETA1, THETA2] = meshgrid(theta1, theta2); %grille
X = l1 * cos(THETA1) + l2 * cos(THETA1 + THETA2); % calcul des coordonnées en x...
Y = l1 * sin(THETA1) + l2 * sin(THETA1 + THETA2); % ...et y !
data1 = [X(:) Y(:) THETA1(:)]; % création d'un "dataset" (qu'est-ce au juste ?)
data2 = [X(:) Y(:) THETA2(:)];
%plot Inverse Kinematics
plot(X(:), Y(:), 'b.');
axis equal;
xlabel('X','fontsize',10)
ylabel('Y','fontsize',10)
%
hold on
hp = plot(NaN,NaN, '-o', ...
'LineWidth', 5, ...
'MarkerSize', 5, ...
'MarkerFaceColor', 'r',...
'MarkerEdgeColor', 'none');
for k = 1:numel(theta1)
x = [ 0
l1*cos(theta1(k))
l1*cos(theta1(k))+l2*cos(theta1(k)+theta2(k))];
y = [ 0
l1*sin(theta1(k))
l1*sin(theta1(k))+l2*sin(theta1(k)+theta2(k))];
set(hp, 'XData', x, 'YData', y)
drawnow
end |
Partager