clear;
clc;
L1 = Link([-pi/2 0 0 0 0],'standard');
L2 = Link([0 10 0 -pi/2 0],'standard');
L3 = Link([0 9.5 0 0 0],'standard');
L4 = Link([-pi/2 3.2 -pi/2 0 0],'standard');
L5 = Link([0 0 0 14.5 0],'standard');
r = SerialLink([L1 L2 L3 L4 L5]);
%r.name = '5R工业机械臂-上电姿态';
q = [0 -pi/2 0 -pi/2 0];
r.teach(q);
T = fkine(r,q);
MDH=[-pi/2 0 0 0 0;
0 10 0 -pi/2 0;
0 9.5 0 0 0;
-pi/2 3.2 -pi/2 0 0;
0 0 0 14.5 0;];
T01=[cos(MDH(1,1)) -sin(MDH(1,1)) 0 MDH(1,3);
cos(MDH(1,4))*sin(MDH(1,1)) cos(MDH(1,4))*cos(MDH(1,1)) -sin(MDH(1,4)) -MDH(1,2)*sin(MDH(1,4));
sin(MDH(1,4))*sin(MDH(1,1)) sin(MDH(1,4))*cos(MDH(1,1)) cos(MDH(1,4)) MDH(1,2)*cos(MDH(1,4));
0 0 0 1];
T12=[cos(MDH(2,1)) -sin(MDH(2,1)) 0 MDH(2,3);
cos(MDH(2,4))*sin(MDH(2,1)) cos(MDH(2,4))*cos(MDH(2,1)) -sin(MDH(2,4)) -MDH(2,2)*sin(MDH(2,4));
sin(MDH(2,4))*sin(MDH(2,1)) sin(MDH(2,4))*cos(MDH(2,1)) cos(MDH(2,4)) MDH(2,2)*cos(MDH(2,4));
0 0 0 1];
T23=[cos(MDH(3,1)) -sin(MDH(3,1)) 0 MDH(3,3);
cos(MDH(3,4))*sin(MDH(3,1)) cos(MDH(3,4))*cos(MDH(3,1)) -sin(MDH(3,4)) -MDH(3,2)*sin(MDH(3,4));
sin(MDH(3,4))*sin(MDH(3,1)) sin(MDH(3,4))*cos(MDH(3,1)) cos(MDH(3,4)) MDH(3,2)*cos(MDH(3,4));
0 0 0 1];
T34=[cos(MDH(4,1)) -sin(MDH(4,1)) 0 MDH(4,3);
cos(MDH(4,4))*sin(MDH(4,1)) cos(MDH(4,4))*cos(MDH(4,1)) -sin(MDH(4,4)) -MDH(4,2)*sin(MDH(4,4));
sin(MDH(4,4))*sin(MDH(4,1)) sin(MDH(4,4))*cos(MDH(4,1)) cos(MDH(4,4)) MDH(4,2)*cos(MDH(4,4));
0 0 0 1];
T45=[cos(MDH(5,1)) -sin(MDH(5,1)) 0 MDH(5,3);
cos(MDH(5,4))*sin(MDH(5,1)) cos(MDH(5,4))*cos(MDH(5,1)) -sin(MDH(5,4)) -MDH(5,2)*sin(MDH(5,4));
sin(MDH(5,4))*sin(MDH(5,1)) sin(MDH(5,4))*cos(MDH(5,1)) cos(MDH(5,4)) MDH(5,2)*cos(MDH(5,4));
0 0 0 1];
% T56=[cos(MDH(6,1)) -sin(MDH(6,1)) 0 MDH(6,3);
% cos(MDH(6,4))*sin(MDH(6,1)) cos(MDH(6,4))*cos(MDH(6,1)) -sin(MDH(6,4)) -MDH(6,2)*sin(MDH(6,4));
% sin(MDH(6,4))*sin(MDH(6,1)) sin(MDH(6,4))*cos(MDH(6,1)) cos(MDH(6,4)) MDH(6,2)*cos(MDH(6,4));
% 0 0 0 1];
T06=T01*T12*T23*T34*T45;
