puma2.m

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

clear;
rou = pi/180;
delta = 30;

total = 100+


a2 = 431.8;
a3 = 20.32;
d1 = 660.4;
d2 = 149.09;
d4 = 433.07;
d6 = 56.25;
i  = 1;

X=zeros(1,total);
Y=zeros(1,total);
Z=zeros(1,total);


    for x5 = -100*rou : delta * rou : 100*rou
        for x4 = -110*rou : delta * rou : 170*rou
            for x3 = -45*rou : delta * rou : 225*rou
                for x2 = -225*rou : delta * rou : 45*rou
                    for x1 = -160*rou : delta * rou : 160*rou
                        X(i) = a2*cos(x1)*cos(x2) - d6*(sin(x5)*(sin(x1)*sin(x4) - cos(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3))) + cos(x5)*(cos(x1)*cos(x2)*sin(x3) + cos(x1)*cos(x3)*sin(x2))) - d2*sin(x1) - d4*(cos(x1)*cos(x2)*sin(x3) + cos(x1)*cos(x3)*sin(x2)) + a3*cos(x1)*cos(x2)*cos(x3) - a3*cos(x1)*sin(x2)*sin(x3);
                        Y(i) = d6*(sin(x5)*(cos(x1)*sin(x4) - cos(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))) - cos(x5)*(cos(x2)*sin(x1)*sin(x3) + cos(x3)*sin(x1)*sin(x2))) - d4*(cos(x2)*sin(x1)*sin(x3) + cos(x3)*sin(x1)*sin(x2)) + d2*cos(x1) + a2*cos(x2)*sin(x1) + a3*cos(x2)*cos(x3)*sin(x1) - a3*sin(x1)*sin(x2)*sin(x3);
                        Z(i) = d1 - d6*((sin(x4 - x5)*sin(x2 + x3))/2 - (sin(x2 + x3)*sin(x4 + x5))/2 + cos(x2 + x3)*cos(x5)) - d4*cos(x2 + x3) - a3*sin(x2 + x3) - a2*sin(x2);
                        i = i+1;
                    end
                end
            end
        end
    end

plot3(X,Y,Z)