3D Coordinates and Kinematics Derivations

Preamble stuff (can ignore)



In [1]:

    
from miscpy.utils.sympyhelpers import *
init_printing()



In [2]:

    
th,ph,psi,thd,phd,psid = symbols('theta,phi,psi,thetadot,phidot,psidot')
w1,w2,w3 = symbols('omega_1,omega_2,omega_3')

Spherical Frame - $\theta$ rotation about $\mathbf{e}_3$ followed by a $\phi$ rotation about $\mathbf{c}_2$



In [3]:

    
cCi = Matrix(([cos(th),sin(th),0],[-sin(th),cos(th),0],[0,0,1]))
sCc = Matrix(([cos(ph),0,-sin(ph)],[0,1,0],[sin(ph), 0, cos(ph)]));cCi,sCc









    Out[3]:





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

${}^\mathcal{S}C^{\mathcal{I}} ={}^\mathcal{S}C^{\mathcal{C}}{}^\mathcal{C}C^{\mathcal{I}} $



In [4]:

    
sCi = sCc*cCi; sCi









    Out[4]:





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

3-2-3 $(\psi,\theta,\phi)^{\mathcal{I}}_{\mathcal{B}}$ Euler Angles - $\psi$ rotation about $\mathbf{e}_1$ followed by a $\theta$ rotation about $\mathbf{a}_2$ followed by a $\phi$ rotation about $\mathbf{c}_3$



In [5]:

    
aCi = Matrix(([cos(psi),sin(psi),0],[-sin(psi),cos(psi),0],[0,0,1]))
cCa = Matrix(([cos(th),0,-sin(th)],[0,1,0],[sin(th), 0, cos(th)]))
bCc = Matrix(([cos(ph),sin(ph),0],[-sin(ph),cos(ph),0],[0,0,1])); bCc, cCa, aCi









    Out[5]:





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

${}^\mathcal{B}C^{\mathcal{I}} ={}^\mathcal{B}C^{\mathcal{C}}{}^\mathcal{C}C^{\mathcal{A}}{}^\mathcal{A}C^{\mathcal{I}} $



In [6]:

    
bCi = bCc*cCa*aCi; bCi









    Out[6]:





$$\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]$$

3-2-3 $(\psi,\theta,\phi)^{\mathcal{I}}_{\mathcal{B}}$ Euler Angles - Angular velocity ${}^{\mathcal{I}}\boldsymbol{\omega}^\mathcal{B} = \dot\psi \mathbf{a}_3 + \dot\theta \mathbf{c}_2 + \dot\phi \mathbf{b}_3$

Expressed in terms of only $\mathcal{B}$ frame unit vectors:



In [7]:

    
w = psid*bCi*Matrix([0,0,1]) + thd*bCc*Matrix([0,1,0]) +  phd*Matrix([0,0,1]); w









    Out[7]:





$$\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]$$

Defining ${}^{\mathcal{I}}\boldsymbol{\omega}^\mathcal{B} = \omega_1 \mathbf{b}_1 + \omega_2 \mathbf{b}_2 + \omega_3 \mathbf{b}_3$ And solving for $\dot\psi,\dot\theta,\dot\phi$ in terms of $\omega_1,\omega_2\omega_3$:



In [8]:

    
solve(w - Matrix([w1,w2,w3]),([thd,psid,phd]))









    Out[8]:





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