pumax.m

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

function pumax()
syms x1 x2 x3 x4 x5 x6 d1 d2 d3 d4 d5 d6 a1 a2 a3 a4 a5 a6
                        nA1 = TMatrix(x1,-pi/2,0,d1);
                        nA2 = TMatrix(x2,0,a2,d2);
                        nA3 = TMatrix(x3,-pi/2,a3,0);
                        nA4 = TMatrix(x4,pi/2,0,d4);
                        nA5 = TMatrix(x5,-pi/2,0,0);
                        nA6 = TMatrix(x6,0,0,0);
                        T = nA1*nA2*nA3*nA4*nA5*nA6;
                        PX=simple(T(1,4))
                        PY=simple(T(2,4))
                        PZ=simple(T(3,4))