syms O1 O2 O3 O4 O5 O6
syms alpha1 alpha2 alpha3 alpha4 alpha5 alpha6 
syms d1 d2 d3 d4 d5 d6
syms a1 a2 a3 a4 a5 a6
syms r11 r12 r13 r21 r22 r23 r31 r32 r33
syms px py pz

T0_1=Tcal(alpha1,a1,d1,O1)
T1_2=Tcal(alpha2,a2,d2,O2)
T2_3=Tcal(alpha3,a3,d3,O3)
T3_4=Tcal(alpha4,a4,d4,O4)
T4_5=Tcal(alpha5,a5,d5,O5)
T5_6=Tcal(alpha6,a6,d6,O6)

T0_6 = T0_1*T1_2*T2_3*T3_4*T4_5*T5_6

function Tout = Tcal(alpha,a,d,O)
        Tout=[cos(O),-sin(O),0,a;
               sin(O)*cos(alpha),cos(O)*cos(alpha),-sin(alpha),-sin(alpha)*d;
               sin(O)*sin(alpha),cos(O)*sin(alpha),cos(alpha),cos(alpha)*d;
               0,0,0,1];
end