gendgminverse.m

DGM model generate for serial robot

function TjTi=GENDGMinverse(sigma,alpha,d,theta,r,q u0)
uj=TjT6=TjTi*ui;%uj=jT6=jTi*ui;
TjTi=[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]';
u0=[sx nx ax px;
    sy ny ay py;
    sa na az pz;
    0  0  0  1];
TjTi=GENDGMinverse(sigma,alpha,d,theta,r,q u0)