matlab机械手ikine函数,关于Mtalab-robotics工具箱中的fkine和ikine两个函数的问题讨论！...

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))