In [1]:

from miscpy.utils.sympyhelpers import *
init_printing()
from sympy.utilities.codegen import codegen



## Set up rotation matrices representing a 3-1-3 $(\psi,\theta,\phi)$ Euler angle set.



In [2]:

aCi = rotMat(3,psi)
cCa = rotMat(1,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}1 & 0 & 0\\0 & \cos{\left (\theta \right )} & \sin{\left (\theta \right )}\\0 & - \sin{\left (\theta \right )} & \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-1-3 rotation




Out[3]:

$$\left[\begin{matrix}- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \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 (\theta \right )}\\- \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} & - \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} & \sin{\left (\theta \right )} \cos{\left (\phi \right )}\\\sin{\left (\psi \right )} \sin{\left (\theta \right )} & - \sin{\left (\theta \right )} \cos{\left (\psi \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 )} - \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 (\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 (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \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 )} \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 (\theta \right )} + \dot{\theta} \cos{\left (\phi \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{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )} & - \dot{\theta} \sin{\left (\theta \right )}\end{matrix}\right]$$



## $\tilde{\omega} = {}^\mathcal{B}C^{\mathcal{I}} {}^\mathcal{B}{\dot{C}}^{\mathcal{I}}$



In [5]:

omega_tilde = bCi*bCi_dot.T; omega_tilde




Out[5]:

$$\left[\begin{matrix}\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) & \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) & - \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(- \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 )} & \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 )} & - \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 (\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 )} & \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 )} & - \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]$$



## $\left[{}^\mathcal{I}\boldsymbol{\omega}^{\mathcal{B}}\right]_\mathcal{B} = \left[ {}^\mathcal{B}C^{\mathcal{I}}_{32} \quad {}^\mathcal{B}C^{\mathcal{I}}_{13} \quad {}^\mathcal{B}C^{\mathcal{I}}_{21} \right]$



In [6]:

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




Out[6]:

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




In [7]:

w1,w2,w3 = symbols('omega_1,omega_2,omega_3')




In [8]:

s0 = solve(omega - Matrix([w1,w2,w3]),[psid,thd,phd]); s0




Out[8]:

$$\left \{ \dot{\phi} : - \frac{\omega_{1} \sin{\left (\phi \right )}}{\tan{\left (\theta \right )}} - \frac{\omega_{2} \cos{\left (\phi \right )}}{\tan{\left (\theta \right )}} + \omega_{3}, \quad \dot{\psi} : \frac{\omega_{1} \sin{\left (\phi \right )} + \omega_{2} \cos{\left (\phi \right )}}{\sin{\left (\theta \right )}}, \quad \dot{\theta} : \omega_{1} \cos{\left (\phi \right )} - \omega_{2} \sin{\left (\phi \right )}\right \}$$



## Find EOM (second derivatives of Euler Angles)



In [24]:

I1,I2,I3 = symbols("I_1,I_2,I_3",real=True,positive=True)
iWb_B = omega
I_G_B = diag(I1,I2,I3)
I_G_B




Out[24]:

$$\left[\begin{matrix}I_{1} & 0 & 0\\0 & I_{2} & 0\\0 & 0 & I_{3}\end{matrix}\right]$$




In [10]:

diffmap = {th:thd,psi:psid,ph:phd,thd:thdd,psid:psidd,phd:phdd}
diffmap




Out[10]:

$$\left \{ \phi : \dot{\phi}, \quad \dot{\phi} : \ddot{\phi}, \quad \psi : \dot{\psi}, \quad \dot{\psi} : \ddot{\psi}, \quad \theta : \dot{\theta}, \quad \dot{\theta} : \ddot{\theta}\right \}$$




In [11]:

t1 = I_G_B*difftotalmat(iWb_B,t,diffmap)
t2 = skew(iWb_B)*I_G_B*iWb_B
t1,t2




Out[11]:

$$\left ( \left[\begin{matrix}I_{1} \left(\dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\phi} \dot{\theta} \sin{\left (\phi \right )} + \ddot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\psi} \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )} + \ddot{\theta} \cos{\left (\phi \right )}\right)\\I_{2} \left(- \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\phi} \dot{\theta} \cos{\left (\phi \right )} + \ddot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\psi} \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \ddot{\theta} \sin{\left (\phi \right )}\right)\\I_{3} \left(\ddot{\phi} + \ddot{\psi} \cos{\left (\theta \right )} - \dot{\psi} \dot{\theta} \sin{\left (\theta \right )}\right)\end{matrix}\right], \quad \left[\begin{matrix}I_{2} \left(- \dot{\phi} - \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right) + I_{3} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right)\\I_{1} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) + I_{3} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(- \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\phi \right )}\right)\\I_{1} \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) \left(- \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )}\right) + I_{2} \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right)\end{matrix}\right]\right )$$




In [12]:

dh_G_B = t1+t2
dh_G_B




Out[12]:

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




In [13]:

t3 = expand(dh_G_B[0]*cos(ph)*I2 - dh_G_B[1]*sin(ph)*I1)




In [14]:

sol_thdd = simplify(solve(t3,thdd))
sol_thdd




Out[14]:

$$\left [ \frac{I_{1}^{2} \dot{\phi} \dot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} + \frac{I_{1}^{2} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} + I_{1}^{2} \dot{\psi}^{2} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\theta \right )} + \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4} - I_{1} I_{2} \dot{\phi} \dot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} - I_{1} I_{2} \dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} - I_{1} I_{3} \dot{\phi} \dot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} - \frac{I_{1} I_{3} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} - I_{1} I_{3} \dot{\psi}^{2} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\theta \right )} - \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4} + I_{2}^{2} \dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} - \frac{I_{2}^{2} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} + I_{2}^{2} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4} - I_{2} I_{3} \dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} + \frac{I_{2} I_{3} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} - I_{2} I_{3} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} + \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4}}{I_{1} I_{2}}\right ]$$




In [15]:

t4= expand(dh_G_B[0]*sin(ph)*I2 + dh_G_B[1]*cos(ph)*I1)
t4




Out[15]:

$$I_{1}^{2} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + I_{1}^{2} \dot{\phi} \dot{\theta} \cos^{2}{\left (\phi \right )} + I_{1}^{2} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} + I_{1}^{2} \dot{\psi} \dot{\theta} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - I_{1} I_{2} \dot{\phi} \dot{\theta} \sin^{2}{\left (\phi \right )} - I_{1} I_{2} \dot{\phi} \dot{\theta} \cos^{2}{\left (\phi \right )} + I_{1} I_{2} \ddot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} + I_{1} I_{2} \ddot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} + I_{1} I_{2} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \cos{\left (\theta \right )} + I_{1} I_{2} \dot{\psi} \dot{\theta} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - I_{1} I_{3} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - I_{1} I_{3} \dot{\phi} \dot{\theta} \cos^{2}{\left (\phi \right )} - I_{1} I_{3} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - I_{1} I_{3} \dot{\psi} \dot{\theta} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - I_{2}^{2} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + I_{2}^{2} \dot{\phi} \dot{\theta} \sin^{2}{\left (\phi \right )} - I_{2}^{2} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} + I_{2}^{2} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \cos{\left (\theta \right )} + I_{2} I_{3} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - I_{2} I_{3} \dot{\phi} \dot{\theta} \sin^{2}{\left (\phi \right )} + I_{2} I_{3} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - I_{2} I_{3} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \cos{\left (\theta \right )}$$




In [16]:

sol_psidd = simplify(solve(t4,psidd))
sol_psidd




Out[16]:

$$\left [ \frac{- \frac{I_{1}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{1}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} - \frac{I_{1}^{2} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} - \frac{I_{1}^{2} \dot{\phi} \dot{\theta}}{2} - \frac{I_{1}^{2} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} + \frac{I_{1}^{2} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} - \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} - \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4} + I_{1} I_{2} \dot{\phi} \dot{\theta} - I_{1} I_{2} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )} + \frac{I_{1} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{1} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} + \frac{I_{1} I_{3} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} + \frac{I_{1} I_{3} \dot{\phi} \dot{\theta}}{2} + \frac{I_{1} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} - \frac{I_{1} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} + \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} + \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4} + \frac{I_{2}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{2}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} + \frac{I_{2}^{2} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} - \frac{I_{2}^{2} \dot{\phi} \dot{\theta}}{2} + \frac{I_{2}^{2} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} - \frac{I_{2}^{2} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} - \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} + \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4} - \frac{I_{2} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{2} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} - \frac{I_{2} I_{3} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} + \frac{I_{2} I_{3} \dot{\phi} \dot{\theta}}{2} - \frac{I_{2} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} + \frac{I_{2} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} + \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} - \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4}}{I_{1} I_{2} \sin{\left (\theta \right )}}\right ]$$




In [17]:

sol_phdd = solve(dh_G_B[2],phdd)
sol_phdd




Out[17]:

$$\left [ \frac{I_{1} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin^{2}{\left (\theta \right )} \cos{\left (\phi \right )} - 2 I_{1} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} + I_{1} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )} - I_{1} \dot{\theta}^{2} \sin{\left (\phi \right )} \cos{\left (\phi \right )} - I_{2} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin^{2}{\left (\theta \right )} \cos{\left (\phi \right )} + 2 I_{2} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} - I_{2} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )} + I_{2} \dot{\theta}^{2} \sin{\left (\phi \right )} \cos{\left (\phi \right )} - I_{3} \ddot{\psi} \cos{\left (\theta \right )} + I_{3} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )}}{I_{3}}\right ]$$




In [ ]:



## Find initial orientation such that $\mathbf h$ is down-pointing



In [25]:

h = sqrt(((I_G_B*Matrix([w1,w2,w3])).transpose()*(I_G_B*Matrix([w1,w2,w3])))[0]);h




Out[25]:

$$\sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}$$




In [28]:

eqs1 = simplify(bCi.transpose()*I_G_B*Matrix([w1,w2,w3]) - Matrix([0,0,-h])); eqs1 #equal 0




Out[28]:

$$\left[\begin{matrix}- I_{1} \omega_{1} \left(\sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) - I_{2} \omega_{2} \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) + I_{3} \omega_{3} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\\I_{1} \omega_{1} \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) - I_{2} \omega_{2} \left(\sin{\left (\phi \right )} \sin{\left (\psi \right )} - \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) - I_{3} \omega_{3} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\\I_{1} \omega_{1} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + I_{2} \omega_{2} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + I_{3} \omega_{3} \cos{\left (\theta \right )} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}\end{matrix}\right]$$




In [34]:

simplify(solve(simplify(eqs1[0]*cos(psi) + eqs1[1]*sin(psi)),ph)) #phi solution




Out[34]:

$$\left [ - 2 \operatorname{atan}{\left (\frac{I_{2} \omega_{2} - \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}}{I_{1} \omega_{1}} \right )}, \quad - 2 \operatorname{atan}{\left (\frac{I_{2} \omega_{2} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}}{I_{1} \omega_{1}} \right )}\right ]$$




In [38]:

solve(simplify(expand(simplify(-eqs1[0]*sin(psi) + eqs1[1]*cos(psi)).subs(ph,atan(I1*w1/I2/w2)))),th) #th solution




Out[38]:

$$\left [ - 2 \operatorname{atan}{\left (\frac{I_{3} \omega_{3} - \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}}{\omega_{2} \sqrt{\frac{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}{\omega_{2}^{2}}}} \right )}, \quad - 2 \operatorname{atan}{\left (\frac{I_{3} \omega_{3} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}}{\omega_{2} \sqrt{\frac{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}{\omega_{2}^{2}}}} \right )}\right ]$$




In [36]:

simplify(eqs1[2].subs(ph,atan(I1*w1/I2/w2)))




Out[36]:

$$I_{3} \omega_{3} \cos{\left (\theta \right )} + \omega_{2} \sqrt{\frac{I_{1}^{2} \omega_{1}^{2}}{\omega_{2}^{2}} + I_{2}^{2}} \sin{\left (\theta \right )} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}$$




In [ ]:




In [ ]:



### Generate MATLAB Code



In [18]:

out = codegen(("eom1",sol_psidd[0]), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3]);out




Out[18]:

[('eom1.m',




In [19]:

codegen(("eom1",sol_thdd[0]), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3])




Out[19]:

[('eom1.m',




In [20]:

codegen(("eom1",sol_phdd[0]), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])




Out[20]:

[('eom1.m',
"function out1 = eom1(theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = (I_1.*psidot.^2.*sin(phi).*sin(theta).^2.*cos(phi) - 2*I_1.*psidot.*thetadot.*sin(phi).^2.*sin(theta) + I_1.*psidot.*thetadot.*sin(theta) - I_1.*thetadot.^2.*sin(phi).*cos(phi) - I_2.*psidot.^2.*sin(phi).*sin(theta).^2.*cos(phi) + 2*I_2.*psidot.*thetadot.*sin(phi).^2.*sin(theta) - I_2.*psidot.*thetadot.*sin(theta) + I_2.*thetadot.^2.*sin(phi).*cos(phi) - I_3.*psiddot.*cos(theta) + I_3.*psidot.*thetadot.*sin(theta))./I_3;\n\nend\n")]




In [21]:

codegen(("eom1",[s0[psid],s0[thd],s0[phd]]), 'Octave', argument_sequence=[w1,w2,w3,th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])




Out[21]:

[('eom1.m',
"function [out1, out2, out3] = eom1(omega_1, omega_2, omega_3, theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = (omega_1.*sin(phi) + omega_2.*cos(phi))./sin(theta);\n  out2 = omega_1.*cos(phi) - omega_2.*sin(phi);\n  out3 = -omega_1.*sin(phi)./tan(theta) - omega_2.*cos(phi)./tan(theta) + omega_3;\n\nend\n")]




In [22]:

codegen(("eom1",bCi), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])




Out[22]:

[('eom1.m',
"function out1 = eom1(theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = [-sin(phi).*sin(psi).*cos(theta) + cos(phi).*cos(psi) sin(phi).*cos(psi).*cos(theta) + sin(psi).*cos(phi) sin(phi).*sin(theta); -sin(phi).*cos(psi) - sin(psi).*cos(phi).*cos(theta) -sin(phi).*sin(psi) + cos(phi).*cos(psi).*cos(theta) sin(theta).*cos(phi); sin(psi).*sin(theta) -sin(theta).*cos(psi) cos(theta)];\n\nend\n")]




In [23]:

codegen(("eom1",omega), 'Octave', argument_sequence=[w1,w2,w3,th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])




Out[23]:

[('eom1.m',
"function out1 = eom1(omega_1, omega_2, omega_3, theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = [psidot.*sin(phi).*sin(theta) + thetadot.*cos(phi); psidot.*sin(theta).*cos(phi) - thetadot.*sin(phi); phidot + psidot.*cos(theta)];\n\nend\n")]




In [ ]:




In [ ]:




In [ ]:




In [ ]:




In [ ]:




In [ ]: