Preamble Stuff (ignore)



In [1]:

    
from sympyhelpers import *
init_printing()

Define all the symbols you'll need



In [2]:

    
th,psi,thd,psid,R,g,t,l,thdd,phdd,psidd,N1,N2,N3,mr,tau,J  = \
symbols('theta,psi,thetadot,psidot,R,g,t,l,thetaddot,phiddot,psiddot,N_1,N_2,N_3,m_r,tau,J')
diffdict = {th:thd,thd:thdd,psi:psid,psid:psidd}

Define rotation matrices and angular velocities for $\psi$ rotation about $\mathbf{e}_3$ and $-\theta$ rotation about $\mathbf{b}_2$



In [3]:

    
bCi = rotMat(3,psi) # I->B
cCb = rotMat(2,-th) # B->C
bCi,cCb









    Out[3]:





$$\left ( \left[\begin{matrix}\cos{\left (\psi \right )} & \sin{\left (\psi \right )} & 0\\- \sin{\left (\psi \right )} & \cos{\left (\psi \right )} & 0\\0 & 0 & 1\end{matrix}\right], \quad \left[\begin{matrix}\cos{\left (\theta \right )} & 0 & \sin{\left (\theta \right )}\\0 & 1 & 0\\- \sin{\left (\theta \right )} & 0 & \cos{\left (\theta \right )}\end{matrix}\right]\right )$$



In [4]:

    
iWb = Matrix([0,0,psid]) #B frame
bWc = Matrix([0,-thd,0]) #C frame

Apply Euler's second law to the disk, assuming planar moment of inertia J about $\mathbf{b}_3$ - all in $\mathcal{B}$ frame



In [5]:

    
h_O = J*iWb; 
M_O = Matrix([0,R,0]).cross(Matrix([-N1,-N2,N3]));
h_O,M_O









    Out[5]:





$$\left ( \left[\begin{matrix}0\\0\\J \dot{\psi}\end{matrix}\right], \quad \left[\begin{matrix}N_{3} R\\0\\N_{1} R\end{matrix}\right]\right )$$



In [6]:

    
eom1 = solve(transportEq(h_O,t,diffdict,iWb)-M_O,psidd); eom1









    Out[6]:





$$\left \{ \ddot{\psi} : \frac{N_{1} R}{J}\right \}$$

Calculate rod kinematics (of center of mass) - all in the $\mathcal{B}$ frame



In [7]:

    
r_GO = Matrix([0,R,0]) + cCb.T*Matrix([0,0,l/2]); r_GO









    Out[7]:





$$\left[\begin{matrix}- \frac{l}{2} \sin{\left (\theta \right )}\\R\\\frac{l}{2} \cos{\left (\theta \right )}\end{matrix}\right]$$



In [8]:

    
v_GO = transportEq(r_GO,t,diffdict,iWb)
v_GO









    Out[8]:





$$\left[\begin{matrix}- R \dot{\psi} - \frac{l \dot{\theta}}{2} \cos{\left (\theta \right )}\\- \frac{l \dot{\psi}}{2} \sin{\left (\theta \right )}\\- \frac{l \dot{\theta}}{2} \sin{\left (\theta \right )}\end{matrix}\right]$$



In [9]:

    
a_GO = transportEq(v_GO,t,diffdict,iWb);
simplify(a_GO)









    Out[9]:





$$\left[\begin{matrix}- R \ddot{\psi} + \frac{l \dot{\psi}^{2}}{2} \sin{\left (\theta \right )} - \frac{l \ddot{\theta}}{2} \cos{\left (\theta \right )} + \frac{l \dot{\theta}^{2}}{2} \sin{\left (\theta \right )}\\- R \dot{\psi}^{2} - \frac{l \ddot{\psi}}{2} \sin{\left (\theta \right )} - l \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}\\- \frac{l}{2} \left(\ddot{\theta} \sin{\left (\theta \right )} + \dot{\theta}^{2} \cos{\left (\theta \right )}\right)\end{matrix}\right]$$

Solve for normal forces by applying Euler's first law to rod COM



In [10]:

    
Ns = solve(mr*a_GO - Matrix([N1,N2,mr*g-N3]),(N1,N2,N3));Ns









    Out[10]:





$$\left \{ N_{1} : \frac{m_{r}}{2} \left(- 2 R \ddot{\psi} + l \dot{\psi}^{2} \sin{\left (\theta \right )} - l \ddot{\theta} \cos{\left (\theta \right )} + l \dot{\theta}^{2} \sin{\left (\theta \right )}\right), \quad N_{2} : - \frac{m_{r}}{2} \left(2 R \dot{\psi}^{2} + l \ddot{\psi} \sin{\left (\theta \right )} + 2 l \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}\right), \quad N_{3} : \frac{m_{r}}{2} \left(2 g + l \ddot{\theta} \sin{\left (\theta \right )} + l \dot{\theta}^{2} \cos{\left (\theta \right )}\right)\right \}$$

