puma5m.asv

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

                        
rou = pi/180;
delta1 = 20;
delta2 = 10;
delta3 = 10;
delta4 = 14;
delta5 = 27;
delta6 = 27;

total = (delta1+1)*(delta2+1)*(delta3+1)



a2 = 431.8;
a3 = 0;%20.32;
d1 = 0;%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 : 200 / delta5 : 100
%        for x4 = -110 : 280  / delta4 : 170
            for x1 = -160 *rou : 320*rou  / delta1 : 160*rou
               for x2 = -225*rou : 270*rou  / delta2 : 45*rou
                    for x3 = -45*rou : 270*rou / delta3 : 225*rou
                        X(i) = 1/2*d4*sin(-x3+x1-x2)-1/2*d4*sin(x3+x1+x2)+1/2*a3*cos(-x3+x1-x2)+1/2*a3*cos(x3+x1+x2)+1/2*a2*cos(x1-x2)+1/2*a2*cos(x1+x2)-sin(x1)*d2;
                        Y(i) = 1/2*d4*cos(x3+x1+x2)-1/2*d4*cos(-x3+x1-x2)+1/2*a3*sin(x3+x1+x2)+1/2*a3*sin(-x3+x1-x2)+1/2*a2*sin(x1+x2)+1/2*a2*sin(x1-x2)+cos(x1)*d2;
                        Z(i) = -cos(x2+x3)*d4-a3*sin(x2+x3)-a2*sin(x2)+d1;
                        X(i) = 432*c1*c2 - 149*s1 + 433*c1*c2*s3 + 433*c1*c3*s2; 
                        Y(i) = 149*c1 + 432*c2*s1 + 433*c2*s1*s3 + 433*c3*s1*s2;
                        Z(i) = 433*c23 - 432*s2;
                        i = i+1;
                    end
                end
            end
 %       end
    %end

plot3(X,Y,Z)