with(linalg):
readlib(eqn):
TT:= proc (a, al, d, th) array(1..4,1..4,
                    [[cos(th),         -sin(th)       , 0,        a         ],
                     [sin(th)*cos(al), cos(th)*cos(al), -sin(al), -sin(al)*d],
                     [sin(th)*sin(al), cos(th)*sin(al), cos(al),  cos(al)*d ],
                     [0,               0,               0,        1         ]
                    ]) end:

al:=array(0..5,[0,0,-Pi/2,Pi/2,Pi/2,Pi/2]);
a:=array(0..5,[0,l1,l2,l3,0,0]);
d:=array(1..6,[0,0,0,0,0,0]);
th:=array(1..6,[th1,th2,th3,th4,th5,th6]);

T0_1:=TT(a[0],al[0],d[1],th[1]);
T1_2:=TT(a[1],al[1],d[2],th[2]);
T2_3:=TT(a[2],al[2],d[3],th[3]);
T3_4:=TT(a[3],al[3],d[4],th[4]);
T4_5:=TT(a[4],al[4],d[5],th[5]);
T5_6:=TT(a[5],al[5],d[6],th[6]);


T0_6:=multiply(T0_1,T1_2,T2_3,T3_4,T4_5,T5_6);
T4_6:=multiply(T3_4,T4_5,T5_6);

rvec:=array(1..3,[T0_6[i,4]$i=1..3]):
rr:=simplify(rvec[1]^2+rvec[2]^2+rvec[3]^2);
zz:=factor(T0_6[3,4]);

