puma4.m

PUMA560机器人正解程序包含六个关节

clear                        
rou = pi/180;
delta1 = 16;%设定步长
delta2 = 27;
delta3 = 27;
a2 = 431.8;%设定常量的值
a3 = 20.32;
d1 = 660.4;
d2 = 149.09;
d4 = 433.07;
d6 = 56.25;
total = (delta1+1)*(delta2+1)*(delta3+1)%计算总的计算量
i  = 1;
X=zeros(1,total);%设置结果矩阵
Y=zeros(1,total);
Z=zeros(1,total);


    %for x5 = -100*rou : 200 * rou / delta : 100*rou
        %for x4 = -110*rou : 280 * rou / delta : 170*rou
            for x3 = -45 : 270 / delta1 : 225
                for x2 = -225 : 270  / delta2 : 45
                    for x1 = -160 : 320  / delta3 : 160
                        X(i) = cos(x1)*a3*cos(x2-x3)+cos(x1)*a2*cos(x2)-sin(x1)*d2;
                        Y(i) = sin(x1)*a3*cos(x2-x3)+sin(x1)*a2*cos(x2)+cos(x1)*d2;
                        Z(i) = -a3*sin(x2+x3)-a2*sin(x2)+d1;
                        i = i+1;
                    end
                end
            end
        %end
    %end

%plot3(X,Y,Z)