2.1 Dùng hàm .

teach của Peter Corke

* Source code hàm .teach
%link lenghts
L1 = 3;
L2 = 3;
L3 = 3;

%D-H
H0_1 = Link([0, L1, 0, 0, 0]);
H0_1.qlim =[-pi/2 pi/2];
H1_2 = Link([0, 0, 0, -pi/2, 1, 0]);
H1_2.qlim = [0 10];
H2_3 = Link([0, 0, 0, 0, 1, 0]);
H2_3.qlim = [0 10];
Robot = SerialLink([H0_1 H1_2 H2_3], 'name', 'RPP_Robot');
Robot.plot([0 0 0], 'workspace', [-10 10 -10 10 0 10]);
Robot.teach

2.2 Thiết kế GUI động học thuận và động học nghịch

* Giao diện thiết kế GUI hoàn chỉnh

* Source code
Phần động học thuận:
function btn_Forward_Callback(hObject, eventdata, handles)
% hObject handle to btn_Forward (see GCBO)
% eventdata reserved - to be defined in a future version
of MATLAB
% handles structure with handles and user data (see
GUIDATA)
syms a1 a2 a3 Theta_1 d2 d3
%link lenghts
L1 = 3;
L2 = 3;
L3 = 3;

Th_1 = str2double(handles.Theta_1.String)*pi/180;
D2 = str2double(handles.d2.String);
D3 = str2double(handles.d3.String);
%D-H

H0_1 = Link([0, L1, 0, 0, 0]);

H0_1.qlim =[-pi/2 pi/2];
H1_2 = Link([0, 0, 0, -pi/2, 1, 0]);
H1_2.qlim = [0 D2];
H2_3 = Link([0, 0, 0, 0, 1, 0]);
H2_3.qlim = [0 D3];
Robot = SerialLink([H0_1 H1_2 H2_3], 'name', 'RPP_Robot');
Robot.plot([Th_1 D2 D3], 'workspace', [-10 10 -10 10 0
10]);
T = Robot.fkine([Th_1 D2 D3]);
% Tim toa do X Y Z tuong ung theo ma tran T da cho trong
vi du 3.2
handles.Pos_X.String = num2str(T(1,4));
handles.Pos_Y.String = num2str(T(2,4));
handles.Pos_Z.String = num2str(T(3,4));
Phần động học nghịch
function btn_Inverse_Callback(hObject, eventdata, handles)
% hObject handle to btn_Inverse (see GCBO)
% eventdata reserved - to be defined in a future version
of MATLAB
% handles structure with handles and user data (see
GUIDATA)
PX = str2double(handles.Pos_X.String);
PY = str2double(handles.Pos_Y.String);
PZ = str2double(handles.Pos_Z.String);
%link lenghts
 L1 = 3;
L2 = 3;
L3 = 3;

%D-H
H0_1 = Link([0, L1, 0, 0, 0]);
H0_1.qlim =[-pi/2 pi/2];
H1_2 = Link([0, 0, 0, -pi/2, 1, 0]);
H1_2.qlim = [0 10];
H2_3 = Link([0, 0, 0, 0, 1, 0]);
H2_3.qlim = [0 10];
Robot = SerialLink([H0_1 H1_2 H2_3], 'name', 'RPP_Robot');

T = [ 1 0 0 PX
0 1 0 PY
0 0 1 PZ
0 0 0 1];
J = Robot.ikine(T, [0 0 0], [1 1 1 0 0 0]);
handles.Theta_1.String = num2str(J(1)*180/pi);
handles.d2.String = num2str(J(2));
handles.d3.String = num2str(J(3));
Robot.plot(J,'workspace', [-10 10 -10 10 0 10]);

2.3 So sánh
* Nhập tọa độ x, y, z vào phần Inverse bên GUI cho ra các góc:
* Thay các góc vào giao diện .teach cho ra tọa độ
* Kết luận
Do giải động học nghịch đối với trường hợp số biến khớp bằng với số ẩn sẽ cho ra nhiều
cặp nghiệm khác nhau nên đôi lúc kết quả sẽ rơi vào trùng nhau (Lần 1 và 2) hoặc khác
nhau (Lần 3) giữa GUI và Teach. Điều này phụ thuộc vào cấu trúc cụ thế và vị trí mục
tiêu của nó. Không phải tất cả robot nào cũng có nhiều cặp nghiệm, có một số robot chỉ
cho ra một cặp nghiệm duy nhất cho mỗi vị trí mục tiêu.