In [4]:

    

from sympy import *
init_printing()


    

In [16]:

    

th1, th2, th3 = symbols('theta_1, theta_2, theta_3')
dcm_321 = Matrix([[cos(th1)*cos(th2),
         cos(th1)*sin(th2)*sin(th3) - sin(th1)*cos(th3),
         cos(th1)*sin(th2)*cos(th3) + sin(th1)*sin(th3)],
        [sin(th1)*cos(th2),
         sin(th1)*sin(th2)*sin(th3) + cos(th1)*cos(th3),
         sin(th1)*sin(th2)*cos(th3) - cos(th1)*sin(th3)],
        [-sin(th2),
        cos(th2)*sin(th3),
        cos(th2)*cos(th3)]
    ])
dcm_321


    

    
Out[16]:





$$\left[\begin{matrix}\cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} & - \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{3} \right )} + \sin{\left (\theta_{2} \right )} \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{1} \right )} & \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{3} \right )} + \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{3} \right )}\\\sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} & \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} \sin{\left (\theta_{3} \right )} + \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{3} \right )} & \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{3} \right )} - \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{1} \right )}\\- \sin{\left (\theta_{2} \right )} & \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{2} \right )} & \cos{\left (\theta_{2} \right )} \cos{\left (\theta_{3} \right )}\end{matrix}\right]$$

In [18]:

    

phi, theta, psi = symbols('phi, theta, psi')
sub_eulers = {
    th1: psi, th2: theta, th3: phi
}
dcm_321_euler = dcm_321.subs(sub_eulers)
dcm_321_euler


    

    
Out[18]:





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

In [38]:

    

dcm_pitch_up = dcm_321_euler.subs({theta:pi/2, phi:0})
dcm_pitch_up


    

    
Out[38]:





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

In [39]:

    

atan2(dcm_pitch_up[1,2], dcm_pitch_up[0,2])


    

    
Out[39]:





$$\operatorname{atan_{2}}{\left (\sin{\left (\psi \right )},\cos{\left (\psi \right )} \right )}$$

In [40]:

    

dcm_pitch_down = dcm_321_euler.subs({theta:pi/2, phi:0})
dcm_pitch_down


    

    
Out[40]:





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

In [41]:

    

dcm_pitch_down = dcm_321_euler.subs({theta:-pi/2, phi:0})
atan2(-dcm_pitch_down[1,2], -dcm_pitch_down[0,2])


    

    
Out[41]:





$$\operatorname{atan_{2}}{\left (\sin{\left (\psi \right )},\cos{\left (\psi \right )} \right )}$$