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;