-
Notifications
You must be signed in to change notification settings - Fork 0
/
simulasyon.m
78 lines (59 loc) · 2.02 KB
/
simulasyon.m
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
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
% Written by Leyla Tülü
% 17.06.2021 14:26:08
clc; clear all; close all;
figure
set(gcf,'Position',[100 100 1600 800])
% Defined link lenghts
l1 = 600;
l2 = 400;
l3 = 300;
% Starting position qs
% Target location for end effector
% qs -> q1 -> q2 -> q3 -> q4 -> q5 -> q1
x = [0, -300, -100, -300, 200, 200, -300];
y = [600, 500, 500, 500, 500, 500, 500];
z = [0, 200, 400, 500, 500, 200, 200];
t1 = [];
t2 = [];
t3 = [];
for i = 1:length(x)
clf
[t1(i), t2(i), t3(i)] = invkin(x(i), y(i), z(i), l1, l2, l3);
d2 = z(i);
% DH matrix parameters
a = [0, 0, l2, l3];
alpha = [0, 0, 0, 0];
d = [0, d2, 0, 0];
theta = [t1(i), 0, t3(i)-90, 0];
% T : Transformation Matrix
% Calculate the Transformation Matrix
for i = 1:length(theta)
T(:, :, i) = DH(a(i), alpha(i), d(i), theta(i));
end
% Calculate the forward kinematics
EndT = eye(4,4);
for k = 1:length(theta)
mat = EndT;
EndT = EndT * T(:,:,k);
plot3([mat(1,4) EndT(1,4)], [mat(2,4) EndT(2,4)], [mat(3,4) EndT(3,4)], 'k', 'LineWidth', 3);
hold on, grid on
axis([-550 550 -300 800 -100 800]);
scatter3(EndT(1,4), EndT(2,4), EndT(3,4), 'MarkerFaceColor', [0 1 0]);
scatter3(0, 0, 0, 'MarkerFaceColor', [0 1 0]);
title('3 DOF RPR MANIPULATOR SIMULATION')
xlabel('X-axis')
ylabel('Y-axis')
zlabel('Z-axis')
end
% Plotting orbital points
for k = 1:length(x)-1
plot3([x(k) x(k+1)], [y(k) y(k+1)], [z(k) z(k+1)],'c','LineWidth', 3), hold on
end
h = [-30 20 10];
view(h)
str1 = {['X : ', num2str(EndT(1,4)), ' Y : ', num2str(EndT(2,4)), ' Z : ', num2str(EndT(3,4))]};
str2 = {['X : ', num2str(0), ' Y : ', num2str(0), ' Z : ', num2str(0)]};
text(EndT(1,4)+10, EndT(2,4), EndT(3,4), str1, 'Color', 'b', 'FontSize', 12)
text(0, 0, -10, str2, 'Color', 'k', 'FontSize', 12)
pause(0.4)
end