lab2rcs

syms q1 q2 q3 q4 l1 l2 l3 l4

R01=TransfoMat('r','z',q1)
R12=TransfoMat('t','z',l1)*TransfoMat('t','x',-l2)*TransfoMat('r','x',q2)
R23=TransfoMat('t','z',l3)*TransfoMat('r','x',q3)
R34=TransfoMat('t','y',q4+l4)*TransfoMat('t','x',l2)

R04=R01*R12*R23*R34;

M=eval(R04)

q1=0;
q2=-pi/20;
q3=pi/20;
q4=10;
l1=5;
l2=2;
l3=5;
l4=5;

M=subs(M)

R01=subs(R01)
R12=subs(R12)
R23=subs(R23)
R34=subs(R34)

D={TransfoMat('r','z',q1) TransfoMat('t','z',l1) TransfoMat('t','x',l2) TransfoMat('r','x',q2) TransfoMat('t','z',l3) TransfoMat('t','x',-l2) TransfoMat('r','x',q3) TransfoMat('t','y',q4+l4)}