223
Jq = taymay2dof_Jacob(a(1), b(1));
dq = inv(Jq)*(-f);
a(2) = a(1)+dq(1); b(2) = b(1)+dq(2);
a(1) = a(2); b(1) = b(2);
f = taymay2dof_ptlk(a(1), b(1), xE(i), yE(i));
k = k+1;
end
q1(i) = a(1); q2(i) = b(1);
end
k = 1:1:n;
figure(1)
plot(k, q1,'k-', k, q2,'k--', 'linewidth',1), grid on
legend('q_1', 'q_2'), xlabel('k = 1..n'), ylabel('[rad]')
axis([0 n+1 0 2.1])
figure(2)
x0 = 0; y0 = 0; plot(x0,y0,'o-','linewidth',1), hold on
for i = 1:n
xA(i) = l1*cos(q1(i)); xE(i) = xA(i)+l2*cos(q1(i)-q2(i));
yA(i) = l1*sin(q1(i)); yE(i) = yA(i)+l2*sin(q1(i)-q2(i));
x_day = [x0, xA(i), xE(i)];
y_day = [y0, yA(i), yE(i)];
plot(x_day, y_day, 'k-','linewidth',1), grid on, axis equal
end
Chạy chương trình cho ta các kết quả như trong bảng 9-5. ðồ thị của các góc khớp
và cấu hình tương ứng của rôbốt ñược ñưa ra như trên hình 9-36.
0
5
10
15
20
0
0.5
1
1.5
2
k = 1..n
[r
a
d
]
q
1
q
2
0
0.5
1
-0.2
0
0.2
0.4
0.6
0.8
1
x [m]
y
[
m
]
Hình 9-36. ðồ thị các tọa ñộ
1
2
,
q q
và cấu hình của rôbốt
