puma560 6轴机械臂的运动学正逆解算法

6轴机械臂的运动学正逆解算法，仅供参考。

function p = axis6_fkine3（theta0theta1theta2theta3theta4theta5xyz）
%T1
t1 = [
    cosd（theta0） -sind（theta0） 0 0;
    sind（theta0） cosd（theta0） 0 0;
    0 0 1 0;
    0 0 0 1
    ];
%T2
t2 = [
    0 0 1 0;
    -sind（theta1） -cosd（theta1） 0 0;
    cosd（theta1） -sind（theta1） 0 0.178;
    0 0 0 1
    ];
%T3
t3 = [
    cosd（theta2） -sind（theta2） 0 0.205;
    sind（theta2） cosd（theta2） 0 0;
    0 0 1 0;
    0 0 0 1
    ];
%T4
t4 = [
    0 0 1 0.105;
    -cosd（theta3）  sind（theta3） 0 0.000;
    -sind（theta3） -cosd（theta3） 0 0;
    0 0 0 1;
    ];
%T5
t5 = [
    sind（theta4）  cosd（theta4） 0 0;
    0 0 1 0;
    cosd（theta4） -sind（theta4） 0 0.105;
    0 0 0 1
    ];
%T6
t6 = [
    0   0 1 0.1315;
    cosd（theta5）  -sind（theta5） 0 0;
    sind（theta5） 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      12098  2016-04-04 22:40  axis6_fkine3.m

     文件       7672  2016-04-04 22:38  axis6_ikine3.m

----------- ---------  ---------- -----  ----

                19770                    2