sp, st, sf, cp, ct, cf, py, s2, s1, c2, c1, pz, px, l2, l1, 
-cf*cp*ct+c1*c2+sf*sp
-cp*ct*sf+c2*s1-cf*sp
cp*st+s2
-cf*ct*sp-c1*s2+cp*sf
ct*sf*sp-cf*cp-s1*s2
-sp*st+c2
-cf*st+s1
-sf*st-c1
ct
c1*c2*l2-px
c2*l2*s1-py
l2*s2+l1-pz
c1^2+s1^2-1
c2^2+s2^2-1
cf^2+sf^2-1
ct^2+st^2-1
cp^2+sp^2-1
