{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Double pendulum"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "lang": "en"
   },
   "source": [
    "Consider the pendulum suspended on the pendulum. We weill use dAlembert principle to derive eqaution of motion in generalized coordinaes. Naturally, we choose two angles as coordinates which comply with contraints."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "var('t')\n",
    "var('l1 l2 m1 m2 g')\n",
    "\n",
    "xy_names = [('x1','x1'),('y1','y1'),('x2','x2'),('y2','y2')]\n",
    "uv_names = [('phi1','\\\\varphi_1'),('phi2','\\\\varphi_2')]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "load('cas_utils.sage')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "to_fun, to_var = make_symbols(xy_names,uv_names)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x2u = {x1:l1*sin(phi1),\\\n",
    "       y1:-l1*cos(phi1),\\\n",
    "       x2:l1*sin(phi1)+l2*sin(phi2),\\\n",
    "       y2:-l1*cos(phi1)-l2*cos(phi2)}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "transform_virtual_displacements(xy_names,uv_names,verbose=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "dAlemb = (m1*x1.subs(x2u).subs(to_fun).diff(t,2))*dx1_polar + \\\n",
    "         (m1*y1.subs(x2u).subs(to_fun).diff(t,2)+m1*g)*dy1_polar+\\\n",
    "         (m2*x2.subs(x2u).subs(to_fun).diff(t,2))*dx2_polar + \\\n",
    "         (m2*y2.subs(x2u).subs(to_fun).diff(t,2)+m2*g)*dy2_polar\n",
    "dAlemb = dAlemb.subs(to_var)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath(dAlemb)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "eq1 = dAlemb.expand().coefficient(dphi1).trig_simplify()\n",
    "eq2 = dAlemb.expand().coefficient(dphi2).trig_simplify()\n",
    "showmath(eq1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath(eq2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "sol =  solve([eq1,eq2],[phi1dd,phi2dd])[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath(sol)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath( sol[0].rhs().denominator() )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "(l1*(2*m1+m2-m2*cos(2*phi1-2*phi2))).expand_trig().expand_trig().expand().show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bool ( -2*sol[0].rhs().denominator()==(l1*(2*m1+m2-m2*cos(2*phi1-2*phi2))).expand_trig().expand_trig().expand() )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "lang": "en"
   },
   "source": [
    "\n",
    "Since the \"textbook\" solution contains a slightly different form, let's check if we have these formulas:\n",
    "\n",
    "$$T(\\varphi_1,\\varphi_2,\\dot{\\varphi}_1,\\dot{\\varphi}_2) = \\frac{m_1}{2} l_1^2 \\dot{\\varphi}_1^2 + \\frac{m_2}{2} \\left( l_1^2 \\dot{\\varphi}_1^2 + l_2^2 \\dot{\\varphi}_2^2 + 2 l_1 l_2 \\dot{\\varphi}_1 \\dot{\\varphi}_2 \\cos(\\varphi_1-\\varphi_2) \\right)$$ $$V(\\varphi_1,\\varphi_2) = -(m_1+m_2) g l_1 \\cos\\varphi_1 - m_2 g l_2 \\cos\\varphi_2$$\n",
    "$$m_{2}l_{2}\\ddot{\\varphi}_{2}\\cos\\left(\\varphi_{1}-\\varphi_{2}\\right)+\\left(m_{1}+m_{2}\\right)l_{1}\\ddot{\\varphi}_{1}+m_{2}l_{2}\\dot{\\varphi}_{2}^{2}\\sin\\left(\\varphi_{1}-\\varphi_{2}\\right)+\\left(m_{1}+m_{2}\\right)g\\sin\\varphi_{1}=0$$\n",
    "$$l_{2}\\ddot{\\varphi}_{2}+l_{1}\\ddot{\\varphi}_{1}\\cos\\left(\\varphi_{1}-\\varphi_{2}\\right)-l_{1}\\dot{\\varphi}_{1}^{2}\\sin\\left(\\varphi_{1}-\\varphi_{2}\\right)+g\\sin\\varphi_{2}=0$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rown_wiki = [m2*l2*cos(phi1-phi2)*phi2dd+(m1+m2)*l1*phi1dd+m2*l2*phi2d^2 * sin(phi1-phi2)+ (m1+m2)*g*sin(phi1),\\\n",
    " l2*phi2dd+l1*cos(phi1-phi2)*phi1dd-l1*phi1d^2*sin(phi1-phi2)+g*sin(phi2)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath(rown_wiki[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath(rown_wiki[1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rown_wiki[0].show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "(eq1/l1).reduce_trig().show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rown_wiki[0].show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bool((eq1/l1) == rown_wiki[0] )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "(eq2/l2/m2).reduce_trig().show()\n",
    "rown_wiki[1].show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bool((eq2/l2/m2) == rown_wiki[1] )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Euler -Langrange\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Ekin = 1/2*(m1*x1.subs(x2u).subs(to_fun).diff(t).subs(to_var)^2+\\\n",
    "m1*y1.subs(x2u).subs(to_fun).diff(t).subs(to_var)^2+\\\n",
    "m2*x2.subs(x2u).subs(to_fun).diff(t).subs(to_var)^2+\\\n",
    "m2*y2.subs(x2u).subs(to_fun).diff(t).subs(to_var)^2 )\n",
    "\n",
    "Epot = m1*g*y1.subs(x2u)+m2*g*y2.subs(x2u)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath( Epot.collect(cos(phi1)) )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath( Epot )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "showmath( Ekin.trig_simplify() )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "L = Ekin - Epot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "len(L.expand().operands())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "EL1 = L.diff(phi1d).subs(to_fun).diff(t).subs(to_var) - L.diff(phi1)\n",
    "EL2 = L.diff(phi2d).subs(to_fun).diff(t).subs(to_var) - L.diff(phi2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "EL1.expand().operands()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "EL1 = (EL1/l1).trig_reduce()\n",
    "EL2 = (EL2/l2).trig_reduce()\n",
    "showmath(EL1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "sol =  solve([EL1,EL2],[phi1dd,phi2dd])[0]\n",
    "show(sol)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "expr = sol[0].rhs()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for ex_ in expr.factor().numerator().operands():\n",
    "    show(ex_)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Numerical analysis\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np \n",
    "\n",
    "ode =  [phi1d,phi2d]+[sol[0].rhs(),sol[1].rhs()]\n",
    "ode = map(lambda x:x.subs({l1:1,l2:1,m1:1,m2:1,g:9.81}),ode)\n",
    "\n",
    "times = srange(0,5,.01)\n",
    "numsol = desolve_odeint(ode,[2.1,0,0,0],times,[phi1,phi2,phi1d,phi2d])\n",
    "p = line ( zip(np.sin(numsol[:,0])+np.sin(numsol[:,1]),\\\n",
    "               -np.cos(numsol[:,0])-np.cos(numsol[:,1])), color='gray' )\n",
    "p.show(figsize=4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_dp(f1,f2,pars):\n",
    "    mass1 = vector([x1,y1]).subs(x2u).subs(pars).subs({phi1:f1,phi2:f2})\n",
    "    mass2 = vector([x2,y2]).subs(x2u).subs(pars).subs({phi1:f1,phi2:f2})\n",
    "    plt = point([(0,0),mass1],aspect_ratio=1,size=40)\n",
    "    plt += point(mass2,xmin=-2,xmax=2,ymin=-2,ymax=2,size=40)\n",
    "    plt += line([(0,0),mass1,mass2],color='gray')\n",
    "\n",
    "    return plt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_dp(numsol[213,0],numsol[213,1],{l1:1,l2:1})+p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "@interact\n",
    "def _(ith=slider(0,numsol.shape[0]-1)):\n",
    "    f1,f2 = numsol[ith,:2]\n",
    "    plot_dp(f1,f2,{l1:1,l2:1}).show(axes=False)\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\\newpage"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "SageMath 8.6",
   "language": "",
   "name": "sagemath"
  },
  "language": "python",
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.15"
  },
  "nbTranslate": {
   "displayLangs": [
    "en",
    "pl"
   ],
   "hotkey": "alt-t",
   "langInMainMenu": true,
   "sourceLang": "pl",
   "targetLang": "en",
   "useGoogleTranslate": true
  },
  "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": 2
}