puma.m

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) -sin(x1) 0 0;sin(x1)  cos(x1) 0 0;0 0 1 0;0 0 0 1;];
                        A2=[cos(x2) -sin(x2) 0 0;0 0 1 d2;-sin(x2) -cos(x2) 0 0;0 0 0 1];
                        A3=[cos(x3) -sin(x3) 0 a2;sin(x3) cos(x3) 0 0;0 0 1 0;0 0 0 1];
                        %A4=[cos(x4) -sin(x4) 0 a3;0 0 1 d4;-sin(x4) -cos(x4) 0 0;0 0 0 1];
                        A5=[cos(x5) -sin(x5) 0 0;0 0 -1 0;sin(x5) cos(x5) 0 0;0 0 0 1];
                        %A6=[cos(x6) -sin(x6) 0 0;0 0 1 0;-sin(x6) -cos(x6) 0 0;0 0 0 1];
                        T=A1*A2*A3*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)