symgendgm11.m

DGM model generate for serial robot

function T0Tn=SYMGENDGM11(sigma,alpha,d,theta,r,q)
n=length(q);
T0Tn=eye(4);
w=[];
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];
w = [w,T];
m=inv(T);
T0Tn=T0Tn*T;
end
t3t4 = w(:,13:16);
R3R4=t3t4(1:3,1:3)
R4R3=inv(R3R4)