symgendgm.m

来自「DGM model generate for serial robot」· M 代码 · 共 19 行

function T0Tn=SYMGENDGM(sigma,alpha,d,theta,r,q)
n=length(q);
T0Tn=eye(4);
for j=1:n
    if sigma(j)==0,
        theta(j)=q(j);
    else
        r(j)=q(j);
    end
    
T=[cos(theta(j)) -sin(theta(j)) 0 d(j);
    cos(alpha(j))*sin(theta(j)) cos(alpha(j))*cos(theta(j)) -sin(alpha(j)) -r(j)*sin(alpha(j));
    sin(alpha(j))*sin(theta(j)) sin(alpha(j))*cos(theta(j)) cos(alpha(j)) r(j)*cos(alpha(j));
    0 0 0 1];
m=inv(T)
T0Tn=T0Tn*T
end