puma3.m

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

function puma3()
syms x1 x2 x3 x4 x5 x6 d1 d2 d3 d4 d5 d6 a1 a2 a3 a4 a5 a6

                        A1 = [cos(x1) 0 -sin(x1) 0;sin(x1) 0 cos(x1) 0;0 -1 0 d1;0 0 0 1];
                        A2 = [cos(x2) -sin(x2) 0 a2*cos(x2);sin(x2) cos(x2) 0 a2*sin(x2);0 0 1 d2;0 0 0 1];
                        A3 = [cos(x3) 0 -sin(x3) a3*cos(x3);sin(x3) 0 cos(x3) a3*sin(x3);0 -1 0 0;0 0 0 1];
                        A4 = [cos(x4) 0 sin(x4) 0;sin(x4) 0 -cos(x4) 0;0 1 0 d4;0 0 0 1];
                        A5 = [cos(x5) 0 -sin(x5) 0;sin(x5) 0 cos(x5) 0;0 -1 0 0;0 0 0 1];
                        A6 = [cos(x6) -sin(x6) 0 0;sin(x6) cos(x6) 0 0;0 0 1 d6;0 0 0 1];
                        T = A1*A2*A3*A4;
                        simple(T(1,4));
                        simple(T(2,4));
                        simple(T(3,4));                       
rou = pi/180;
delta = 10;

total = (delta+1)^6



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 : 200 * rou / delta : 100*rou
        for x4 = -110*rou : 280 * rou / delta : 170*rou
            for x3 = -45*rou : 270 * rou / delta : 225*rou
                for x2 = -225*rou : 270 * rou / delta : 45*rou
                    for x1 = -160*rou : 320 * rou / delta : 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)