Parallel axis theorem to get MOI matrix for the rod about $Q$



In [11]:

    
r_QG = Matrix([0,0,-l/2])
I_Q = mr*l**2/12*(eye(3) - diag(0,0,1)) + mr*((r_QG.transpose()*r_QG)[0]*eye(3) - r_QG*r_QG.transpose())
I_Q









    Out[11]:





$$\left[\begin{matrix}\frac{l^{2} m_{r}}{3} & 0 & 0\\0 & \frac{l^{2} m_{r}}{3} & 0\\0 & 0 & 0\end{matrix}\right]$$

Calculate angular velocity and momentum about $Q$ in $\mathcal C$ frame components



In [12]:

    
iWc = bWc + cCb*iWb
h_Q = I_Q*iWc
iWc,h_Q









    Out[12]:





$$\left ( \left[\begin{matrix}\dot{\psi} \sin{\left (\theta \right )}\\- \dot{\theta}\\\dot{\psi} \cos{\left (\theta \right )}\end{matrix}\right], \quad \left[\begin{matrix}\frac{m_{r} \dot{\psi}}{3} l^{2} \sin{\left (\theta \right )}\\- \frac{m_{r} \dot{\theta}}{3} l^{2}\\0\end{matrix}\right]\right )$$

Now apply Euler's second law for rod about $Q$ (all in $\mathcal C$ frame)



In [13]:

    
dh_Q = transportEq(h_Q,t,diffdict,iWc)
M_Q = Matrix([tau,mr*g*l/2*sin(th),0])
r_QO = cCb*Matrix([0,R,0])
a_QO = simplify(transportEq(transportEq(r_QO,t,diffdict,iWc),t,diffdict,iWc))
dh_Q,M_Q,a_QO









    Out[13]:





$$\left ( \left[\begin{matrix}\frac{m_{r} \ddot{\psi}}{3} l^{2} \sin{\left (\theta \right )} + \frac{2 m_{r}}{3} l^{2} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}\\\frac{l^{2} m_{r}}{3} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} - \frac{m_{r} \ddot{\theta}}{3} l^{2}\\0\end{matrix}\right], \quad \left[\begin{matrix}\tau\\\frac{g l}{2} m_{r} \sin{\left (\theta \right )}\\0\end{matrix}\right], \quad \left[\begin{matrix}- R \ddot{\psi} \cos{\left (\theta \right )}\\- R \dot{\psi}^{2}\\R \ddot{\psi} \sin{\left (\theta \right )}\end{matrix}\right]\right )$$



In [14]:

    
r_QG = Matrix([0,0,-l/2])
eom2 = solve(dh_Q - (M_Q + mr*r_QG.cross(a_QO)),thdd)
eom2









    Out[14]:





$$\left \{ \ddot{\theta} : \frac{1}{2 l} \left(- 3 R \ddot{\psi} \cos{\left (\theta \right )} - 3 g \sin{\left (\theta \right )} + l \dot{\psi}^{2} \sin{\left (2 \theta \right )}\right)\right \}$$

Finally, combine the solutions to the disk and rod motions for the full equations of motion



In [15]:

    
simplify(solve([thdd - eom2[thdd],psidd - eom1[psidd].subs(N1,Ns[N1])],(psidd,thdd)))









    Out[15]:





$$\left \{ \ddot{\psi} : \frac{R m_{r} \left(3 g \cos{\left (\theta \right )} + 2 l \dot{\psi}^{2} \sin^{2}{\left (\theta \right )} + 2 l \dot{\theta}^{2}\right) \sin{\left (\theta \right )}}{4 J + 3 R^{2} m_{r} \sin^{2}{\left (\theta \right )} + R^{2} m_{r}}, \quad \ddot{\theta} : - \frac{1}{l \left(4 J + 3 R^{2} m_{r} \sin^{2}{\left (\theta \right )} + R^{2} m_{r}\right)} \left(\frac{3 l}{2} R^{2} m_{r} \left(\dot{\psi}^{2} + \dot{\theta}^{2}\right) \sin{\left (2 \theta \right )} + 2 \left(J + R^{2} m_{r}\right) \left(3 g - 2 l \dot{\psi}^{2} \cos{\left (\theta \right )}\right) \sin{\left (\theta \right )}\right)\right \}$$