from Serial import *
R = Serial("RxRxRxPx", '', Matrix([['x','x','x','0'],['y','y','y','x']]).T)
n_mot = 4
mot = [Motor("mot"+str(i+1),str(R.dof+i+1)) for i in range(n_mot)]
R.StaticBal = solve(R.gh_, R.lg)
R.Mh_sb_ = simplify(R.Mh_.subs(R.StaticBal))
R.vh_sb_ = simplify(R.vh_.subs(R.StaticBal))
R.gh_sb_ = simplify(R.gh_.subs(R.StaticBal))
ph_ = R.ph_
po_ = Matrix([mot[i].p_ for i in range(n_mot)])
p_ = Matrix([ph_,po_])
M_ = R.Mh_sb_
for i in range(n_mot):
M_ = diag(M_, mot[i].M_)
v_ = Matrix([R.vh_sb_] + [mot[i].v_ for i in range(n_mot)])
g_ = Matrix([R.gh_sb_] + [mot[i].g_ for i in range(n_mot)])
phi_ = po_ - Matrix([R.vw_[0][0]+ph_[1],0,0, R.vw_[0][0]+symbols('beta')*ph_[1],0,0, R.vw_[1][0]+ph_[2],0,0, R.vw_[1][0]+symbols('gamma')*ph_[2],0,0]).subs(R.StaticBal)
Ah_ = phi_.jacobian(ph_)
Ao_ = phi_.jacobian(po_)
C_ = Matrix([eye(R.dof),simplify(-Ao_**-1 * Ah_)])
Mh_ = simplify( (C_.T * M_ * C_) )
vh_ = simplify( (C_.T * ( M_ * C_.diff(t)*ph_ + v_ ) ))
gh_ = simplify(C_.T *g_)
