tmatrix.m

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

function result = TMatrix(theta,alpha,A,D)	
sa = sin(alpha);
ca = cos(alpha);

if abs(sa) < eps
    sa = 0;
end

if abs(ca) < eps
    ca = 0;
end

M_11=cos(theta);				M_21=sin(theta);				M_31=0.0;			M_41=0.0;
M_12=-sin(theta)*ca;            M_22=cos(theta)*ca;             M_32=sa;            M_42=0.0;
M_13=sin(theta)*sa;            M_23=-cos(theta)*sa;             M_33=ca;            M_43=0.0;
M_14=A*cos(theta);              M_24=A*sin(theta);              M_34=D;             M_44=1.0;

result = [M_11 M_12 M_13 M_14;
          M_21 M_22 M_23 M_24;
          M_31 M_32 M_33 M_34;
          M_41 M_42 M_43 M_44;];