def _gen_rbt_Si( rbt ):
var('a theta alpha d',domain=RR)
D_exp2trig = { e**(SR.wild(0)) : e**SR.wild(0).real_part() * ( cos(SR.wild(0).imag_part()) + I*sin(SR.wild(0).imag_part()) ) }
M = matrix(SR,[[1,0,0,a],[0,cos(alpha),-sin(alpha),0],[0,sin(alpha),cos(alpha),d],[0,0,0,1]])
P = matrix([[0,-1,0,0],[1,0,0,0],[0,0,0,0],[0,0,0,0]])
ePtM = (exp(P*theta)*M).expand().subs(D_exp2trig).simplify_rational()
restore('a theta alpha d')
if bool( ePtM != rbt.T_dh ):
raise Exception('_gen_rbt_Si: interlink transformation does not follows implemented DH formulation')
S = ( inverse_T(M) * P * M ).expand().simplify_trig()
dof = rbt.dof
Si = range(dof+1)
for l in range(dof): Si[l+1] = se3unskew( S.subs(LoP_to_D(zip(rbt.params_dh , rbt.links_dh[l]))) )
return Si
