In [2]:

    

from miscpy.utils.sympyhelpers import *
init_printing()


    

In [3]:

    

l,th,thd,thdd,mp,mq,t,TP,TQ,FPQ = symbols('l,theta,thetadot,thetaddot,m_P,m_Q,t,T_P,T_Q,F_{PQ}')
diffmap = {th:thd,thd:thdd}
iWb = Matrix([0,0,thd])
bCi = rotMat(3,(th-pi/2))


    

In [4]:

    

r_PO = Matrix([sqrt(3)*l/2,-l/2,0])
v_PO = transportEq(r_PO,t,diffmap,iWb)
r_QO = Matrix([sqrt(3)*l/2,l/2,0])
v_QO = transportEq(r_QO,t,diffmap,iWb)


    

In [5]:

    

h_O = mp*r_PO.cross(v_PO) + mq*r_QO.cross(v_QO)
mat2vec(h_O,polarframe)


    

    
Out[5]:





$$\left(l^{2} m_{P} \dot{\theta} + l^{2} m_{Q} \dot{\theta}\right) \mathbf{e_z}$$

In [6]:

    

M_PO = r_PO.cross(Matrix([-sqrt(3)/2*TP,TP/2,0])+ Matrix([0,FPQ,0])+ bCi*Matrix([0,-mp*g,0]))
M_QO = r_QO.cross(Matrix([-sqrt(3)/2*TQ,-TQ/2,0])+ Matrix([0,-FPQ,0])+ bCi*Matrix([0,-mq*g,0]))
M_PO,M_QO


    

    
Out[6]:





$$\left ( \left[\begin{matrix}0\\0\\\frac{l}{2} \left(- \frac{\sqrt{3} T_{P}}{2} + g m_{P} \cos{\left (\theta \right )}\right) + \frac{\sqrt{3} l}{2} \left(F_{PQ} + \frac{T_{P}}{2} - g m_{P} \sin{\left (\theta \right )}\right)\end{matrix}\right], \quad \left[\begin{matrix}0\\0\\- \frac{l}{2} \left(- \frac{\sqrt{3} T_{Q}}{2} + g m_{Q} \cos{\left (\theta \right )}\right) + \frac{\sqrt{3} l}{2} \left(- F_{PQ} - \frac{T_{Q}}{2} - g m_{Q} \sin{\left (\theta \right )}\right)\end{matrix}\right]\right )$$

In [7]:

    

eom = difftotalmat(h_O,t,diffmap) - (M_PO+M_QO)


    

In [8]:

    

solve(eom,thdd)


    

    
Out[8]:





$$\left \{ \ddot{\theta} : \frac{g}{l \left(m_{P} + m_{Q}\right)} \left(m_{P} \cos{\left (\theta + \frac{\pi}{3} \right )} - m_{Q} \sin{\left (\theta + \frac{\pi}{6} \right )}\right)\right \}$$

In [11]:

    

simplify(solve(eom.subs(mq,mp),thdd))


    

    
Out[11]:





$$\left \{ \ddot{\theta} : - \frac{\sqrt{3} g}{2 l} \sin{\left (\theta \right )}\right \}$$

In [ ]: