syms th1 th2 th3 L1 L2 L3
alpha0=0;a0=0;d1=L1+L2;
alpha1=sym(pi)/2;a1=0;d2=0;
alpha2=0;a2=L3;d3=0;


t01=[cos(th1) -sin(th1) 0 a0;sin(th1)*cos(alpha0) cos(th1)*cos(alpha0) -sin(alpha0) -sin(alpha0)*d1;...
    sin(th1)*sin(alpha0) cos(th1)*sin(alpha0) cos(alpha0) cos(alpha0)*d1;0 0 0 1]

t12=[cos(th2) -sin(th2) 0 a1;sin(th2)*cos(alpha1) cos(th2)*cos(alpha1) -sin(alpha1) -sin(alpha1)*d2;...
    sin(th2)*sin(alpha1) cos(th2)*sin(alpha1) cos(alpha1) cos(alpha1)*d2;0 0 0 1]

t23=[cos(th3) -sin(th3) 0 a2;sin(th3)*cos(alpha2) cos(th3)*cos(alpha2) -sin(alpha2) -sin(alpha2)*d3;...
    sin(th3)*sin(alpha2) cos(th3)*sin(alpha2) cos(alpha2) cos(alpha2)*d3;;0 0 0 1]

t02 = t01*t12

t02*[5 0 0 1]'
