In [1]:

    

from miscpy.utils.sympyhelpers import *
init_printing()


    

In [2]:

    

aCi = rotMat(3,psi)
cCa = rotMat(2,th)
bCc = rotMat(3,ph)
aCi,cCa,bCc


    

    
Out[2]:





$$\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], \quad \left[\begin{matrix}\cos{\left (\phi \right )} & \sin{\left (\phi \right )} & 0\\- \sin{\left (\phi \right )} & \cos{\left (\phi \right )} & 0\\0 & 0 & 1\end{matrix}\right]\right )$$

In [3]:

    

bCi = bCc*cCa*aCi; bCi #3-2-3 rotation


    

    
Out[3]:





$$\left[\begin{matrix}- \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} & \sin{\left (\phi \right )} \cos{\left (\psi \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} & - \sin{\left (\theta \right )} \cos{\left (\phi \right )}\\- \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )} & - \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} & \sin{\left (\phi \right )} \sin{\left (\theta \right )}\\\sin{\left (\theta \right )} \cos{\left (\psi \right )} & \sin{\left (\psi \right )} \sin{\left (\theta \right )} & \cos{\left (\theta \right )}\end{matrix}\right]$$

In [4]:

    

bCi_dot = difftotalmat(bCi,t,{th:thd,psi:psid,ph:phd});
bCi_dot


    

    
Out[4]:





$$\left[\begin{matrix}- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )} & - \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} & \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\\\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )} & - \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )} & \dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\\- \dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )} & \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )} & - \dot{\theta} \sin{\left (\theta \right )}\end{matrix}\right]$$

In [5]:

    

omega_tilde = bCi*bCi_dot.T; omega_tilde


    

    
Out[5]:





$$\left[\begin{matrix}\left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) + \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )}\right) - \left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\theta \right )} \cos{\left (\phi \right )} & \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\right) + \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\right) - \left(\dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\theta \right )} \cos{\left (\phi \right )} & \dot{\theta} \sin^{2}{\left (\theta \right )} \cos{\left (\phi \right )} + \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(- \dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) + \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )}\right)\\\left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )}\right) + \left(- \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) & \left(\dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\right) + \left(- \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) \left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\right) & - \dot{\theta} \sin{\left (\phi \right )} \sin^{2}{\left (\theta \right )} + \left(- \dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(- \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) + \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right)\\\left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \cos{\left (\theta \right )} + \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )}\right) \sin{\left (\psi \right )} \sin{\left (\theta \right )} + \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) \sin{\left (\theta \right )} \cos{\left (\psi \right )} & \left(\dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\right) \cos{\left (\theta \right )} + \left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\right) \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\right) \sin{\left (\psi \right )} \sin{\left (\theta \right )} & - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\theta \right )} + \left(- \dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\psi \right )} \sin{\left (\theta \right )}\end{matrix}\right]$$

In [6]:

    

simplify(Matrix([omega_tilde[2,1],omega_tilde[0,2],omega_tilde[1,0]]))


    

    
Out[6]:





$$\left[\begin{matrix}- \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )}\\\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\\\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\end{matrix}\right]$$

In [ ]: