DevelopmentHamiltonian.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "7a8721ea8371ca1d",
   "metadata": {},
   "source": [
    "# Deriving the Hamiltonian"
   ]
  },
  {
   "cell_type": "code",
   "id": "initial_id",
   "metadata": {
    "collapsed": true,
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:23.270463Z",
     "start_time": "2024-08-05T11:44:23.268161Z"
    }
   },
   "source": [
    "import sympy as sp\n",
    "from MathFunctions import *"
   ],
   "outputs": [],
   "execution_count": 2
  },
  {
   "cell_type": "markdown",
   "id": "b73b8a7cc4097a1c",
   "metadata": {},
   "source": [
    "## `Simple model`"
   ]
  },
  {
   "cell_type": "code",
   "id": "5f2e2df820404435",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.664026Z",
     "start_time": "2024-08-05T11:44:23.531305Z"
    }
   },
   "source": [
    "# Starting from the Lagrangian of the system\n",
    "L = form_lagrangian('simple')\n",
    "display(L)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "g*(l1*m1*cos(theta1(t)) + l1*m2*cos(theta1(t)) + l2*m2*cos(theta2(t))) + l1**2*m1*Derivative(theta1(t), t)**2/2 + m2*(l1**2*Derivative(theta1(t), t)**2 + 2*l1*l2*cos(theta1(t) - theta2(t))*Derivative(theta1(t), t)*Derivative(theta2(t), t) + l2**2*Derivative(theta2(t), t)**2)/2"
      ],
      "text/latex": "$\\displaystyle g \\left(l_{1} m_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + l_{1} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + l_{2} m_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}\\right) + \\frac{l_{1}^{2} m_{1} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2}}{2} + \\frac{m_{2} \\left(l_{1}^{2} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2} + 2 l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)} + l_{2}^{2} \\left(\\frac{d}{d t} \\theta_{2}{\\left(t \\right)}\\right)^{2}\\right)}{2}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 3
  },
  {
   "cell_type": "markdown",
   "id": "118f03e1f16ed4e",
   "metadata": {},
   "source": [
    "#### The Lagrangian:\n",
    "\n",
    "$$\\mathcal{L} = g \\left(l_{1} m_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + l_{1} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + l_{2} m_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}\\right) + \\frac{l_{1}^{2} m_{1} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2}}{2} + \\frac{m_{2} \\left(l_{1}^{2} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2} + 2 l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)} + l_{2}^{2} \\left(\\frac{d}{d t} \\theta_{2}{\\left(t \\right)}\\right)^{2}\\right)}{2} \\quad \\tag{1}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1aa06ac0b73c0247",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 1\n",
    "\n",
    "Derive the canonical momenta of the system; $p_{\\theta_i} = \\frac{\\partial \\mathcal{L}}{\\partial \\dot{\\theta}_i}$"
   ]
  },
  {
   "cell_type": "code",
   "id": "83045200baa7cc2e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.689586Z",
     "start_time": "2024-08-05T11:44:24.665808Z"
    }
   },
   "source": [
    "# the definition: omega_i = sp.diff(theta_i, t) is also made in MathFunctions\n",
    "# Define the generalised velocities\n",
    "omega1 = sp.diff(theta1, t)\n",
    "omega2 = sp.diff(theta2, t)\n",
    "\n",
    "p_theta1_def = sp.diff(L, omega1)\n",
    "p_theta2_def = sp.diff(L, omega2)"
   ],
   "outputs": [],
   "execution_count": 4
  },
  {
   "cell_type": "code",
   "id": "ebc7abd2c8ff0a64",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.700906Z",
     "start_time": "2024-08-05T11:44:24.691541Z"
    }
   },
   "source": [
    "p_theta1_def = sp.expand(p_theta1_def)\n",
    "display(p_theta1_def)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "l1**2*m1*Derivative(theta1(t), t) + l1**2*m2*Derivative(theta1(t), t) + l1*l2*m2*cos(theta1(t) - theta2(t))*Derivative(theta2(t), t)"
      ],
      "text/latex": "$\\displaystyle l_{1}^{2} m_{1} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} + l_{1}^{2} m_{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} + l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 5
  },
  {
   "cell_type": "code",
   "id": "df1a9092029ee860",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.714525Z",
     "start_time": "2024-08-05T11:44:24.705169Z"
    }
   },
   "source": [
    "p_theta2_def = sp.expand(p_theta2_def)\n",
    "display(p_theta2_def)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "l1*l2*m2*cos(theta1(t) - theta2(t))*Derivative(theta1(t), t) + l2**2*m2*Derivative(theta2(t), t)"
      ],
      "text/latex": "$\\displaystyle l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} + l_{2}^{2} m_{2} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 6
  },
  {
   "cell_type": "markdown",
   "id": "1dc8989c65944e88",
   "metadata": {},
   "source": [
    "The canonical momenta are given by:\n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "p_{\\theta_1} &\\equiv \\frac{\\partial \\mathcal{L}}{\\partial \\dot{\\theta_1}} = l_{1}^{2} m_{1} \\dot{\\theta_{1}} + l_{1}^{2} m_{2} \\dot{\\theta_{1}} + l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1} - \\theta_{2} \\right)} \\dot{\\theta_{2}} \\quad \\tag{2} \\\\\n",
    "p_{\\theta_2} &\\equiv \\frac{\\partial \\mathcal{L}}{\\partial \\dot{\\theta_2}} = l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1} - \\theta_{2} \\right)} \\dot{\\theta_{1}} + l_{2}^{2} m_{2} \\dot{\\theta_{2}} \\quad \\tag{3}\n",
    "\\end{align}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4605e8d934312ad6",
   "metadata": {},
   "source": [
    "We define the canonical momenta as SymPy equations;"
   ]
  },
  {
   "cell_type": "code",
   "id": "dff9cc9ce826a7d1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.720905Z",
     "start_time": "2024-08-05T11:44:24.716987Z"
    }
   },
   "source": [
    "# introduce symbolic functions to represent the canonical momenta\n",
    "p_theta1 = sp.Function('p_theta_1')(t)\n",
    "p_theta2 = sp.Function('p_theta_2')(t)"
   ],
   "outputs": [],
   "execution_count": 7
  },
  {
   "cell_type": "code",
   "id": "e5ff0a2ab036a442",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.735020Z",
     "start_time": "2024-08-05T11:44:24.722212Z"
    }
   },
   "source": [
    "# Define SymPy equations for the canonical momenta\n",
    "peq1 = sp.Eq(p_theta1, p_theta1_def)\n",
    "peq2 = sp.Eq(p_theta2, p_theta2_def)"
   ],
   "outputs": [],
   "execution_count": 8
  },
  {
   "cell_type": "code",
   "id": "dd510bdaf31efe36",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.743189Z",
     "start_time": "2024-08-05T11:44:24.736863Z"
    }
   },
   "source": [
    "# Eqn 2\n",
    "display(peq1)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Eq(p_theta_1(t), l1**2*m1*Derivative(theta1(t), t) + l1**2*m2*Derivative(theta1(t), t) + l1*l2*m2*cos(theta1(t) - theta2(t))*Derivative(theta2(t), t))"
      ],
      "text/latex": "$\\displaystyle \\operatorname{p_{\\theta 1}}{\\left(t \\right)} = l_{1}^{2} m_{1} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} + l_{1}^{2} m_{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} + l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 9
  },
  {
   "cell_type": "code",
   "id": "658ba0369b66c7a9",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.752103Z",
     "start_time": "2024-08-05T11:44:24.744684Z"
    }
   },
   "source": [
    "# Eqn 3\n",
    "display(peq2)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Eq(p_theta_2(t), l1*l2*m2*cos(theta1(t) - theta2(t))*Derivative(theta1(t), t) + l2**2*m2*Derivative(theta2(t), t))"
      ],
      "text/latex": "$\\displaystyle \\operatorname{p_{\\theta 2}}{\\left(t \\right)} = l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} + l_{2}^{2} m_{2} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 10
  },
  {
   "cell_type": "markdown",
   "id": "e5549b087fa84109",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 2\n",
    "\n",
    "The Hamiltonian $\\mathcal{H}$ representing the total energy of the system, $T + V$, is the Legendre transform of the Lagrangian given by:\n",
    "\n",
    "$$\n",
    "\\mathcal{H} = \\sum_{i=1}^2  \\dot{\\theta_i} p_{\\theta_i} - \\mathcal{L} \\quad \\tag{4}\n",
    "$$\n",
    "\n",
    "From the Hamiltonian, we aim to derive a set of equations of motion equivalent to the Euler-Lagrange equations:\n",
    "\n",
    "$$\\dot{\\theta_i}=\\frac{\\partial \\mathcal{H}}{\\partial p_{\\theta_i}} \\quad \\tag{5}$$ \n",
    "$$\\dot{p_{\\theta_i}}=-\\frac{\\partial \\mathcal{H}}{\\partial \\theta_i} \\quad \\tag{6}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bcecdd6581f2415b",
   "metadata": {},
   "source": [
    "Solving eqns $(5)$ and $(6)$ requires us to write $\\mathcal{H}$ as a function of $\\theta_1, \\theta_2, p_{\\theta_1} \\ \\& \\ p_{\\theta_2}$\n",
    "\n",
    "In order to achieve that, we must determine $\\dot{\\theta_i}$ and $\\mathcal{L}$ in terms of these variables so that we can substitute them into equation (4). \n",
    "\n",
    "Equations $(2)$ and $(3)$ can be written in matrix form as:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "p_{\\theta_1} \\\\\n",
    "p_{\\theta_2}\n",
    "\\end{bmatrix} \n",
    "= \n",
    "\\mathbf{B} \n",
    "\\begin{bmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{bmatrix}\n",
    "\\quad \\tag{7}\n",
    "$$\n",
    "\n",
    "where $\\mathbf{B}$ is a $2 \\times 2$ matrix with entries that are functions of $\\theta_1$ and $\\theta_2$:\n",
    "\n",
    "$$\n",
    "\\mathbf{B} = \n",
    "\\begin{bmatrix}\n",
    "(m_1 + m_2) l_1^2 & m_2 l_1 l_2 \\cos(\\theta_1 - \\theta_2) \\\\\n",
    "m_2 l_1 l_2 \\cos(\\theta_1 - \\theta_2) & m_2 l_2^2\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "From equation (7), we can obtain the generalised velocities $\\dot{\\theta_i}$ in terms of the canonical momenta $p_{\\theta_i}$ and the angles $\\theta_i$:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{bmatrix}\n",
    "= \n",
    "\\mathbf{B}^{-1} \n",
    "\\begin{bmatrix}\n",
    "p_{\\theta_1} \\\\\n",
    "p_{\\theta_2}\n",
    "\\end{bmatrix}\n",
    "\\quad \\tag{8}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "id": "2c612826e2df7b00",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.765710Z",
     "start_time": "2024-08-05T11:44:24.753536Z"
    }
   },
   "source": [
    "# Defining the coefficient matrix B\n",
    "B = sp.Matrix([\n",
    "    [(m1 + m2) * l1**2, m2 * l1 * l2 * sp.cos(theta1 - theta2)],\n",
    "    [m2 * l1 * l2 * sp.cos(theta1 - theta2), m2 * l2**2]\n",
    "])\n",
    "\n",
    "display(B)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Matrix([\n",
       "[                    l1**2*(m1 + m2), l1*l2*m2*cos(theta1(t) - theta2(t))],\n",
       "[l1*l2*m2*cos(theta1(t) - theta2(t)),                            l2**2*m2]])"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}l_{1}^{2} \\left(m_{1} + m_{2}\\right) & l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\\\l_{1} l_{2} m_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} & l_{2}^{2} m_{2}\\end{matrix}\\right]$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 11
  },
  {
   "cell_type": "markdown",
   "id": "661e953e6e2b923f",
   "metadata": {},
   "source": [
    "We notice that $\\mathbf{B}^{T}=\\mathbf{B}$\n",
    "\n",
    "Also, where the Lagrangian is given $\\mathcal{L}=T-V$ in eqn $(1)$, by eqn $(7)$ we can write the kinetic energy in terms of the matrix $\\mathbf{B}$ as;\n",
    "\n",
    "$$\n",
    "T = \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} & \\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "\\mathbf{B}\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "= \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} & \\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "\\begin{pmatrix}\n",
    "p_{\\theta_1} \\\\\n",
    "p_{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "= \\frac{1}{2} (\\dot{\\theta_1} p_{\\theta_1} + \\dot{\\theta_2} p_{\\theta_2}) \n",
    "\\quad \\tag{9}\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "id": "5b8f03b293c26bdb",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:24.777945Z",
     "start_time": "2024-08-05T11:44:24.768908Z"
    }
   },
   "source": [
    "# B has determinant:\n",
    "det_B = B.det()\n",
    "display(det_B)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "l1**2*l2**2*m1*m2 - l1**2*l2**2*m2**2*cos(theta1(t) - theta2(t))**2 + l1**2*l2**2*m2**2"
      ],
      "text/latex": "$\\displaystyle l_{1}^{2} l_{2}^{2} m_{1} m_{2} - l_{1}^{2} l_{2}^{2} m_{2}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1}^{2} l_{2}^{2} m_{2}^{2}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 12
  },
  {
   "cell_type": "markdown",
   "id": "e6c7ea01a321aa8a",
   "metadata": {},
   "source": [
    "A matrix is invertible if and only if its determinant is non-zero\n",
    "\n",
    "Given that $m_1, m_2, l_1, l_1$ are strictly positive and $1 - \\cos^2(x) \\equiv \\sin^2(x)$ which is bounded between $0$ and $1$ for all $x \\in \\mathbb{R}$, $\\mathbf{B}$ is always invertible."
   ]
  },
  {
   "cell_type": "code",
   "id": "e3661f2dc8b8bdfd",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:25.884479Z",
     "start_time": "2024-08-05T11:44:24.779272Z"
    }
   },
   "source": [
    "B_inv = B.inv()\n",
    "display(B_inv)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Matrix([\n",
       "[                          1/(l1**2*m1 - l1**2*m2*cos(theta1(t) - theta2(t))**2 + l1**2*m2), -cos(theta1(t) - theta2(t))/(l1*l2*m1 - l1*l2*m2*cos(theta1(t) - theta2(t))**2 + l1*l2*m2)],\n",
       "[-cos(theta1(t) - theta2(t))/(l1*l2*m1 - l1*l2*m2*cos(theta1(t) - theta2(t))**2 + l1*l2*m2),          (m1 + m2)/(l2**2*m1*m2 - l2**2*m2**2*cos(theta1(t) - theta2(t))**2 + l2**2*m2**2)]])"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}\\frac{1}{l_{1}^{2} m_{1} - l_{1}^{2} m_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1}^{2} m_{2}} & - \\frac{\\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1} l_{2} m_{1} - l_{1} l_{2} m_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1} l_{2} m_{2}}\\\\- \\frac{\\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1} l_{2} m_{1} - l_{1} l_{2} m_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1} l_{2} m_{2}} & \\frac{m_{1} + m_{2}}{l_{2}^{2} m_{1} m_{2} - l_{2}^{2} m_{2}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2}^{2}}\\end{matrix}\\right]$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 13
  },
  {
   "cell_type": "code",
   "id": "9d192a5ef0d783da",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:25.887771Z",
     "start_time": "2024-08-05T11:44:25.885380Z"
    }
   },
   "source": [
    "# Define canonical momenta as a matrix\n",
    "p = sp.Matrix([p_theta1, p_theta2])"
   ],
   "outputs": [],
   "execution_count": 14
  },
  {
   "cell_type": "markdown",
   "id": "af06ffdeaf2e576",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 2b\n",
    "\n",
    "We derive eqns $(5)$ from eqn $(8)$ which we can later use to check the validity of our Hamiltonian"
   ]
  },
  {
   "cell_type": "code",
   "id": "d8965809b656219f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:25.898922Z",
     "start_time": "2024-08-05T11:44:25.888865Z"
    }
   },
   "source": [
    "# Define the generalised velocities by eqn(8)\n",
    "omega = B_inv * p\n",
    "omega1_sol, omega2_sol = omega[0], omega[1]"
   ],
   "outputs": [],
   "execution_count": 15
  },
  {
   "cell_type": "code",
   "id": "fe2d7e2a60e25dec",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:25.907592Z",
     "start_time": "2024-08-05T11:44:25.900060Z"
    }
   },
   "source": [
    "display(omega1_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-p_theta_2(t)*cos(theta1(t) - theta2(t))/(l1*l2*m1 - l1*l2*m2*cos(theta1(t) - theta2(t))**2 + l1*l2*m2) + p_theta_1(t)/(l1**2*m1 - l1**2*m2*cos(theta1(t) - theta2(t))**2 + l1**2*m2)"
      ],
      "text/latex": "$\\displaystyle - \\frac{\\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1} l_{2} m_{1} - l_{1} l_{2} m_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1} l_{2} m_{2}} + \\frac{\\operatorname{p_{\\theta 1}}{\\left(t \\right)}}{l_{1}^{2} m_{1} - l_{1}^{2} m_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1}^{2} m_{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 16
  },
  {
   "cell_type": "code",
   "id": "94b62fa75c85c57",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:25.916216Z",
     "start_time": "2024-08-05T11:44:25.908616Z"
    }
   },
   "source": [
    "display(omega2_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(m1 + m2)*p_theta_2(t)/(l2**2*m1*m2 - l2**2*m2**2*cos(theta1(t) - theta2(t))**2 + l2**2*m2**2) - p_theta_1(t)*cos(theta1(t) - theta2(t))/(l1*l2*m1 - l1*l2*m2*cos(theta1(t) - theta2(t))**2 + l1*l2*m2)"
      ],
      "text/latex": "$\\displaystyle \\frac{\\left(m_{1} + m_{2}\\right) \\operatorname{p_{\\theta 2}}{\\left(t \\right)}}{l_{2}^{2} m_{1} m_{2} - l_{2}^{2} m_{2}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2}^{2}} - \\frac{\\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1} l_{2} m_{1} - l_{1} l_{2} m_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1} l_{2} m_{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 17
  },
  {
   "cell_type": "markdown",
   "id": "d5c9bbf366db574c",
   "metadata": {},
   "source": [
    "We now apply the trigonometric substitution, $1 - \\cos^2(x) = \\sin^2(x)$ and simplify"
   ]
  },
  {
   "cell_type": "code",
   "id": "383e39703f9dbf17",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:26.355039Z",
     "start_time": "2024-08-05T11:44:25.917078Z"
    }
   },
   "source": [
    "# Simplify omega1_sol substitution for trigonometric identity\n",
    "omega1_sol = sp.simplify(omega1_sol.subs(sp.cos(theta1 - theta2)**2, 1 - sp.sin(theta1 - theta2)**2))\n",
    "display(omega1_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-l1*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2*p_theta_1(t))/(l1**2*l2*(m1 + m2*sin(theta1(t) - theta2(t))**2))"
      ],
      "text/latex": "$\\displaystyle \\frac{- l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}}{l_{1}^{2} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 18
  },
  {
   "cell_type": "code",
   "id": "7a58850a2addf75e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:27.221137Z",
     "start_time": "2024-08-05T11:44:26.356098Z"
    }
   },
   "source": [
    "# Simplify omega2_sol with substitution for trigonometric identity\n",
    "omega2_sol_simplified = sp.simplify(omega2_sol.subs(sp.cos(theta1 - theta2)**2, 1 - sp.sin(theta1 - theta2)**2))\n",
    "\n",
    "# Explicitly substitute m2 - m2*cos^2(theta1 - theta2) with m2*sin^2(theta1 - theta2)\n",
    "omega2_sol_targeted = omega2_sol_simplified.subs(m2 - m2 * sp.cos(theta1 - theta2)**2, m2 * sp.sin(theta1 - theta2)**2)\n",
    "omega2_sol = sp.simplify(omega2_sol_targeted)\n",
    "display(omega2_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(l1*(m1 + m2)*p_theta_2(t) - l2*m2*p_theta_1(t)*cos(theta1(t) - theta2(t)))/(l1*l2**2*m2*(m1 + m2*sin(theta1(t) - theta2(t))**2))"
      ],
      "text/latex": "$\\displaystyle \\frac{l_{1} \\left(m_{1} + m_{2}\\right) \\operatorname{p_{\\theta 2}}{\\left(t \\right)} - l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1} l_{2}^{2} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 19
  },
  {
   "cell_type": "markdown",
   "id": "d44d8574ed06ec1d",
   "metadata": {},
   "source": [
    "Hence, we have defined:\n",
    "\n",
    "$$\\dot{\\theta_1}=\\frac{- l_{1} \\operatorname{p_{\\theta 2}} \\cos{\\left(\\theta_{1} - \\theta_{2} \\right)} + l_{2} \\operatorname{p_{\\theta 1}}}{l_{1}^{2} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1} - \\theta_{2} \\right)}\\right)} \\quad \\tag{10}$$\n",
    "\n",
    "$$\\dot{\\theta_2}=\\frac{l_{1} \\left(m_{1} + m_{2}\\right) \\operatorname{p_{\\theta 2}} - l_{2} m_{2} \\operatorname{p_{\\theta 1}} \\cos{\\left(\\theta_{1} - \\theta_{2} \\right)}}{l_{1} l_{2}^{2} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1} - \\theta_{2} \\right)}\\right)} \\quad \\tag{11}$$\n",
    "\n",
    "Which we could substitute into equation $(4)$ to yield the Hamiltonian"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "17a213370cc5dd1f",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 3\n",
    "\n",
    "Let's circumvent the complexity of deriving equation $(4)$ from $(10)$ & $(11)$ and derive the Hamiltonian from $H=T+V$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c0e8645da4ad3e14",
   "metadata": {},
   "source": [
    "Starting from eqn $(8)$ using the transposed relation;\n",
    "\n",
    "$$\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "= \\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix}\n",
    "\\left(\\mathbf{B}^{-1}\\right)^T = \\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix} \\mathbf{B}^{-1},\n",
    "$$\n",
    "\n",
    "By eqn $(9)$ we can write the kinetic energy, in the form\n",
    "\n",
    "$$\n",
    "T = \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix}\n",
    "\\mathbf{B}^{-1}\n",
    "\\begin{pmatrix}\n",
    "p_1 \\\\\n",
    "p_2\n",
    "\\end{pmatrix}.\n",
    "$$\n",
    "\n",
    "Now the Hamiltonian function becomes,\n",
    "\n",
    "$$\n",
    "\\mathcal{H} = T + V = \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix}\n",
    "\\mathbf{B}^{-1}\n",
    "\\begin{pmatrix}\n",
    "p_1 \\\\\n",
    "p_2\n",
    "\\end{pmatrix}\n",
    "- (m_1 + m_2) g l_1 \\cos \\theta_1 - m_2 g l_2 \\cos \\theta_2.\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "id": "d679a98c86441c69",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:27.985290Z",
     "start_time": "2024-08-05T11:44:27.222044Z"
    }
   },
   "source": [
    "# Kinetic energy T expressed in terms of canonical momenta\n",
    "T = sp.Rational(1,2) * p.T * B_inv * p\n",
    "\n",
    "# Simplify T, apply trig ident\n",
    "T_target = T[0].subs(sp.cos(theta1 - theta2)**2, 1 -  sp.sin(theta1 - theta2)**2)\n",
    "T_simp = sp.simplify(T_target)\n",
    "display(T_simp)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(l1**2*m1*p_theta_2(t)**2 + l1**2*m2*p_theta_2(t)**2 - 2*l1*l2*m2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2**2*m2*p_theta_1(t)**2)/(2*l1**2*l2**2*m2*(m1 + m2*sin(theta1(t) - theta2(t))**2))"
      ],
      "text/latex": "$\\displaystyle \\frac{l_{1}^{2} m_{1} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + l_{1}^{2} m_{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 2 l_{1} l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}}{2 l_{1}^{2} l_{2}^{2} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 20
  },
  {
   "cell_type": "code",
   "id": "be7c6195603ffc35",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:27.992427Z",
     "start_time": "2024-08-05T11:44:27.986562Z"
    }
   },
   "source": [
    "# Potential energy V\n",
    "V = -(m1 + m2) * g * l1 * sp.cos(theta1) - m2 * g * l2 * sp.cos(theta2)\n",
    "display(V)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "g*l1*(-m1 - m2)*cos(theta1(t)) - g*l2*m2*cos(theta2(t))"
      ],
      "text/latex": "$\\displaystyle g l_{1} \\left(- m_{1} - m_{2}\\right) \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} - g l_{2} m_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 21
  },
  {
   "cell_type": "code",
   "id": "4a9c292b1c540eb5",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:28.003477Z",
     "start_time": "2024-08-05T11:44:27.993637Z"
    }
   },
   "source": [
    "# Return the Hamiltonian \n",
    "H = T_simp + V\n",
    "display(H)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "g*l1*(-m1 - m2)*cos(theta1(t)) - g*l2*m2*cos(theta2(t)) + (l1**2*m1*p_theta_2(t)**2 + l1**2*m2*p_theta_2(t)**2 - 2*l1*l2*m2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2**2*m2*p_theta_1(t)**2)/(2*l1**2*l2**2*m2*(m1 + m2*sin(theta1(t) - theta2(t))**2))"
      ],
      "text/latex": "$\\displaystyle g l_{1} \\left(- m_{1} - m_{2}\\right) \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} - g l_{2} m_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)} + \\frac{l_{1}^{2} m_{1} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + l_{1}^{2} m_{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 2 l_{1} l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}}{2 l_{1}^{2} l_{2}^{2} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 22
  },
  {
   "cell_type": "markdown",
   "id": "b6654b906f3705ea",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 4\n",
    "\n",
    "#### Forming the first order system\n",
    "\n",
    "Partially differentiating $\\mathcal{H}$ w.r.t $p_{\\theta_i}$ should return eqns $(10)$ and $(11)$ "
   ]
  },
  {
   "cell_type": "code",
   "id": "1b5ef1a49b01e470",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:28.355432Z",
     "start_time": "2024-08-05T11:44:28.004373Z"
    }
   },
   "source": [
    "# Partially differentiate H with respect to p_theta1 and p_theta2\n",
    "H_theta1 = sp.diff(H, p_theta1)\n",
    "H_theta1 = sp.simplify(H_theta1)\n",
    "display(H_theta1)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-l1*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2*p_theta_1(t))/(l1**2*l2*(m1 + m2*sin(theta1(t) - theta2(t))**2))"
      ],
      "text/latex": "$\\displaystyle \\frac{- l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}}{l_{1}^{2} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 23
  },
  {
   "cell_type": "code",
   "id": "b6f31a6ed3ad55b1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:28.832638Z",
     "start_time": "2024-08-05T11:44:28.356270Z"
    }
   },
   "source": [
    "H_theta2 = sp.diff(H, p_theta2)\n",
    "H_theta2 = sp.simplify(H_theta2)\n",
    "display(H_theta2)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(l1*m1*p_theta_2(t) + l1*m2*p_theta_2(t) - l2*m2*p_theta_1(t)*cos(theta1(t) - theta2(t)))/(l1*l2**2*m2*(m1 + m2*sin(theta1(t) - theta2(t))**2))"
      ],
      "text/latex": "$\\displaystyle \\frac{l_{1} m_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} + l_{1} m_{2} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} - l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1} l_{2}^{2} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 24
  },
  {
   "cell_type": "markdown",
   "id": "f3e5c6dea427e743",
   "metadata": {},
   "source": [
    "Which are indeed equivalent to eqns $(10)$ and $(11)$ \n",
    "\n",
    "Deriving the equations for $\\dot{p_{\\theta_1}}$ and $\\dot{p_{\\theta_2}}$"
   ]
  },
  {
   "cell_type": "code",
   "id": "396bb74ae513d47c",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:35.272582Z",
     "start_time": "2024-08-05T11:44:28.833868Z"
    }
   },
   "source": [
    "H_p_theta1 = -sp.diff(H, theta1)\n",
    "H_p_theta1 = sp.simplify(H_p_theta1)\n",
    "display(H_p_theta1)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-g*l1**3*l2**2*(m1 + m2)*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2*sin(theta1(t)) - l1*l2*(m1 + m2*sin(theta1(t) - theta2(t))**2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) + (l1**2*m1*p_theta_2(t)**2 + l1**2*m2*p_theta_2(t)**2 - 2*l1*l2*m2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2**2*m2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(l1**2*l2**2*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2)"
      ],
      "text/latex": "$\\displaystyle \\frac{- g l_{1}^{3} l_{2}^{2} \\left(m_{1} + m_{2}\\right) \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2} \\sin{\\left(\\theta_{1}{\\left(t \\right)} \\right)} - l_{1} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + \\left(l_{1}^{2} m_{1} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + l_{1}^{2} m_{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 2 l_{1} l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1}^{2} l_{2}^{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 25
  },
  {
   "cell_type": "code",
   "id": "541fc7a269766c70",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.051431Z",
     "start_time": "2024-08-05T11:44:35.273558Z"
    }
   },
   "source": [
    "H_p_theta2 = -sp.diff(H, theta2)\n",
    "H_p_theta2 = sp.simplify(H_p_theta2)\n",
    "display(H_p_theta2)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-g*l1**2*l2**3*m2*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2*sin(theta2(t)) + l1*l2*(m1 + m2*sin(theta1(t) - theta2(t))**2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) - (l1**2*m1*p_theta_2(t)**2 + l1**2*m2*p_theta_2(t)**2 - 2*l1*l2*m2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2**2*m2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(l1**2*l2**2*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2)"
      ],
      "text/latex": "$\\displaystyle \\frac{- g l_{1}^{2} l_{2}^{3} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2} \\sin{\\left(\\theta_{2}{\\left(t \\right)} \\right)} + l_{1} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} - \\left(l_{1}^{2} m_{1} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + l_{1}^{2} m_{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 2 l_{1} l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1}^{2} l_{2}^{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 26
  },
  {
   "cell_type": "code",
   "id": "1055f11c76d9ecf5",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.072591Z",
     "start_time": "2024-08-05T11:44:40.052564Z"
    }
   },
   "source": [
    "# Declare all as SymPy equations\n",
    "Heq1 = sp.Eq(omega1, H_theta1)\n",
    "Heq2 = sp.Eq(omega2, H_theta2)\n",
    "Heq3 = sp.Eq(sp.diff(p_theta1, t), H_p_theta1)\n",
    "Heq4 = sp.Eq(sp.diff(p_theta2, t), H_p_theta2)"
   ],
   "outputs": [],
   "execution_count": 27
  },
  {
   "cell_type": "markdown",
   "id": "d03dfe06fd3934cf",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 5\n",
    "\n",
    "#### Declare matrix equations"
   ]
  },
  {
   "cell_type": "code",
   "id": "2d662078569d0fab",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.105616Z",
     "start_time": "2024-08-05T11:44:40.073694Z"
    }
   },
   "source": [
    "LHS_FIRST = sp.Matrix([[Heq1.lhs], [Heq2.lhs], [Heq3.lhs], [Heq4.lhs]])\n",
    "RHS_FIRST = sp.Matrix([[Heq1.rhs], [Heq2.rhs], [Heq3.rhs], [Heq4.rhs]])\n",
    "\n",
    "MAT_EQ = sp.Eq(LHS_FIRST, RHS_FIRST)\n",
    "display(MAT_EQ)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Eq(Matrix([\n",
       "[   Derivative(theta1(t), t)],\n",
       "[   Derivative(theta2(t), t)],\n",
       "[Derivative(p_theta_1(t), t)],\n",
       "[Derivative(p_theta_2(t), t)]]), Matrix([\n",
       "[                                                                                                                                                                                                                                                                                                                                           (-l1*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2*p_theta_1(t))/(l1**2*l2*(m1 + m2*sin(theta1(t) - theta2(t))**2))],\n",
       "[                                                                                                                                                                                                                                                                                                              (l1*m1*p_theta_2(t) + l1*m2*p_theta_2(t) - l2*m2*p_theta_1(t)*cos(theta1(t) - theta2(t)))/(l1*l2**2*m2*(m1 + m2*sin(theta1(t) - theta2(t))**2))],\n",
       "[(-g*l1**3*l2**2*(m1 + m2)*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2*sin(theta1(t)) - l1*l2*(m1 + m2*sin(theta1(t) - theta2(t))**2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) + (l1**2*m1*p_theta_2(t)**2 + l1**2*m2*p_theta_2(t)**2 - 2*l1*l2*m2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2**2*m2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(l1**2*l2**2*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2)],\n",
       "[       (-g*l1**2*l2**3*m2*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2*sin(theta2(t)) + l1*l2*(m1 + m2*sin(theta1(t) - theta2(t))**2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) - (l1**2*m1*p_theta_2(t)**2 + l1**2*m2*p_theta_2(t)**2 - 2*l1*l2*m2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + l2**2*m2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(l1**2*l2**2*(m1 + m2*sin(theta1(t) - theta2(t))**2)**2)]]))"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\\\\\frac{d}{d t} \\theta_{2}{\\left(t \\right)}\\\\\\frac{d}{d t} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}\\\\\\frac{d}{d t} \\operatorname{p_{\\theta 2}}{\\left(t \\right)}\\end{matrix}\\right] = \\left[\\begin{matrix}\\frac{- l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}}{l_{1}^{2} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}\\\\\\frac{l_{1} m_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} + l_{1} m_{2} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} - l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1} l_{2}^{2} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}\\\\\\frac{- g l_{1}^{3} l_{2}^{2} \\left(m_{1} + m_{2}\\right) \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2} \\sin{\\left(\\theta_{1}{\\left(t \\right)} \\right)} - l_{1} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + \\left(l_{1}^{2} m_{1} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + l_{1}^{2} m_{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 2 l_{1} l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1}^{2} l_{2}^{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2}}\\\\\\frac{- g l_{1}^{2} l_{2}^{3} m_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2} \\sin{\\left(\\theta_{2}{\\left(t \\right)} \\right)} + l_{1} l_{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} - \\left(l_{1}^{2} m_{1} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + l_{1}^{2} m_{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 2 l_{1} l_{2} m_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{2}^{2} m_{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{l_{1}^{2} l_{2}^{2} \\left(m_{1} + m_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)^{2}}\\end{matrix}\\right]$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 28
  },
  {
   "cell_type": "markdown",
   "id": "fa6cb6b1d9f4ac23",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "## `Compound Model`"
   ]
  },
  {
   "cell_type": "code",
   "id": "8aaea39b7e7699bd",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.873885Z",
     "start_time": "2024-08-05T11:44:40.108860Z"
    }
   },
   "source": [
    "# Starting from the Lagrangian of the system\n",
    "L = form_lagrangian('compound')\n",
    "display(L)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "7*M1*l1**2*Derivative(theta1(t), t)**2/24 + M2*l1**2*Derivative(theta1(t), t)**2/8 + M2*l1*l2*cos(theta1(t) - theta2(t))*Derivative(theta1(t), t)*Derivative(theta2(t), t)/4 + 7*M2*l2**2*Derivative(theta2(t), t)**2/24 + g*(M1*l1*cos(theta1(t)) + M2*l1*cos(theta1(t)) + M2*l2*cos(theta2(t)))/2"
      ],
      "text/latex": "$\\displaystyle \\frac{7 M_{1} l_{1}^{2} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2}}{24} + \\frac{M_{2} l_{1}^{2} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2}}{8} + \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{4} + \\frac{7 M_{2} l_{2}^{2} \\left(\\frac{d}{d t} \\theta_{2}{\\left(t \\right)}\\right)^{2}}{24} + \\frac{g \\left(M_{1} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}\\right)}{2}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 29
  },
  {
   "cell_type": "markdown",
   "id": "2384994ef26ce43f",
   "metadata": {},
   "source": [
    "#### The Lagrangian\n",
    "\n",
    "$$\\mathcal{L}=\\frac{7 M_{1} l_{1}^{2} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2}}{24} + \\frac{M_{2} l_{1}^{2} \\left(\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\right)^{2}}{8} + \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{4} + \\frac{7 M_{2} l_{2}^{2} \\left(\\frac{d}{d t} \\theta_{2}{\\left(t \\right)}\\right)^{2}}{24} + \\frac{g \\left(M_{1} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}\\right)}{2} \\quad \\tag{1}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "46e216e42fb3f067",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 1\n",
    "\n",
    "Derive the canonical momenta of the system; $p_{\\theta_i} = \\frac{\\partial \\mathcal{L}}{\\partial \\dot{\\theta}_i}$"
   ]
  },
  {
   "cell_type": "code",
   "id": "9115a774751d9680",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.898421Z",
     "start_time": "2024-08-05T11:44:40.879233Z"
    }
   },
   "source": [
    "# the definition: omega_i = sp.diff(theta_i, t) is also made in MathFunctions\n",
    "# Define the generalised velocities\n",
    "omega1 = sp.diff(theta1, t)\n",
    "omega2 = sp.diff(theta2, t)\n",
    "\n",
    "p_theta1_def = sp.diff(L, omega1)\n",
    "p_theta2_def = sp.diff(L, omega2)"
   ],
   "outputs": [],
   "execution_count": 30
  },
  {
   "cell_type": "code",
   "id": "546c455afbe65ceb",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.908880Z",
     "start_time": "2024-08-05T11:44:40.900542Z"
    }
   },
   "source": [
    "p_theta1_def = sp.expand(p_theta1_def)\n",
    "display(p_theta1_def)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "7*M1*l1**2*Derivative(theta1(t), t)/12 + M2*l1**2*Derivative(theta1(t), t)/4 + M2*l1*l2*cos(theta1(t) - theta2(t))*Derivative(theta2(t), t)/4"
      ],
      "text/latex": "$\\displaystyle \\frac{7 M_{1} l_{1}^{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{12} + \\frac{M_{2} l_{1}^{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{4} + \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{4}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 31
  },
  {
   "cell_type": "code",
   "id": "6b7a5d6209459d6d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.919406Z",
     "start_time": "2024-08-05T11:44:40.912451Z"
    }
   },
   "source": [
    "p_theta2_def = sp.expand(p_theta2_def)\n",
    "display(p_theta2_def)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "M2*l1*l2*cos(theta1(t) - theta2(t))*Derivative(theta1(t), t)/4 + 7*M2*l2**2*Derivative(theta2(t), t)/12"
      ],
      "text/latex": "$\\displaystyle \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{4} + \\frac{7 M_{2} l_{2}^{2} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{12}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 32
  },
  {
   "cell_type": "markdown",
   "id": "9aa7f7a69ad4ecf0",
   "metadata": {},
   "source": [
    "The canonical momenta are given by:\n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "p_{\\theta_1} &\\equiv \\frac{\\partial \\mathcal{L}}{\\partial \\dot{\\theta_1}} = \\frac{7 M_{1} l_{1}^{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{12} + \\frac{M_{2} l_{1}^{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{4} + \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{4} \\quad \\tag{2} \\\\\n",
    "p_{\\theta_2} &\\equiv \\frac{\\partial \\mathcal{L}}{\\partial \\dot{\\theta_2}} = \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{4} + \\frac{7 M_{2} l_{2}^{2} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{12} \\quad \\tag{3}\n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "Which we define as SymPy equations"
   ]
  },
  {
   "cell_type": "code",
   "id": "62d02fd12764d66c",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.934283Z",
     "start_time": "2024-08-05T11:44:40.920845Z"
    }
   },
   "source": [
    "# introduce symbolic functions to represent the canonical momenta\n",
    "p_theta1 = sp.Function('p_theta_1')(t)\n",
    "p_theta2 = sp.Function('p_theta_2')(t)\n",
    "\n",
    "# Define SymPy equations for the canonical momenta\n",
    "peq1 = sp.Eq(p_theta1, p_theta1_def)\n",
    "peq2 = sp.Eq(p_theta2, p_theta2_def)"
   ],
   "outputs": [],
   "execution_count": 33
  },
  {
   "cell_type": "code",
   "id": "fdc235ccf86c5e8b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.942743Z",
     "start_time": "2024-08-05T11:44:40.935518Z"
    }
   },
   "source": [
    "# Eqn 2\n",
    "display(peq1)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Eq(p_theta_1(t), 7*M1*l1**2*Derivative(theta1(t), t)/12 + M2*l1**2*Derivative(theta1(t), t)/4 + M2*l1*l2*cos(theta1(t) - theta2(t))*Derivative(theta2(t), t)/4)"
      ],
      "text/latex": "$\\displaystyle \\operatorname{p_{\\theta 1}}{\\left(t \\right)} = \\frac{7 M_{1} l_{1}^{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{12} + \\frac{M_{2} l_{1}^{2} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{4} + \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{4}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 34
  },
  {
   "cell_type": "code",
   "id": "550c98cf3abcb08",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.949601Z",
     "start_time": "2024-08-05T11:44:40.943876Z"
    }
   },
   "source": [
    "# Eqn 3\n",
    "display(peq2)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Eq(p_theta_2(t), M2*l1*l2*cos(theta1(t) - theta2(t))*Derivative(theta1(t), t)/4 + 7*M2*l2**2*Derivative(theta2(t), t)/12)"
      ],
      "text/latex": "$\\displaystyle \\operatorname{p_{\\theta 2}}{\\left(t \\right)} = \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta_{1}{\\left(t \\right)}}{4} + \\frac{7 M_{2} l_{2}^{2} \\frac{d}{d t} \\theta_{2}{\\left(t \\right)}}{12}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 35
  },
  {
   "cell_type": "markdown",
   "id": "2c2a1d7c1a6058e1",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 2\n",
    "\n",
    "The Hamiltonian $\\mathcal{H}$ representing the total energy of the system, $T + V$, is the Legendre transform of the Lagrangian given by:\n",
    "\n",
    "$$\n",
    "\\mathcal{H} = \\sum_{i=1}^2  \\dot{\\theta_i} p_{\\theta_i} - \\mathcal{L} \\quad \\tag{4}\n",
    "$$\n",
    "\n",
    "From the Hamiltonian, we aim to derive a set of equations of motion equivalent to the Euler-Lagrange equations:\n",
    "\n",
    "$$\\dot{\\theta_i}=\\frac{\\partial \\mathcal{H}}{\\partial p_{\\theta_i}} \\quad \\tag{5}$$ \n",
    "$$\\dot{p_{\\theta_i}}=-\\frac{\\partial \\mathcal{H}}{\\partial \\theta_i} \\quad \\tag{6}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6cccefcaa54c4764",
   "metadata": {},
   "source": [
    "Equations $(2)$ and $(3)$ can be written in matrix form as:\n",
    "\n",
    "$$\n",
    "\\begin{pmatrix}\n",
    "p_{\\theta_1} \\\\\n",
    "p_{\\theta_2}\n",
    "\\end{pmatrix} \n",
    "= \n",
    "\\mathbf{B} \n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "\\quad \\tag{7}\n",
    "$$\n",
    "\n",
    "where $\\mathbf{B}$ is a $2 \\times 2$ matrix with entries that are functions of $\\theta_1$ and $\\theta_2$:\n",
    "\n",
    "$$\n",
    "\\mathbf{B} = \\begin{pmatrix}\n",
    "\\left(\\frac{7M_1}{12} + \\frac{M_2}{4}\\right)l_1^2 & \\frac{M_2 l_1 l_2 \\cos(\\theta_1(t) - \\theta_2(t))}{4} \\\\\n",
    "\\frac{M_2 l_1 l_2 \\cos(\\theta_1(t) - \\theta_2(t))}{4} & \\frac{7M_2 l_2^2}{12}\n",
    "\\end{pmatrix}\n",
    "$$\n",
    "\n",
    "From equation (7), we can obtain the generalised velocities $\\dot{\\theta_i}$ in terms of the canonical momenta $p_{\\theta_i}$ and the angles $\\theta_i$:\n",
    "\n",
    "$$\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "= \n",
    "\\mathbf{B}^{-1} \n",
    "\\begin{pmatrix}\n",
    "p_{\\theta_1} \\\\\n",
    "p_{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "\\quad \\tag{8}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "id": "fbb89d08ba69d8d6",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.962410Z",
     "start_time": "2024-08-05T11:44:40.951508Z"
    }
   },
   "source": [
    "# Defining the coefficient matrix B\n",
    "B = sp.Matrix([\n",
    "    [sp.Rational(7,12)*M1*l1**2 + sp.Rational(1,4)*M2*l1**2, sp.Rational(1,4)*M2*l1*l2*sp.cos(theta1 - theta2)],\n",
    "    [sp.Rational(1,4)*M2*l1*l2*sp.cos(theta1 - theta2), sp.Rational(7,12)*M2*l2**2]\n",
    "])\n",
    "\n",
    "display(B)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Matrix([\n",
       "[           7*M1*l1**2/12 + M2*l1**2/4, M2*l1*l2*cos(theta1(t) - theta2(t))/4],\n",
       "[M2*l1*l2*cos(theta1(t) - theta2(t))/4,                         7*M2*l2**2/12]])"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}\\frac{7 M_{1} l_{1}^{2}}{12} + \\frac{M_{2} l_{1}^{2}}{4} & \\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{4}\\\\\\frac{M_{2} l_{1} l_{2} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{4} & \\frac{7 M_{2} l_{2}^{2}}{12}\\end{matrix}\\right]$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 36
  },
  {
   "cell_type": "markdown",
   "id": "a2c9a93db9ec596d",
   "metadata": {},
   "source": [
    "We again notice that $\\mathbf{B}^{T}=\\mathbf{B}$\n",
    "\n",
    "Also, where the Lagrangian is given $\\mathcal{L}=T-V$ in eqn $(1)$, by eqn $(7)$ we can write the kinetic energy in terms of the matrix $\\mathbf{B}$ as;\n",
    "\n",
    "$$\n",
    "T = \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} & \\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "\\mathbf{B}\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "= \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} & \\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "\\begin{pmatrix}\n",
    "p_{\\theta_1} \\\\\n",
    "p_{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "= \\frac{1}{2} (\\dot{\\theta_1} p_{\\theta_1} + \\dot{\\theta_2} p_{\\theta_2}) \n",
    "\\quad \\tag{9}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "id": "ddaecfeb08a85f4c",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:40.989658Z",
     "start_time": "2024-08-05T11:44:40.965423Z"
    }
   },
   "source": [
    "# B has determinant:\n",
    "det_B = B.det()\n",
    "display(det_B)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "49*M1*M2*l1**2*l2**2/144 - M2**2*l1**2*l2**2*cos(theta1(t) - theta2(t))**2/16 + 7*M2**2*l1**2*l2**2/48"
      ],
      "text/latex": "$\\displaystyle \\frac{49 M_{1} M_{2} l_{1}^{2} l_{2}^{2}}{144} - \\frac{M_{2}^{2} l_{1}^{2} l_{2}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{16} + \\frac{7 M_{2}^{2} l_{1}^{2} l_{2}^{2}}{48}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 37
  },
  {
   "cell_type": "markdown",
   "id": "ca35f3c431a744cc",
   "metadata": {},
   "source": [
    "A matrix is invertible if and only if its determinant is non-zero\n",
    "\n",
    "Given that $M_1, M_2, l_1, l_1$ are strictly positive and $\\cos^2(x)$ is bounded between $0$ and $1$ for all $x \\in \\mathbb{R}$, $\\mathbf{B}$ is always invertible."
   ]
  },
  {
   "cell_type": "code",
   "id": "4bd630606306e2cf",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:42.130818Z",
     "start_time": "2024-08-05T11:44:40.991776Z"
    }
   },
   "source": [
    "B_inv = B.inv()\n",
    "display(B_inv)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Matrix([\n",
       "[                            84/(49*M1*l1**2 - 9*M2*l1**2*cos(theta1(t) - theta2(t))**2 + 21*M2*l1**2), -36*cos(theta1(t) - theta2(t))/(49*M1*l1*l2 - 9*M2*l1*l2*cos(theta1(t) - theta2(t))**2 + 21*M2*l1*l2)],\n",
       "[-36*cos(theta1(t) - theta2(t))/(49*M1*l1*l2 - 9*M2*l1*l2*cos(theta1(t) - theta2(t))**2 + 21*M2*l1*l2),       (84*M1 + 36*M2)/(49*M1*M2*l2**2 - 9*M2**2*l2**2*cos(theta1(t) - theta2(t))**2 + 21*M2**2*l2**2)]])"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}\\frac{84}{49 M_{1} l_{1}^{2} - 9 M_{2} l_{1}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2} l_{1}^{2}} & - \\frac{36 \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{49 M_{1} l_{1} l_{2} - 9 M_{2} l_{1} l_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2} l_{1} l_{2}}\\\\- \\frac{36 \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{49 M_{1} l_{1} l_{2} - 9 M_{2} l_{1} l_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2} l_{1} l_{2}} & \\frac{84 M_{1} + 36 M_{2}}{49 M_{1} M_{2} l_{2}^{2} - 9 M_{2}^{2} l_{2}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2}^{2} l_{2}^{2}}\\end{matrix}\\right]$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 38
  },
  {
   "cell_type": "code",
   "id": "db3fcb2ec3850203",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:42.139944Z",
     "start_time": "2024-08-05T11:44:42.133114Z"
    }
   },
   "source": [
    "# Define canonical momenta as a matrix\n",
    "p = sp.Matrix([p_theta1, p_theta2])"
   ],
   "outputs": [],
   "execution_count": 39
  },
  {
   "cell_type": "markdown",
   "id": "4b54abc558461c3e",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 2b\n",
    "\n",
    "We derive eqns $(5)$ from eqn $(8)$ which we can later use to check the validity of our Hamiltonian"
   ]
  },
  {
   "cell_type": "code",
   "id": "fe3ba4dcc27d3d16",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:42.159592Z",
     "start_time": "2024-08-05T11:44:42.143055Z"
    }
   },
   "source": [
    "# Define the generalised velocities by eqn(8)\n",
    "omega = B_inv * p\n",
    "omega1_sol, omega2_sol = omega[0], omega[1]"
   ],
   "outputs": [],
   "execution_count": 40
  },
  {
   "cell_type": "code",
   "id": "7e90b41e460c9c51",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:42.176821Z",
     "start_time": "2024-08-05T11:44:42.162347Z"
    }
   },
   "source": [
    "display(omega1_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-36*p_theta_2(t)*cos(theta1(t) - theta2(t))/(49*M1*l1*l2 - 9*M2*l1*l2*cos(theta1(t) - theta2(t))**2 + 21*M2*l1*l2) + 84*p_theta_1(t)/(49*M1*l1**2 - 9*M2*l1**2*cos(theta1(t) - theta2(t))**2 + 21*M2*l1**2)"
      ],
      "text/latex": "$\\displaystyle - \\frac{36 \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{49 M_{1} l_{1} l_{2} - 9 M_{2} l_{1} l_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2} l_{1} l_{2}} + \\frac{84 \\operatorname{p_{\\theta 1}}{\\left(t \\right)}}{49 M_{1} l_{1}^{2} - 9 M_{2} l_{1}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2} l_{1}^{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 41
  },
  {
   "cell_type": "code",
   "id": "92f9a16393e457f9",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:42.187185Z",
     "start_time": "2024-08-05T11:44:42.177855Z"
    }
   },
   "source": [
    "display(omega2_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(84*M1 + 36*M2)*p_theta_2(t)/(49*M1*M2*l2**2 - 9*M2**2*l2**2*cos(theta1(t) - theta2(t))**2 + 21*M2**2*l2**2) - 36*p_theta_1(t)*cos(theta1(t) - theta2(t))/(49*M1*l1*l2 - 9*M2*l1*l2*cos(theta1(t) - theta2(t))**2 + 21*M2*l1*l2)"
      ],
      "text/latex": "$\\displaystyle \\frac{\\left(84 M_{1} + 36 M_{2}\\right) \\operatorname{p_{\\theta 2}}{\\left(t \\right)}}{49 M_{1} M_{2} l_{2}^{2} - 9 M_{2}^{2} l_{2}^{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2}^{2} l_{2}^{2}} - \\frac{36 \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{49 M_{1} l_{1} l_{2} - 9 M_{2} l_{1} l_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2} l_{1} l_{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 42
  },
  {
   "cell_type": "markdown",
   "id": "44b468b6b96f9479",
   "metadata": {},
   "source": [
    "We now apply the trigonometric substitution, $1 - \\cos^2(x) = \\sin^2(x)$ and simplify"
   ]
  },
  {
   "cell_type": "code",
   "id": "d990ad093277201f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:42.668877Z",
     "start_time": "2024-08-05T11:44:42.188093Z"
    }
   },
   "source": [
    "# Simplify omega1_sol substitution for trigonometric identity\n",
    "omega1_sol = sp.simplify(omega1_sol.subs(sp.cos(theta1 - theta2)**2, 1 - sp.sin(theta1 - theta2)**2))\n",
    "display(omega1_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "12*(-3*l1*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*l2*p_theta_1(t))/(l1**2*l2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2))"
      ],
      "text/latex": "$\\displaystyle \\frac{12 \\left(- 3 l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}\\right)}{l_{1}^{2} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 43
  },
  {
   "cell_type": "code",
   "id": "f8b20ec8e19437d1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:43.617055Z",
     "start_time": "2024-08-05T11:44:42.669795Z"
    }
   },
   "source": [
    "# Simplify omega2_sol with substitution for trigonometric identity\n",
    "omega2_sol_simplified = sp.simplify(omega2_sol.subs(sp.cos(theta1 - theta2)**2, 1 - sp.sin(theta1 - theta2)**2))\n",
    "\n",
    "# Explicitly substitute m2 - m2*cos^2(theta1 - theta2) with m2*sin^2(theta1 - theta2)\n",
    "omega2_sol_targeted = omega2_sol_simplified.subs(m2 - m2 * sp.cos(theta1 - theta2)**2, m2 * sp.sin(theta1 - theta2)**2)\n",
    "omega2_sol = sp.simplify(omega2_sol_targeted)\n",
    "display(omega2_sol)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "12*(-3*M2*l2*p_theta_1(t)*cos(theta1(t) - theta2(t)) + l1*(7*M1 + 3*M2)*p_theta_2(t))/(M2*l1*l2**2*(49*M1 - 9*M2*cos(theta1(t) - theta2(t))**2 + 21*M2))"
      ],
      "text/latex": "$\\displaystyle \\frac{12 \\left(- 3 M_{2} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1} \\cdot \\left(7 M_{1} + 3 M_{2}\\right) \\operatorname{p_{\\theta 2}}{\\left(t \\right)}\\right)}{M_{2} l_{1} l_{2}^{2} \\cdot \\left(49 M_{1} - 9 M_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 44
  },
  {
   "cell_type": "markdown",
   "id": "bba47a65dadafaa",
   "metadata": {},
   "source": [
    "Hence, we have defined:\n",
    "\n",
    "$$\\dot{\\theta_1}=\\frac{12 \\left(- 3 l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}\\right)}{l_{1}^{2} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)} \\quad \\tag{10}$$\n",
    "\n",
    "$$\\dot{\\theta_2}=\\frac{12 \\left(- 3 M_{2} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + l_{1} \\cdot \\left(7 M_{1} + 3 M_{2}\\right) \\operatorname{p_{\\theta 2}}{\\left(t \\right)}\\right)}{M_{2} l_{1} l_{2}^{2} \\cdot \\left(49 M_{1} - 9 M_{2} \\cos^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 21 M_{2}\\right)} \\quad \\tag{11}$$\n",
    "\n",
    "Which we could substitute into equation $(4)$ to yield the Hamiltonian"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2f0881a9621749c1",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 3\n",
    "\n",
    "Let's circumvent the complexity of deriving equation $(4)$ from $(10)$ & $(11)$ and derive the Hamiltonian from $H=T+V$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b29801a183744887",
   "metadata": {},
   "source": [
    "Starting from eqn $(8)$ using the transposed relation;\n",
    "\n",
    "$$\n",
    "\\begin{pmatrix}\n",
    "\\dot{\\theta_1} \\\\\n",
    "\\dot{\\theta_2}\n",
    "\\end{pmatrix}\n",
    "= \\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix}\n",
    "\\left(\\mathbf{B}^{-1}\\right)^T = \\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix} \\mathbf{B}^{-1},\n",
    "$$\n",
    "\n",
    "By eqn $(9)$ we can write the kinetic energy, in the form\n",
    "\n",
    "$$\n",
    "T = \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix}\n",
    "\\mathbf{B}^{-1}\n",
    "\\begin{pmatrix}\n",
    "p_1 \\\\\n",
    "p_2\n",
    "\\end{pmatrix}.\n",
    "$$\n",
    "\n",
    "Now the Hamiltonian function becomes,\n",
    "\n",
    "$$\n",
    "\\mathcal{H} = T + V = \\frac{1}{2}\n",
    "\\begin{pmatrix}\n",
    "p_1 & p_2\n",
    "\\end{pmatrix}\n",
    "\\mathbf{B}^{-1}\n",
    "\\begin{pmatrix}\n",
    "p_1 \\\\\n",
    "p_2\n",
    "\\end{pmatrix}\n",
    "-g\\left(\\frac{ \\left(M_{1} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}\\right)}{2}\\right)\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "id": "e3b0837406e6042c",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:44.422079Z",
     "start_time": "2024-08-05T11:44:43.617965Z"
    }
   },
   "source": [
    "# Kinetic energy T expressed in terms of canonical momenta\n",
    "T = sp.Rational(1,2) * p.T * B_inv * p\n",
    "\n",
    "# Simplify T, apply trig ident\n",
    "T_target = T[0].subs(sp.cos(theta1 - theta2)**2, 1 -  sp.sin(theta1 - theta2)**2)\n",
    "T_simp = sp.simplify(T_target)\n",
    "display(T_simp)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "6*(7*M1*l1**2*p_theta_2(t)**2 + 3*M2*l1**2*p_theta_2(t)**2 - 6*M2*l1*l2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*M2*l2**2*p_theta_1(t)**2)/(M2*l1**2*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2))"
      ],
      "text/latex": "$\\displaystyle \\frac{6 \\cdot \\left(7 M_{1} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + 3 M_{2} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 6 M_{2} l_{1} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 M_{2} l_{2}^{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right)}{M_{2} l_{1}^{2} l_{2}^{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 45
  },
  {
   "cell_type": "code",
   "id": "cac4d147370dfc99",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:44.428888Z",
     "start_time": "2024-08-05T11:44:44.423124Z"
    }
   },
   "source": [
    "# Define the potential energy V \n",
    "V = -sp.Rational(1, 2) * g * (M1 * l1 * sp.cos(theta1) + M2 * l1 * sp.cos(theta1) + M2 * l2 * sp.cos(theta2))\n",
    "display(V)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-g*(M1*l1*cos(theta1(t)) + M2*l1*cos(theta1(t)) + M2*l2*cos(theta2(t)))/2"
      ],
      "text/latex": "$\\displaystyle - \\frac{g \\left(M_{1} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}\\right)}{2}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 46
  },
  {
   "cell_type": "code",
   "id": "7d48946cbd6c6b3",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:44.439479Z",
     "start_time": "2024-08-05T11:44:44.429932Z"
    }
   },
   "source": [
    "# Return the Hamiltonian \n",
    "H = T_simp + V\n",
    "display(H)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-g*(M1*l1*cos(theta1(t)) + M2*l1*cos(theta1(t)) + M2*l2*cos(theta2(t)))/2 + 6*(7*M1*l1**2*p_theta_2(t)**2 + 3*M2*l1**2*p_theta_2(t)**2 - 6*M2*l1*l2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*M2*l2**2*p_theta_1(t)**2)/(M2*l1**2*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2))"
      ],
      "text/latex": "$\\displaystyle - \\frac{g \\left(M_{1} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{1} \\cos{\\left(\\theta_{1}{\\left(t \\right)} \\right)} + M_{2} l_{2} \\cos{\\left(\\theta_{2}{\\left(t \\right)} \\right)}\\right)}{2} + \\frac{6 \\cdot \\left(7 M_{1} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + 3 M_{2} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 6 M_{2} l_{1} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 M_{2} l_{2}^{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right)}{M_{2} l_{1}^{2} l_{2}^{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 47
  },
  {
   "cell_type": "markdown",
   "id": "31b1d51fc3b34372",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 4\n",
    "\n",
    "#### Forming the first order system\n",
    "\n",
    "Partially differentiating $\\mathcal{H}$ w.r.t $p_{\\theta_i}$ should return eqns $(10)$ and $(11)$ "
   ]
  },
  {
   "cell_type": "code",
   "id": "ab262173fb79743a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:44.802932Z",
     "start_time": "2024-08-05T11:44:44.440439Z"
    }
   },
   "source": [
    "# Partially differentiate H with respect to p_theta1 and p_theta2\n",
    "H_theta1 = sp.diff(H, p_theta1)\n",
    "H_theta1 = sp.simplify(H_theta1)\n",
    "display(H_theta1)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "12*(-3*l1*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*l2*p_theta_1(t))/(l1**2*l2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2))"
      ],
      "text/latex": "$\\displaystyle \\frac{12 \\left(- 3 l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}\\right)}{l_{1}^{2} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 48
  },
  {
   "cell_type": "code",
   "id": "42ff9cad70e566",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:45.282455Z",
     "start_time": "2024-08-05T11:44:44.804320Z"
    }
   },
   "source": [
    "H_theta2 = sp.diff(H, p_theta2)\n",
    "H_theta2 = sp.simplify(H_theta2)\n",
    "display(H_theta2)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "12*(7*M1*l1*p_theta_2(t) + 3*M2*l1*p_theta_2(t) - 3*M2*l2*p_theta_1(t)*cos(theta1(t) - theta2(t)))/(M2*l1*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2))"
      ],
      "text/latex": "$\\displaystyle \\frac{12 \\cdot \\left(7 M_{1} l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} + 3 M_{2} l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} - 3 M_{2} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}{M_{2} l_{1} l_{2}^{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 49
  },
  {
   "cell_type": "markdown",
   "id": "116d94b57dc656c8",
   "metadata": {},
   "source": [
    "Which are indeed equivalent to eqns $(10)$ and $(11)$ \n",
    "\n",
    "Deriving the equations for $\\dot{p_{\\theta_1}}$ and $\\dot{p_{\\theta_2}}$"
   ]
  },
  {
   "cell_type": "code",
   "id": "3e9c80d9ef340202",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:52.239822Z",
     "start_time": "2024-08-05T11:44:45.283413Z"
    }
   },
   "source": [
    "H_p_theta1 = -sp.diff(H, theta1)\n",
    "H_p_theta1 = sp.simplify(H_p_theta1)\n",
    "display(H_p_theta1)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-g*l1**3*l2**2*(M1 + M2)*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2*sin(theta1(t)) - 72*l1*l2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) + 216*(7*M1*l1**2*p_theta_2(t)**2 + 3*M2*l1**2*p_theta_2(t)**2 - 6*M2*l1*l2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*M2*l2**2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(2*l1**2*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2)"
      ],
      "text/latex": "$\\displaystyle \\frac{- g l_{1}^{3} l_{2}^{2} \\left(M_{1} + M_{2}\\right) \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2} \\sin{\\left(\\theta_{1}{\\left(t \\right)} \\right)} - 72 l_{1} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 216 \\cdot \\left(7 M_{1} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + 3 M_{2} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 6 M_{2} l_{1} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 M_{2} l_{2}^{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{2 l_{1}^{2} l_{2}^{2} \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 50
  },
  {
   "cell_type": "code",
   "id": "59adb941e5940f53",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:57.275466Z",
     "start_time": "2024-08-05T11:44:52.240732Z"
    }
   },
   "source": [
    "H_p_theta2 = -sp.diff(H, theta2)\n",
    "H_p_theta2 = sp.simplify(H_p_theta2)\n",
    "display(H_p_theta2)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-M2*g*l1**2*l2**3*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2*sin(theta2(t)) + 72*l1*l2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) - 216*(7*M1*l1**2*p_theta_2(t)**2 + 3*M2*l1**2*p_theta_2(t)**2 - 6*M2*l1*l2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*M2*l2**2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(2*l1**2*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2)"
      ],
      "text/latex": "$\\displaystyle \\frac{- M_{2} g l_{1}^{2} l_{2}^{3} \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2} \\sin{\\left(\\theta_{2}{\\left(t \\right)} \\right)} + 72 l_{1} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} - 216 \\cdot \\left(7 M_{1} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + 3 M_{2} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 6 M_{2} l_{1} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 M_{2} l_{2}^{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{2 l_{1}^{2} l_{2}^{2} \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2}}$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 51
  },
  {
   "cell_type": "code",
   "id": "1918b969ee1261db",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:57.298761Z",
     "start_time": "2024-08-05T11:44:57.276578Z"
    }
   },
   "source": [
    "# Declare all as SymPy equations\n",
    "Heq1 = sp.Eq(omega1, H_theta1)\n",
    "Heq2 = sp.Eq(omega2, H_theta2)\n",
    "Heq3 = sp.Eq(sp.diff(p_theta1, t), H_p_theta1)\n",
    "Heq4 = sp.Eq(sp.diff(p_theta2, t), H_p_theta2)"
   ],
   "outputs": [],
   "execution_count": 52
  },
  {
   "cell_type": "markdown",
   "id": "e0613190220135ce",
   "metadata": {},
   "source": [
    "----\n",
    "&nbsp;\n",
    "### Step 5\n",
    "\n",
    "#### Declare matrix equations"
   ]
  },
  {
   "cell_type": "code",
   "id": "1a7684085bb3bc82",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-08-05T11:44:57.359976Z",
     "start_time": "2024-08-05T11:44:57.300153Z"
    }
   },
   "source": [
    "LHS_FIRST = sp.Matrix([[Heq1.lhs], [Heq2.lhs], [Heq3.lhs], [Heq4.lhs]])\n",
    "RHS_FIRST = sp.Matrix([[Heq1.rhs], [Heq2.rhs], [Heq3.rhs], [Heq4.rhs]])\n",
    "\n",
    "MAT_EQ = sp.Eq(LHS_FIRST, RHS_FIRST)\n",
    "display(MAT_EQ)"
   ],
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Eq(Matrix([\n",
       "[   Derivative(theta1(t), t)],\n",
       "[   Derivative(theta2(t), t)],\n",
       "[Derivative(p_theta_1(t), t)],\n",
       "[Derivative(p_theta_2(t), t)]]), Matrix([\n",
       "[                                                                                                                                                                                                                                                                                                                                                                             12*(-3*l1*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*l2*p_theta_1(t))/(l1**2*l2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2))],\n",
       "[                                                                                                                                                                                                                                                                                                                                              12*(7*M1*l1*p_theta_2(t) + 3*M2*l1*p_theta_2(t) - 3*M2*l2*p_theta_1(t)*cos(theta1(t) - theta2(t)))/(M2*l1*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2))],\n",
       "[(-g*l1**3*l2**2*(M1 + M2)*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2*sin(theta1(t)) - 72*l1*l2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) + 216*(7*M1*l1**2*p_theta_2(t)**2 + 3*M2*l1**2*p_theta_2(t)**2 - 6*M2*l1*l2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*M2*l2**2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(2*l1**2*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2)],\n",
       "[       (-M2*g*l1**2*l2**3*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2*sin(theta2(t)) + 72*l1*l2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)*p_theta_1(t)*p_theta_2(t)*sin(theta1(t) - theta2(t)) - 216*(7*M1*l1**2*p_theta_2(t)**2 + 3*M2*l1**2*p_theta_2(t)**2 - 6*M2*l1*l2*p_theta_1(t)*p_theta_2(t)*cos(theta1(t) - theta2(t)) + 7*M2*l2**2*p_theta_1(t)**2)*sin(theta1(t) - theta2(t))*cos(theta1(t) - theta2(t)))/(2*l1**2*l2**2*(49*M1 + 9*M2*sin(theta1(t) - theta2(t))**2 + 12*M2)**2)]]))"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}\\frac{d}{d t} \\theta_{1}{\\left(t \\right)}\\\\\\frac{d}{d t} \\theta_{2}{\\left(t \\right)}\\\\\\frac{d}{d t} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}\\\\\\frac{d}{d t} \\operatorname{p_{\\theta 2}}{\\left(t \\right)}\\end{matrix}\\right] = \\left[\\begin{matrix}\\frac{12 \\left(- 3 l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)}\\right)}{l_{1}^{2} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)}\\\\\\frac{12 \\cdot \\left(7 M_{1} l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} + 3 M_{2} l_{1} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} - 3 M_{2} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}\\right)}{M_{2} l_{1} l_{2}^{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)}\\\\\\frac{- g l_{1}^{3} l_{2}^{2} \\left(M_{1} + M_{2}\\right) \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2} \\sin{\\left(\\theta_{1}{\\left(t \\right)} \\right)} - 72 l_{1} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 216 \\cdot \\left(7 M_{1} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + 3 M_{2} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 6 M_{2} l_{1} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 M_{2} l_{2}^{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{2 l_{1}^{2} l_{2}^{2} \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2}}\\\\\\frac{- M_{2} g l_{1}^{2} l_{2}^{3} \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2} \\sin{\\left(\\theta_{2}{\\left(t \\right)} \\right)} + 72 l_{1} l_{2} \\cdot \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right) \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} - 216 \\cdot \\left(7 M_{1} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} + 3 M_{2} l_{1}^{2} \\operatorname{p_{\\theta 2}}^{2}{\\left(t \\right)} - 6 M_{2} l_{1} l_{2} \\operatorname{p_{\\theta 1}}{\\left(t \\right)} \\operatorname{p_{\\theta 2}}{\\left(t \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 7 M_{2} l_{2}^{2} \\operatorname{p_{\\theta 1}}^{2}{\\left(t \\right)}\\right) \\sin{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} \\cos{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)}}{2 l_{1}^{2} l_{2}^{2} \\left(49 M_{1} + 9 M_{2} \\sin^{2}{\\left(\\theta_{1}{\\left(t \\right)} - \\theta_{2}{\\left(t \\right)} \\right)} + 12 M_{2}\\right)^{2}}\\end{matrix}\\right]$"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 53
  },
  {
   "cell_type": "markdown",
   "id": "790756ab43806824",
   "metadata": {},
   "source": [
    "### This completes the derivation of both models"
   ]
  }
 ],
 "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.9.12"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}