puma机器人运动学正解MATLAB算法

本程序在MATLAB环境下编写PUMA机器人的运动学正解算法，根据输入的机器人各关节角度得出机器人末端执行器的空间位姿，并且通过仿真验证。最终所有的关节角度数据和末端空间位姿均存储在相应的数据文件中，便于后续程序的引用和处理。

%this program is to calculate the transformation matrix from joint 1 to
%joint 6; and use the cycling program to generate the position data and
%joint data;


%this parameter is for PUMA 560
alf=[pi/2 0 -pi/2 pi/2 -pi/2 0];
a=[0 0.4318 0.0203 0 0 0];
d=[0 0 0.15 0.4318 0 0];
% this is the Home_Position
%thta=[0 0 0 0 0 0];
% qz position 
%thta=[0 0+pi/2 0 0 0+pi 0+pi];
% qr position joint is exceed the joint value
%thta=[0 pi/2+pi/2 -pi/2 0 0+pi 0+pi];
% qs position 
%thta=[0 0+pi/2 -pi/2 0 0+pi 0+pi];
% qn position 
%thta=[0 pi/6+pi/2 -2*pi/3 0 pi/4+pi 0+pi];

%thta=[-pi/2 pi/2-pi/4 -pi/3 -2*pi/3 pi-pi/2 pi-2*pi/3];
thta1=[-pi/2 -pi/3 0 pi/3 pi/2];
    thta2=[-pi -pi/2 -pi/6 0 pi/6];
       thta3=[-pi/6 0 pi/4 pi/2 2*pi/3];
        thta4=[-pi/2 0 pi/4 pi/3 pi/2 2*pi/3];
        thta5=[-pi/2 -pi/3  pi/6 pi/4 pi/2];
        thta6=[-pi -2*pi/3 -pi/2 0 pi/4 pi 4*pi/3];
        pjoints=[];
        ppos=[];
        pjoint1=[];
        save ‘pumajoints.mat‘ pjoints;
        save ‘pumaposition.mat‘ ppos;
        save ‘pumafirst.mat‘ pjoint1;
x=sin（alf）;
x（abs（x）<1e-16）=0;
y=cos（alf）;
y（abs（y）<1e-16）=0;

for j=2:6
for k=2:4
    for l=1:5
        for m=2:5
            for n=1:4
                for s=3:4
                    total=[thta1（s）-0.001 thta2（n） thta3（l） thta4（m） thta5（k） thta6（j）];
                    %actjoin=total-thta;
%Robotics Vision & Control: （c） Peter Corke result 后置坐标法建模（标准建模方法std-DH）
%矩阵变换顺序为Trans（00di）Rot（zthta）trans（ai00）Rot（xalf）
%运算结果与p560.fkine（q