{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "c7202593-1a48-40f7-b85f-b2bc628e2d39",
   "metadata": {},
   "source": [
    "# Deriving the Equations of Motion of a Simple Spherical Pendulum\n",
    "\n",
    "We model the spherical simple pendulum as a point mass ($m_P$) attached by a massless, rigid link of length $\\ell$ to an inertially fixed point $O$.  The mass is free to move in 3D subject to the constraint of the link length.\n",
    "\n",
    "<img src=\"spherical_pendulum.png\" width=\"600\"> \n",
    "\n",
    "We define an inertial frame $\\mathcal{I} = (\\mathbf{\\hat{e}}_1,\\mathbf{\\hat{e}}_2,\\mathbf{\\hat{e}}_3)$ such that $\\mathbf{\\hat{e}}_3$ points opposite the direction of gravity, and a spherical reference frame $\\mathcal{S} = (\\mathbf{\\hat{e}}_\\phi,\\mathbf{\\hat{e}}_\\theta,\\mathbf{\\hat{e}}_\\rho)$ such that $\\mathbf{\\hat{e}}_\\rho$ points from $O$ to $P$ (e.g. $\\mathbf{\\hat{e}}_\\rho \\equiv \\mathbf{\\hat{r}}_{P/O}$). The system's generalized coordinates are $\\theta$ (the azimuth angle) and $\\phi$ (the zenith angle). The only forces acting on the pendulum mass are gravity and the link constraint force, which we will assume has magnitude $F_C$."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a96a9d81-8f3e-4e0b-9cb2-9d106140bccf",
   "metadata": {},
   "source": [
    "## Define system coordiantes and variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "e70ee92e-47a2-4c6a-969b-c60fc988eb56",
   "metadata": {},
   "outputs": [],
   "source": [
    "# load sympy and symphlepers and init printing\n",
    "from sympyhelpers.sympyhelpers import *\n",
    "sympy.init_printing()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "cfa1fbe7-42bc-45cb-8d70-21daaacc36f6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# generate coordinate variables and diffmap\n",
    "allsyms, diffmap = gendiffvars([('th','theta'), ('ph', 'phi')])\n",
    "locals().update(allsyms)\n",
    "# define strictly positive quantities (link length, mass, and gravitational acceleration)\n",
    "l,m,g = symbols(\"\\\\ell, m_P, g\", positive=True)\n",
    "# define constraint force as strictly real quantity\n",
    "Fc = symbols(\"F_C\", real=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a7a4f84d-0720-4d8e-8cff-612d2a9e9c6e",
   "metadata": {},
   "source": [
    "## Define Direction Cosine Matrices and Angular Velocities\n",
    "Anytime we define a non-inertial frame, we should provide ourselves with DCMs and angular velocities so that we can do any required frame component conversions and differentiations. In this case, the spherical frame is related to the inertial frame via a rotation about $\\mathbf{\\hat{e}}_3$ by angle $\\theta$ followed by a rotation about $\\mathbf{\\hat{e}}_\\theta$ by angle $\\phi$.  Both rotations are positive (counter-clockwise) so the total DCM is given by:\n",
    "$${{\\vphantom{C}}^{\\mathcal{S}}\\!{C}^{\\mathcal{I}}} = C_2(\\phi)C_3(\\theta)$$\n",
    "Similarly, we can find the net angular velocity of the $\\mathcal{S}$ frame in the $\\mathcal{I}$ frame by summing the two intermediate angular velocities: \n",
    "$$ {{\\vphantom{\\boldsymbol{\\omega}}}^{\\mathcal{I}}\\!{\\boldsymbol{\\omega}}^{\\mathcal{S}}} = {{\\vphantom{\\boldsymbol{\\omega}}}^{\\mathcal{I}}\\!{\\boldsymbol{\\omega}}^{\\mathcal{C}}} + {{\\vphantom{\\boldsymbol{\\omega}}}^{\\mathcal{C}}\\!{\\boldsymbol{\\omega}}^{\\mathcal{S}}} = \\dot\\theta \\mathbf{\\hat{e}}_3 + \\dot\\phi\\mathbf{\\hat{e}}_\\theta$$\n",
    "where $\\mathcal{C}$ is the intermediate Cylindrical frame resulting from the first ($\\theta$) rotation. In order to put the final expression entirely in $\\mathcal{S}$ frame components, we also need to transform the first angular velocity:\n",
    "$$[{{\\vphantom{\\boldsymbol{\\omega}}}^{\\mathcal{I}}\\!{\\boldsymbol{\\omega}}^{\\mathcal{S}}}]_\\mathcal{S} = {{\\vphantom{C}}^{\\mathcal{S}}\\!{C}^{\\mathcal{I}}}[{{\\vphantom{\\boldsymbol{\\omega}}}^{\\mathcal{I}}\\!{\\boldsymbol{\\omega}}^{\\mathcal{C}}} ]_\\mathcal{I} + [{{\\vphantom{\\boldsymbol{\\omega}}}^{\\mathcal{C}}\\!{\\boldsymbol{\\omega}}^{\\mathcal{S}}} ]_\\mathcal{S}$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "e8a9ed4a-6aa8-428a-8e92-96cffab28649",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left( \\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], \\  \\dot{\\phi} \\hat{\\mathbf{e}}_\\theta - \\dot{\\theta} \\sin{\\left(\\phi \\right)} \\hat{\\mathbf{e}}_\\phi + \\dot{\\theta} \\cos{\\left(\\phi \\right)} \\hat{\\mathbf{e}}_\\rho\\right)$"
      ],
      "text/plain": [
       "⎛⎡cos(φ)⋅cos(θ)  sin(θ)⋅cos(φ)  -sin(φ)⎤                                                                               \n",
       "⎜⎢                                     ⎥                                                                               \n",
       "⎜⎢   -sin(θ)        cos(θ)         0   ⎥, φ̇⋅\\hat{\\mathbf{e}}_\\theta - θ̇⋅sin(φ)⋅\\hat{\\mathbf{e}}_\\phi + θ̇⋅cos(φ)⋅\\hat\n",
       "⎜⎢                                     ⎥                                                                               \n",
       "⎝⎣sin(φ)⋅cos(θ)  sin(φ)⋅sin(θ)  cos(φ) ⎦                                                                               \n",
       "\n",
       "              ⎞\n",
       "              ⎟\n",
       "{\\mathbf{e}}_\\rho⎟\n",
       "              ⎟\n",
       "              ⎠"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# define spherical frame DCM and angular velocity\n",
    "# all vectors will be in S frame components\n",
    "sCi = rotMat(2,ph)*rotMat(3,th)\n",
    "IwS_S = (sCi*Matrix([0,0,thd])) + Matrix([0,phd,0])\n",
    "sCi,mat2vec(IwS_S,sphericalframe)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "782b1c60-79ce-43e7-8bf2-b2f283217769",
   "metadata": {},
   "source": [
    "## Kinematics\n",
    "We define the position vector $\\mathbf{r}_{P/O} \\triangleq \\ell \\mathbf{\\hat{e}}_\\rho$ and differentiate with respect to the inertial frame to find the inertial velocity (in $\\mathcal{S}$ frame components."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "7ad0bfc9-e92c-4d4e-afe8-7d8c726c835e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAABwAAAAWCAYAAADTlvzyAAAACXBIWXMAAA7EAAAOxAGVKw4bAAACM0lEQVRIDa2U7VEiQRCGV8rfFhFwawJ+JKBrAuoloFwEagIuksCpCYARHJqAaAJ3RQJuXQJSJoDPi9NbvcOqQNFVL/3dPdPTy9pkMkkWpU6ns0FOCh8tmrtW1zAUPKfYGPnWFw2+e2xNcLpo04YvJtkVvEYt0LcsxvnOsO2DG++3uK/4TEOCr8Ajhd7gKSiA0THCEb6R/OAAPTPnXFwjNeR53gITsCEb/NJ8q+LxDXW7QbjdXAdeNGjdEmiizdPI9D4J+gmsLzkmfC1sqezIz+LzUrmlocEdiT/AGLSxxRu6h10xoh7YBDpkHsdiqyU/0gMiChL/w9ugD0rCfojyFAzb6F3wC/030LbK/y35hhnRw5Aori31dBOU+I0HwX7lgz+TyzckILUgmo1MFkfXt5hKhproGm1MO7GhTp82pICWQPQX+eFDrPw2nZYiZ06XmEf6p6rd0E5n44kTCmfQn0LX6V+KxGr7Vf8f8luDH43LaGyC58RokYbBtut9JhNzabJxbCfI7aDfo7e0NGMEjXEI7KYJti3gN+8UfwGOsVfeEF2bWjlIyE3ht+AZ/yO48N+hbqo1/wMyoBFU3hNd4zkHP4HIJjIzZmJf8O/DNZ0E3lN82TAYtTwpTp1oaSJfdZ7gm1YEWQc4s6WZ2jHqNNMTWeCSPCPPbp9QV285hD9Ubrhk8Zk0Ck/HFxyv4ti64pUbyrAiyqhTvp+v2fDKKmRuovfTjWqfZuUN6aVPa6CmdfQOQMkjvlTm7hcAAAAASUVORK5CYII=",
      "text/latex": [
       "$\\displaystyle \\ell \\hat{\\mathbf{e}}_\\rho$"
      ],
      "text/plain": [
       "\\ell⋅\\hat{\\mathbf{e}}_\\rho"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# position vector\n",
    "r_PO_S = Matrix([0,0,l])\n",
    "mat2vec(r_PO_S,sphericalframe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "fd82ef2c-14bf-48d7-83ca-e1e46cd8a95c",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\ell \\dot{\\phi} \\hat{\\mathbf{e}}_\\phi + \\ell \\dot{\\theta} \\sin{\\left(\\phi \\right)} \\hat{\\mathbf{e}}_\\theta$"
      ],
      "text/plain": [
       "\\ell⋅φ̇⋅\\hat{\\mathbf{e}}_\\phi + \\ell⋅θ̇⋅sin(φ)⋅\\hat{\\mathbf{e}}_\\theta"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# velocity vector\n",
    "v_PO_S = transportEq(r_PO_S, t, diffmap, IwS_S)\n",
    "mat2vec(v_PO_S,sphericalframe)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dc55eb02-bec8-4d9c-91f6-97709b98dd7b",
   "metadata": {},
   "source": [
    "## Forces\n",
    "As per our free body diagram, the only forces acting on the mass are gravity ($-m_Pg\\mathbf{\\hat{e}}_3$) and the link constraint force ($-F_C \\mathbf{\\hat{e}}_\\rho$). Again, we will express everything in $\\mathcal{S}$ frame components."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "97c82ff4-a264-4237-ac60-86ec4a0132cc",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle g m_{P} \\sin{\\left(\\phi \\right)} \\hat{\\mathbf{e}}_\\phi - g m_{P} \\cos{\\left(\\phi \\right)} \\hat{\\mathbf{e}}_\\rho$"
      ],
      "text/plain": [
       "g⋅m_P⋅sin(φ)⋅\\hat{\\mathbf{e}}_\\phi - g⋅m_P⋅cos(φ)⋅\\hat{\\mathbf{e}}_\\rho"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# force due to gravity\n",
    "F_g_S = sCi*Matrix([0,0,-m*g])\n",
    "mat2vec(F_g_S,sphericalframe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "196eb027-27ab-4862-b7aa-8b84ee5b9970",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAD0AAAAWCAYAAABzCZQcAAAACXBIWXMAAA7EAAAOxAGVKw4bAAADVUlEQVRYCdWX+1HbQBDGhYe/GSogcgPBaQDkBhKnAR4NhNAArwYypgEMDcS4AQQNhKEBNG4gHhpwfp+505zOljlZSpjszKd93t7u3elkr02n0+h/pbOzsw1qj+FPVXpYa7JpJv/I5JuBBWTEjwNj58IYq4ZvgebbRw9uvDWXrZ6hw/AE3AMVJNnHMTb5D8BK5DR8RIJd0MemBQ+i9aCowCAmvgFbhJ+DIfLFoqHYr7Cni3yBth5xX8jzYuK7yN+Qg3a70aZNAYnhd4YvYs8YJ4scITYavPHjsF36tjK9VeaoYe+asemyHBQZtCvLcqzq+1s7ndHU2BaFrCPfgY+MbWh9q3KTM9Z45IcqeRpt2ilk4BVxgJ7biFt5lxm7Q65rINLd0MamS/MUHnTEm/5k7TG5CkqBfafbyAkFiZcSfp2GGEyQZ4sC3wAvdhDyZ2Q1mIFt64OfoJ8DXW4j+FJqdKeZqWtm+wEPuqhMI4fE/wIpiLHpdh6APvgKLEkX6cuQL4Z0oKaFt5tmsFb4GlQh/RjI31lnYIKc4csnRpY708MnfDqeEdxtTKYRNi3CoxQRur7DsWRoE33nVSw8OwWtRFln8BiclvgXmjXGd2Czx3Pg+SboqWeLiLcNH/o+o/+EZ45v05Fj5MTRJQb3MDveFFDp9vMms2pihDtrECf3k6sb2x68Bz74Pk9PHT1z5DvyXjj6UpHYDQJ0Ch6RX1pLo6s5uyY8DRh2Toz/XhaGqSkwtkYjp0b/ZO0uJ+bE1SVj0wIfSIZu0beabDohaUbSvFDN4hN+rXoMCifCjyvR97FnoEeeHTcGXZdnYTGw6baP4ZfgAVlzHte6vUm0RZJroPctBhE224z721guS/bdzKzB5yZvDFehOaHr/tnG8B3oT4Z8Ez0gHfnjVzF/9pF2c41vOvKkbtPa1a6T9E3RFK5CY1BoyhncQx44ei4yXp+qC4Pc7gvEaUMizef4EuSjlmP4l+IRk/m7MpufInUkU7iaq0MJg+0piMind1t5R43+IqtSoSnikDFXIAOxwQCfuzuYqxM5lNc2/VsZsOmERO/WtCYXUYi9kGafk1dr/Sd5n8myC59bwHdvun578xloVO/zPbw9742i93qnF9XSpK1DsmFZwj/5jDdc8jS97QAAAABJRU5ErkJggg==",
      "text/latex": [
       "$\\displaystyle - F_{C} \\hat{\\mathbf{e}}_\\rho$"
      ],
      "text/plain": [
       "-F_C⋅\\hat{\\mathbf{e}}_\\rho"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# force due to link constraint\n",
    "F_C_S = Matrix([0,0,-Fc])\n",
    "mat2vec(F_C_S,sphericalframe)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "88d7910c-a4c3-4841-8a2b-3d76c5d3e848",
   "metadata": {},
   "source": [
    "## Angular Momentum Balance\n",
    "We will first find the equations of motion via the angular momentum form of Newton's second law.  This is the most direct approach to getting to the equations of motion using Newtonian formalism, but does not allow us to solve for the link force magnitude.\n",
    "We find the angular momentum of $P$ about $O$:\n",
    "$${\\vphantom{\\mathbf h_{P/O}}}^{\\mathcal{I}}\\!{\\mathbf h_{P/O}} = \\mathbf r_{P/O} \\times m_P {}^{\\mathcal{I}}\\!{\\mathbf v_{P/O}}$$\n",
    "differentiate it with respect to the inertial reference frame, and set the solution equal to the net moment on $P$ about $O$:\n",
    "$$\\mathbf{M}_{P/O} = \\mathbf r_{P/O} \\times \\mathbf{F}_P$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "81e58576-b6b8-4af2-92e7-b82a3dd89d4b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\ell^{2} m_{P} \\dot{\\phi} \\hat{\\mathbf{e}}_\\theta - \\ell^{2} m_{P} \\dot{\\theta} \\sin{\\left(\\phi \\right)} \\hat{\\mathbf{e}}_\\phi$"
      ],
      "text/plain": [
       "    2                                     2                                   \n",
       "\\ell ⋅m_P⋅φ̇⋅\\hat{\\mathbf{e}}_\\theta - \\ell ⋅m_P⋅θ̇⋅sin(φ)⋅\\hat{\\mathbf{e}}_\\phi"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# angular momentum of P about O\n",
    "h_PO_S = m*r_PO_S.cross(v_PO_S)\n",
    "mat2vec(h_PO_S, sphericalframe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "e7c6ac4d-1e5d-46df-b881-8d1c85bf6e8d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\ell^{2} m_{P} \\left(\\ddot{\\phi} - \\frac{\\dot{\\theta}^{2} \\sin{\\left(2 \\phi \\right)}}{2}\\right) \\hat{\\mathbf{e}}_\\theta - \\ell^{2} m_{P} \\left(2 \\dot{\\phi} \\dot{\\theta} \\cos{\\left(\\phi \\right)} + \\ddot{\\theta} \\sin{\\left(\\phi \\right)}\\right) \\hat{\\mathbf{e}}_\\phi$"
      ],
      "text/plain": [
       "          ⎛     2         ⎞                                                                                    \n",
       "    2     ⎜    θ̇ ⋅sin(2⋅φ)⎟                               2                                                    \n",
       "\\ell ⋅m_P⋅⎜φ̈ - ───────────⎟⋅\\hat{\\mathbf{e}}_\\theta - \\ell ⋅m_P⋅(2⋅φ̇⋅θ̇⋅cos(φ) + θ̈⋅sin(φ))⋅\\hat{\\mathbf{e}}_\\phi\n",
       "          ⎝         2     ⎠                                                                                    "
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# inertial derivative of angular momentum\n",
    "dh_PO_S = simplify(transportEq(h_PO_S, t, diffmap, IwS_S))\n",
    "mat2vec(dh_PO_S, sphericalframe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "a7eb5e84-8757-48b6-9c57-017ffb2628f1",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAIIAAAAVCAYAAACKYDYGAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAHmUlEQVRoBe2a+1HdVhCHb5j8TXADRG7Avm6AiAZs3ACGBozdABgawHYDPBowoQFkGoDQABoaMKEB5/vEOfKRrnQfnssk8Xhnfnd3f7tnz1uSH4OvX78OfuLh12BnZ+f5964zbZ/M0pb8xVnbLAw65N27d4tgG7zuCP+kZlwB1nGfJuU0zch90pE3hF/v4Eco8hYh/wRHPbVG2kiMHISk0Hvi5SzFOnv4j5PMbx18eah5UnebJThDX025FGvtPNoewz1Dr7RjqU88HoIt+D/AB7iug5U2q+yRgwC7Cxz4HToDJfiR5RmTWwLZvCcZNmEVfTqH2jvUcGPd7D7xEL0g5wrcgVX8vC855RsHgYbLBN+A9yFpyYJpgx/NZn5vmdNT9Dw2q708XipfC1NJWP+yKznsQ0FsoysuR85xyKtT8D/WzhijcRDIc+An7WJj2v8QIeZ7Ne+JUNNLlaNnOWA5bQrQJwcEPLhzl19jRQbsI8dHi++XAf466lC7S4g70Qx93o7DrcBdou+Moa09BCX2jZyCXdXArHOrwJx/6Mfx5OAWnIAhXL1BIV6i67GRM0h57DjWEruRZ26HuJZFB99JUdN3uY/yS9BZnxwf+UvmancVgo/jHGCfd+V0cb/4xxKFRuuoI/A7cME24D6iGwLnpvrk+AIKkAPzHeAeeI3tZPxyfQqGIANynuaLJM92JfgAduDrzcGfi1DTW/TJ2sCxvwG+t4VzdkyO0f730IPAO8cM7ADHmY51n5xjuF4h/olgNdfeJALkbaMegzOwGvQmeovYyGbDmed8GnuDvwJ/BBTnbM014LwauXAjspAwDqKk0Q16AxyCLvkM+YU8N/0c+z1wIx/jL6NvA7+E7cBK/I+B28ffxfawHKJ9p1nDRXND5irUfkLBDF0dMLQfUHtwpR1h2/8z7VQC70IqL8FJ4ByrB+MI37mOk4xg2ZdAe/+IfmEOehNdAD/Sj9EvwJ/YXX2UxOLYMKt5PEd9rpz77x33xpqutx+YxsdK/WogKwdFaKS+a7eE24bLgJsfxQ1XPKk5sG2cwAH2FVyULBgubFq/MbGYrCZvBeWkNoCbEMU29u2JT/uI8aiHxNuPUsc6rVzS/iZJvg12hk75JKUyh/yWbVKfeouoz8CbfSyH5KAAxj2wJeYucO6pXOOspgS2F1Fpr+sJnDXEKaiF+l6SePk+pQchi1kkjSwsnIO34CF2uol5aFeoid2A9ZQLtsoJlOYknGYOCjAi5J6DjECO3ksT8O3nM3iU8tEmfgUu8f9Cl2htD+cxelpx4ectbyiYMY50Pvo3rY6ylj/i0sYNjXlL+F6ctgxTgpxlfJ8Uq6A6lAsmhIDmBXbj5EgGWQu6fZvqzaVtnEjNxcZB5+gi2JWiTZyIr4c+sV7RE1zq4Sua+rbdASVwDj5yfWT+m7JJ5ycTBpARLyfkGE7nn+HnLTh3kcoRTsWxFl7qYXwiDEPWuMFlIecy6KhyjCI6QefoItiVokNPYQbOKuLbj4tyS/wUeDoH6PSJI5WD9mTkbVtodEmo5y3ZIy6s/RrlbdgHN3IPJLdj6mbErmOccbg2ZeLHy3EQuZZOa9ftyPEbo5pnK792ifvEyNDnkmj7GixEQwdJO7hnvv1WMfKvIoU9srldXMjPgy6CjmoN4yQ42ksxoE7qFZGXAy7SLfDDqk+GBKxZC+0+4lyCrCYfxrCPvKd02eLNKxLOd/4hY602K+E1V8FF5Mi5wS6C/yzyqSZnO/G9PCXcisB2fcoFfqrbiC7AEFRCkh9Yz6OPPgHmuvkD9CLKAStF9Xv/kwe7CDoqJ1DSzoGnkuHE10LWEc+Jl2CDmP8Q5qQcvB+JL0H76UGoIW8b3r1zi7rs4OdJWf9RT8EteNcjSsY8qnVBe8BLtBvWJUuQ7bG/givBGu3c3Frw93HSA5Ljn9UJ9+Mo0r9H8BFh525KDi4pcoquBd+cXXABboGTGcI/RlcSOnZANWcA3zbW3KwSww++g7JPJ3iI3zgo+C7MAN1oJzdJaOOi2O4auHiO+SXwa92P0HVsD8oQGHN8fkD18fvkrIEMlOCEXNuPCLyXaBedbkKdF/pwLM5P7fqs6hNrrDtcJfBevr/Bb9iNCxBib4hZS3E+SuN1Qd5XuKfoK4No1+ZV4/8i8G/Yy2Bl2n/7JvcaHKT5+P5b+HLKacsZa/MhtjImdk1svavdJM6asS76CZh6bpNqTxOnv2swshZpW+LrYHtSnm1C7kHaflabGjS5//8X2K77mX78WPRweDq8jY0bWQXuY40/i5O7DJ2BM1ALvCe1cVoNhtp1XmoQO0/9aMPHPorIzaJpX48Du7oBs7SfQ663fQPsTah1yPg6173VzqfbToub1fWp519muTa74JUFFvyZJDTykfhXK+8DvkWPW/xcXOr6eLYPZQPfQ/G/EsbsAfBV4yO9TzLiEw8BOSsUcL3P+wpNyXuQ/FOTh3Qr9t14Iowp5PuroJGDUd4C30F/6DyE0JcHTPzfZYsJeKC9zd8lrIUHyTWvbu93FQmNqHWKKRqy0PB6nLApL5LwK7hN4OPlp4xZAdboivA+ersn7bCHT2k/Al3zB1vvfwD50PcZzVAeuQAAAABJRU5ErkJggg==",
      "text/latex": [
       "$\\displaystyle \\ell g m_{P} \\sin{\\left(\\phi \\right)} \\hat{\\mathbf{e}}_\\theta$"
      ],
      "text/plain": [
       "\\ell⋅g⋅m_P⋅sin(φ)⋅\\hat{\\mathbf{e}}_\\theta"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# net torque on P aobut O.  Note that adding in the constraint \n",
    "# force is unnecessary as it cannot produce a torque about O\n",
    "# we include it just for completness sake\n",
    "M_PO_S = r_PO_S.cross(F_g_S+F_C_S)\n",
    "mat2vec(M_PO_S,sphericalframe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "b5fb0db0-29ac-498f-ba1e-2b3351bea982",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\ddot{\\phi} : \\frac{\\dot{\\theta}^{2} \\sin{\\left(2 \\phi \\right)}}{2} + \\frac{g \\sin{\\left(\\phi \\right)}}{\\ell}, \\  \\ddot{\\theta} : - \\frac{2 \\dot{\\phi} \\dot{\\theta} \\cos{\\left(\\phi \\right)}}{\\sin{\\left(\\phi \\right)}}\\right\\}$"
      ],
      "text/plain": [
       "⎧    2                                       ⎫\n",
       "⎪   θ̇ ⋅sin(2⋅φ)   g⋅sin(φ)     -2⋅φ̇⋅θ̇⋅cos(φ) ⎪\n",
       "⎨φ̈: ─────────── + ────────, θ̈: ──────────────⎬\n",
       "⎪        2          \\ell           sin(φ)    ⎪\n",
       "⎩                                            ⎭"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# solve the equations of motion\n",
    "sol1 = solve(dh_PO_S - M_PO_S, (phdd,thdd))\n",
    "sol1"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2fa7ca23-1a7d-4c8f-9e52-8633357ec88e",
   "metadata": {},
   "source": [
    "## Linear Momentum Balance\n",
    "We will repeat the derivation using the linear momentum form of Newton's second law.  This requires us to evaluate the inertial acceleration of $P$ with respect to $O$ and then set this (times the mass) equal to the resultant force on $P$.  We can then solve for the equations of motion as well as the magnitude of the link force."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "18301a27-cba2-4987-b90b-c07dddb1c98a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left(\\ell \\ddot{\\phi} - \\ell \\dot{\\theta}^{2} \\sin{\\left(\\phi \\right)} \\cos{\\left(\\phi \\right)}\\right) \\hat{\\mathbf{e}}_\\phi + \\left(- \\ell \\dot{\\phi}^{2} - \\ell \\dot{\\theta}^{2} \\sin^{2}{\\left(\\phi \\right)}\\right) \\hat{\\mathbf{e}}_\\rho + \\left(2 \\ell \\dot{\\phi} \\dot{\\theta} \\cos{\\left(\\phi \\right)} + \\ell \\ddot{\\theta} \\sin{\\left(\\phi \\right)}\\right) \\hat{\\mathbf{e}}_\\theta$"
      ],
      "text/plain": [
       "⎛               2              ⎞                         ⎛        2         2    2   ⎞                                 \n",
       "⎝\\ell⋅φ̈ - \\ell⋅θ̇ ⋅sin(φ)⋅cos(φ)⎠⋅\\hat{\\mathbf{e}}_\\phi + ⎝- \\ell⋅φ̇  - \\ell⋅θ̇ ⋅sin (φ)⎠⋅\\hat{\\mathbf{e}}_\\rho + (2⋅\\\n",
       "\n",
       "                                                   \n",
       "ell⋅φ̇⋅θ̇⋅cos(φ) + \\ell⋅θ̈⋅sin(φ))⋅\\hat{\\mathbf{e}}_\\theta"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# acceleration vector\n",
    "a_PO_S = transportEq(v_PO_S, t, diffmap, IwS_S)\n",
    "mat2vec(a_PO_S,sphericalframe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "d92c6a11-7058-47e6-add6-b716f69a43a2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left\\{ F_{C} : \\ell m_{P} \\dot{\\phi}^{2} + \\ell m_{P} \\dot{\\theta}^{2} \\sin^{2}{\\left(\\phi \\right)} - g m_{P} \\cos{\\left(\\phi \\right)}, \\  \\ddot{\\phi} : \\dot{\\theta}^{2} \\sin{\\left(\\phi \\right)} \\cos{\\left(\\phi \\right)} + \\frac{g \\sin{\\left(\\phi \\right)}}{\\ell}, \\  \\ddot{\\theta} : - \\frac{2 \\dot{\\phi} \\dot{\\theta} \\cos{\\left(\\phi \\right)}}{\\sin{\\left(\\phi \\right)}}\\right\\}$"
      ],
      "text/plain": [
       "⎧               2             2    2                        2                 g⋅sin(φ)     -2⋅φ̇⋅θ̇⋅cos(φ) ⎫\n",
       "⎨F_C: \\ell⋅m_P⋅φ̇  + \\ell⋅m_P⋅θ̇ ⋅sin (φ) - g⋅m_P⋅cos(φ), φ̈: θ̇ ⋅sin(φ)⋅cos(φ) + ────────, θ̈: ──────────────⎬\n",
       "⎩                                                                               \\ell           sin(φ)    ⎭"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sol2 = solve(m*a_PO_S - (F_g_S+F_C_S), (phdd,thdd, Fc))\n",
    "sol2"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "adfe0d93-9152-43e7-b633-faba940527e0",
   "metadata": {},
   "source": [
    "A quick check to ensure that the two solutions match:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "cafb12a5-0048-465d-a868-96daf9f7739a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAAoAAAAPCAYAAADd/14OAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAA1ElEQVQoFbWS4Q0BQRCF966Aq+E0IKcBUYlQgajA0QEN0AEdEA04GnBRgWhgfW/tICLnl0nezpu3b2f3bjfx3juLsiz78E6sz+QV2k11GkWHsIbn5LEAr8ARnskTjBTqVJBnEhXwPWkH5uDZ0TpIe48DxYBFmW1dIEj8DG2vKFLc3QdvHHvWsdGlyf8Y65/7OlfrYy4Yr6D1ZUEetcrOuEEwMc6FJK2m2cmMEwTdTDtMM8B1dUMwkpbYo2BC/3MKlkBHkWmLviC/jCoUcYFoBQ8vR8Udfl9DEd5m0FsAAAAASUVORK5CYII=",
      "text/latex": [
       "$\\displaystyle 0$"
      ],
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "simplify(sol1[phdd]- sol2[phdd])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "1f71b1d3-64ea-4ad7-a0dc-98a0bbd6ed84",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAAoAAAAPCAYAAADd/14OAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAA1ElEQVQoFbWS4Q0BQRCF966Aq+E0IKcBUYlQgajA0QEN0AEdEA04GnBRgWhgfW/tICLnl0nezpu3b2f3bjfx3juLsiz78E6sz+QV2k11GkWHsIbn5LEAr8ARnskTjBTqVJBnEhXwPWkH5uDZ0TpIe48DxYBFmW1dIEj8DG2vKFLc3QdvHHvWsdGlyf8Y65/7OlfrYy4Yr6D1ZUEetcrOuEEwMc6FJK2m2cmMEwTdTDtMM8B1dUMwkpbYo2BC/3MKlkBHkWmLviC/jCoUcYFoBQ8vR8Udfl9DEd5m0FsAAAAASUVORK5CYII=",
      "text/latex": [
       "$\\displaystyle 0$"
      ],
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "simplify(sol1[thdd]- sol2[thdd])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a0e0272d-4531-483d-8c03-ad986b371b99",
   "metadata": {},
   "source": [
    "## Euler-Lagrange Solution\n",
    "Finally, let's solve for the equations of motion using the Euler-Lagrange equations.  We will need to evaluate the Lagrangian: $L = T-V$ where $T$ is the total kinetic energy of the system and $V$ is the total potential energy.  The kinetic energy is solely due to the particle's velocity, while the potential energy is solely due to gravity.  We define the potential energy zero point such that\n",
    "$$V = mg\\left(\\mathbf{r}_{P/O} \\cdot \\mathbf{\\hat{e}}_3\\right)$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "29eb569f-f6a2-468e-9993-4847b491f9c0",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\frac{m_{P} \\left(\\ell^{2} \\dot{\\phi}^{2} + \\ell^{2} \\dot{\\theta}^{2} \\sin^{2}{\\left(\\phi \\right)}\\right)}{2}$"
      ],
      "text/plain": [
       "    ⎛    2  2       2  2    2   ⎞\n",
       "m_P⋅⎝\\ell ⋅φ̇  + \\ell ⋅θ̇ ⋅sin (φ)⎠\n",
       "─────────────────────────────────\n",
       "                2                "
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# compute the kinetic energy\n",
    "T = m/2*(v_PO_S.dot(v_PO_S))\n",
    "T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "30211f93-6211-4fd2-a325-180192562481",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\ell g m_{P} \\cos{\\left(\\phi \\right)}$"
      ],
      "text/plain": [
       "\\ell⋅g⋅m_P⋅cos(φ)"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# compute the potential energy\n",
    "V = m*g*(r_PO_S.dot(sCi*Matrix([0,0,1])))\n",
    "V"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "acf83da0-470f-4d78-86de-f8b9b2c18e55",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle - \\ell g m_{P} \\cos{\\left(\\phi \\right)} + \\frac{m_{P} \\left(\\ell^{2} \\dot{\\phi}^{2} + \\ell^{2} \\dot{\\theta}^{2} \\sin^{2}{\\left(\\phi \\right)}\\right)}{2}$"
      ],
      "text/plain": [
       "                         ⎛    2  2       2  2    2   ⎞\n",
       "                     m_P⋅⎝\\ell ⋅φ̇  + \\ell ⋅θ̇ ⋅sin (φ)⎠\n",
       "-\\ell⋅g⋅m_P⋅cos(φ) + ─────────────────────────────────\n",
       "                                     2                "
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# define Lagrangian\n",
    "L = T-V\n",
    "L"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "8c490fa9-e749-40a3-9ff0-d2967b9c0973",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\ddot{\\phi} : \\frac{\\dot{\\theta}^{2} \\sin{\\left(2 \\phi \\right)}}{2} + \\frac{g \\sin{\\left(\\phi \\right)}}{\\ell}, \\  \\ddot{\\theta} : \\frac{2 \\dot{\\phi} \\dot{\\theta} \\sin{\\left(2 \\phi \\right)}}{\\cos{\\left(2 \\phi \\right)} - 1}\\right\\}$"
      ],
      "text/plain": [
       "⎧    2                                       ⎫\n",
       "⎪   θ̇ ⋅sin(2⋅φ)   g⋅sin(φ)     2⋅φ̇⋅θ̇⋅sin(2⋅φ)⎪\n",
       "⎨φ̈: ─────────── + ────────, θ̈: ──────────────⎬\n",
       "⎪        2          \\ell        cos(2⋅φ) - 1 ⎪\n",
       "⎩                                            ⎭"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sol3 = EulerLagrange(L, (th,ph), diffmap)\n",
    "sol3"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8b77b487-4ebf-40f7-9d86-b7905d2a3563",
   "metadata": {},
   "source": [
    "Again, let's check to make sure our solutions all match:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "0524e968-c64a-451a-b3bd-26ad938103f0",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAAoAAAAPCAYAAADd/14OAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAA1ElEQVQoFbWS4Q0BQRCF966Aq+E0IKcBUYlQgajA0QEN0AEdEA04GnBRgWhgfW/tICLnl0nezpu3b2f3bjfx3juLsiz78E6sz+QV2k11GkWHsIbn5LEAr8ARnskTjBTqVJBnEhXwPWkH5uDZ0TpIe48DxYBFmW1dIEj8DG2vKFLc3QdvHHvWsdGlyf8Y65/7OlfrYy4Yr6D1ZUEetcrOuEEwMc6FJK2m2cmMEwTdTDtMM8B1dUMwkpbYo2BC/3MKlkBHkWmLviC/jCoUcYFoBQ8vR8Udfl9DEd5m0FsAAAAASUVORK5CYII=",
      "text/latex": [
       "$\\displaystyle 0$"
      ],
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "simplify(sol1[phdd]- sol3[phdd])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "bd68b6cb-6680-4883-b294-da7a058470f9",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAAoAAAAPCAYAAADd/14OAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAA1ElEQVQoFbWS4Q0BQRCF966Aq+E0IKcBUYlQgajA0QEN0AEdEA04GnBRgWhgfW/tICLnl0nezpu3b2f3bjfx3juLsiz78E6sz+QV2k11GkWHsIbn5LEAr8ARnskTjBTqVJBnEhXwPWkH5uDZ0TpIe48DxYBFmW1dIEj8DG2vKFLc3QdvHHvWsdGlyf8Y65/7OlfrYy4Yr6D1ZUEetcrOuEEwMc6FJK2m2cmMEwTdTDtMM8B1dUMwkpbYo2BC/3MKlkBHkWmLviC/jCoUcYFoBQ8vR8Udfl9DEd5m0FsAAAAASUVORK5CYII=",
      "text/latex": [
       "$\\displaystyle 0$"
      ],
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "simplify(sol1[thdd]- sol3[thdd])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}