puma2.asv

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

clear;
rou = pi/180;
delta = 30 * rou;
a2 = 31.8;
a3 = 20.32;
d2 = 149.09;
d4 = 433.07;
i  = 1;
for x6 = -266*rou:delta:266*rou
    for x5 = -100*rou:delta:100*rou
        for x4 = -110*rou:delta:170*rou
            for x3 = -45*rou:delta:225*rou
                for x2 = -225*rou:delta:45*rou
                    for x1 = -160*rou:delta:160*rou
                        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*A5; %由于A4和A6对工作空间无影响，为减小计算量，节省结算时间，将A4、A6略去
                        X(i) = T(1,4);
                        Y(i) = T(2,4);
                        Z(i) = T(3,4);
                        i = i+1;
                    end
                end
            end
        end
    end
end
%plot3(X,Y,Z)