{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "v1, v2, v3 = me.dynamicsymbols('v1:4')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{v_{1}}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "v1(t)"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle t$"
      ],
      "text/plain": [
       "t"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "me.dynamicsymbols._t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{v_{1}}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Derivative(v1(t), t)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v1.diff(me.dynamicsymbols._t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "N = me.ReferenceFrame('N')\n",
    "A = me.ReferenceFrame('A')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = sm.symbols('t')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle x{\\left(t \\right)}$"
      ],
      "text/plain": [
       "x(t)"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.Function('x')(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = me.dynamicsymbols._t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "v = v1*A.x + v2*A.y + v3*A.z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\u001b[0;31mSignature:\u001b[0m \u001b[0mv\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdt\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0motherframe\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
       "\u001b[0;31mDocstring:\u001b[0m\n",
       "Returns a Vector which is the time derivative of\n",
       "the self Vector, taken in frame otherframe.\n",
       "\n",
       "Calls the global time_derivative method\n",
       "\n",
       "Parameters\n",
       "==========\n",
       "\n",
       "otherframe : ReferenceFrame\n",
       "    The frame to calculate the time derivative in\n",
       "\u001b[0;31mFile:\u001b[0m      /opt/conda/lib/python3.6/site-packages/sympy/physics/vector/vector.py\n",
       "\u001b[0;31mType:\u001b[0m      method\n"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "v.dt?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{v}_{1}\\mathbf{\\hat{a}_x} + \\dot{v}_{2}\\mathbf{\\hat{a}_y} + \\dot{v}_{3}\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "v1'*A.x + v2'*A.y + v3'*A.z"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v.dt(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "a1, a2, a3 = me.dynamicsymbols('a1:4')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle a_{1}\\mathbf{\\hat{a}_x} + a_{2}\\mathbf{\\hat{a}_y} + a_{3}\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "a1*A.x + a2*A.y + a3*A.z"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "N_omega_A = a1*A.x + a2*A.y + a3*A.z\n",
    "N_omega_A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\u001b[0;31mSignature:\u001b[0m \u001b[0mA\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mset_ang_vel\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0motherframe\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mvalue\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
       "\u001b[0;31mDocstring:\u001b[0m\n",
       "Define the angular velocity vector in a ReferenceFrame.\n",
       "\n",
       "Defines the angular velocity of this ReferenceFrame, in another.\n",
       "Angular velocity can be defined with respect to multiple different\n",
       "ReferenceFrames. Care must be taken to not create loops which are\n",
       "inconsistent.\n",
       "\n",
       "Parameters\n",
       "==========\n",
       "\n",
       "otherframe : ReferenceFrame\n",
       "    A ReferenceFrame to define the angular velocity in\n",
       "value : Vector\n",
       "    The Vector representing angular velocity\n",
       "\n",
       "Examples\n",
       "========\n",
       "\n",
       ">>> from sympy.physics.vector import ReferenceFrame, Vector\n",
       ">>> N = ReferenceFrame('N')\n",
       ">>> A = ReferenceFrame('A')\n",
       ">>> V = 10 * N.x\n",
       ">>> A.set_ang_vel(N, V)\n",
       ">>> A.ang_vel_in(N)\n",
       "10*N.x\n",
       "\u001b[0;31mFile:\u001b[0m      /opt/conda/lib/python3.6/site-packages/sympy/physics/vector/frame.py\n",
       "\u001b[0;31mType:\u001b[0m      method\n"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "A.set_ang_vel?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "A.set_ang_vel(N, N_omega_A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle a_{1}\\mathbf{\\hat{a}_x} + a_{2}\\mathbf{\\hat{a}_y} + a_{3}\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "a1*A.x + a2*A.y + a3*A.z"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.ang_vel_in(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle (a_{2} v_{3} - a_{3} v_{2} + \\dot{v}_{1})\\mathbf{\\hat{a}_x} + (- a_{1} v_{3} + a_{3} v_{1} + \\dot{v}_{2})\\mathbf{\\hat{a}_y} + (a_{1} v_{2} - a_{2} v_{1} + \\dot{v}_{3})\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "(a2*v3 - a3*v2 + v1')*A.x + (-a1*v3 + a3*v1 + v2')*A.y + (a1*v2 - a2*v1 + v3')*A.z"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v.dt(A) + me.cross(N_omega_A, v)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle (a_{2} v_{3} - a_{3} v_{2} + \\dot{v}_{1})\\mathbf{\\hat{a}_x} + (- a_{1} v_{3} + a_{3} v_{1} + \\dot{v}_{2})\\mathbf{\\hat{a}_y} + (a_{1} v_{2} - a_{2} v_{1} + \\dot{v}_{3})\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "(a2*v3 - a3*v2 + v1')*A.x + (-a1*v3 + a3*v1 + v2')*A.y + (a1*v2 - a2*v1 + v3')*A.z"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v.dt(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "N = me.ReferenceFrame('N')\n",
    "B = me.ReferenceFrame('B')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\omega_{1}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "omega1(t)"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "w1, w2, w3 = me.dynamicsymbols('omega1:4')\n",
    "w1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "I1, I2, I3 = sm.symbols('I1:4')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "w = w1*B.x + w2*B.y + w3*B.z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "Hc = I1*w1*B.x + I2*w2*B.y + I3*w3*B.z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\omega_{1}\\mathbf{\\hat{b}_x} + \\omega_{2}\\mathbf{\\hat{b}_y} + \\omega_{3}\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "omega1*B.x + omega2*B.y + omega3*B.z"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle I_{1} \\omega_{1}\\mathbf{\\hat{b}_x} + I_{2} \\omega_{2}\\mathbf{\\hat{b}_y} + I_{3} \\omega_{3}\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "I1*omega1*B.x + I2*omega2*B.y + I3*omega3*B.z"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Hc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "B.set_ang_vel(N, w)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "m1, m2, m3 = me.dynamicsymbols('m1:4')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "M = m1*B.x + m2*B.y + m3*B.z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle (I_{1} \\dot{\\omega}_{1} - I_{2} \\omega_{2} \\omega_{3} + I_{3} \\omega_{2} \\omega_{3})\\mathbf{\\hat{b}_x} + (I_{1} \\omega_{1} \\omega_{3} + I_{2} \\dot{\\omega}_{2} - I_{3} \\omega_{1} \\omega_{3})\\mathbf{\\hat{b}_y} + (- I_{1} \\omega_{1} \\omega_{2} + I_{2} \\omega_{1} \\omega_{2} + I_{3} \\dot{\\omega}_{3})\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "(I1*omega1' - I2*omega2*omega3 + I3*omega2*omega3)*B.x + (I1*omega1*omega3 + I2*omega2' - I3*omega1*omega3)*B.y + (-I1*omega1*omega2 + I2*omega1*omega2 + I3*omega3')*B.z"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Hc.dt(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle (I_{1} \\dot{\\omega}_{1} - I_{2} \\omega_{2} \\omega_{3} + I_{3} \\omega_{2} \\omega_{3})\\mathbf{\\hat{b}_x} + (I_{1} \\omega_{1} \\omega_{3} + I_{2} \\dot{\\omega}_{2} - I_{3} \\omega_{1} \\omega_{3})\\mathbf{\\hat{b}_y} + (- I_{1} \\omega_{1} \\omega_{2} + I_{2} \\omega_{1} \\omega_{2} + I_{3} \\dot{\\omega}_{3})\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "(I1*omega1' - I2*omega2*omega3 + I3*omega2*omega3)*B.x + (I1*omega1*omega3 + I2*omega2' - I3*omega1*omega3)*B.y + (-I1*omega1*omega2 + I2*omega1*omega2 + I3*omega3')*B.z"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Hc.dt(B) + me.cross(w, Hc)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\operatorname{m_{1}}{\\left(t \\right)}\\\\\\operatorname{m_{2}}{\\left(t \\right)}\\\\\\operatorname{m_{3}}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[m1(t)],\n",
       "[m2(t)],\n",
       "[m3(t)]])"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "M.to_matrix(B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}I_{1} \\frac{d}{d t} \\omega_{1}{\\left(t \\right)} - I_{2} \\omega_{2}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)} + I_{3} \\omega_{2}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)}\\\\I_{1} \\omega_{1}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)} + I_{2} \\frac{d}{d t} \\omega_{2}{\\left(t \\right)} - I_{3} \\omega_{1}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)}\\\\- I_{1} \\omega_{1}{\\left(t \\right)} \\omega_{2}{\\left(t \\right)} + I_{2} \\omega_{1}{\\left(t \\right)} \\omega_{2}{\\left(t \\right)} + I_{3} \\frac{d}{d t} \\omega_{3}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ I1*Derivative(omega1(t), t) - I2*omega2(t)*omega3(t) + I3*omega2(t)*omega3(t)],\n",
       "[ I1*omega1(t)*omega3(t) + I2*Derivative(omega2(t), t) - I3*omega1(t)*omega3(t)],\n",
       "[-I1*omega1(t)*omega2(t) + I2*omega1(t)*omega2(t) + I3*Derivative(omega3(t), t)]])"
      ]
     },
     "execution_count": 33,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Hc.dt(N).to_matrix(B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\operatorname{m_{1}}{\\left(t \\right)}\\\\\\operatorname{m_{2}}{\\left(t \\right)}\\\\\\operatorname{m_{3}}{\\left(t \\right)}\\end{matrix}\\right] = \\left[\\begin{matrix}I_{1} \\frac{d}{d t} \\omega_{1}{\\left(t \\right)} - I_{2} \\omega_{2}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)} + I_{3} \\omega_{2}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)}\\\\I_{1} \\omega_{1}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)} + I_{2} \\frac{d}{d t} \\omega_{2}{\\left(t \\right)} - I_{3} \\omega_{1}{\\left(t \\right)} \\omega_{3}{\\left(t \\right)}\\\\- I_{1} \\omega_{1}{\\left(t \\right)} \\omega_{2}{\\left(t \\right)} + I_{2} \\omega_{1}{\\left(t \\right)} \\omega_{2}{\\left(t \\right)} + I_{3} \\frac{d}{d t} \\omega_{3}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Eq(Matrix([\n",
       "[m1(t)],\n",
       "[m2(t)],\n",
       "[m3(t)]]), Matrix([\n",
       "[ I1*Derivative(omega1(t), t) - I2*omega2(t)*omega3(t) + I3*omega2(t)*omega3(t)],\n",
       "[ I1*omega1(t)*omega3(t) + I2*Derivative(omega2(t), t) - I3*omega1(t)*omega3(t)],\n",
       "[-I1*omega1(t)*omega2(t) + I2*omega1(t)*omega2(t) + I3*Derivative(omega3(t), t)]]))"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.Eq(M.to_matrix(B), Hc.dt(N).to_matrix(B))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [],
   "source": [
    "me.init_vprinting()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}m_{1}\\\\m_{2}\\\\m_{3}\\end{matrix}\\right] = \\left[\\begin{matrix}I_{1} \\dot{\\omega}_{1} - I_{2} \\omega_{2} \\omega_{3} + I_{3} \\omega_{2} \\omega_{3}\\\\I_{1} \\omega_{1} \\omega_{3} + I_{2} \\dot{\\omega}_{2} - I_{3} \\omega_{1} \\omega_{3}\\\\- I_{1} \\omega_{1} \\omega_{2} + I_{2} \\omega_{1} \\omega_{2} + I_{3} \\dot{\\omega}_{3}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡m₁⎤   ⎡I₁⋅ω₁̇ - I₂⋅ω₂⋅ω₃ + I₃⋅ω₂⋅ω₃ ⎤\n",
       "⎢  ⎥   ⎢                             ⎥\n",
       "⎢m₂⎥ = ⎢I₁⋅ω₁⋅ω₃ + I₂⋅ω₂̇ - I₃⋅ω₁⋅ω₃ ⎥\n",
       "⎢  ⎥   ⎢                             ⎥\n",
       "⎣m₃⎦   ⎣-I₁⋅ω₁⋅ω₂ + I₂⋅ω₁⋅ω₂ + I₃⋅ω₃̇⎦"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.Eq(M.to_matrix(B), Hc.dt(N).to_matrix(B))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}