Deriving inverted pendulum equations of motion two different ways

pendulumcart.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Two ways of deriving pendulum-cart system equations of motion"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1: Lagrange's method, by hand"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "import sympy\n",
    "from sympy import sin,cos, symbols\n",
    "from sympy.physics.mechanics import dynamicsymbols\n",
    "\n",
    "from sympy.physics.vector import init_vprinting\n",
    "init_vprinting(use_latex='mathjax', pretty_print=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### define useful variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$t$$"
      ],
      "text/plain": [
       "t"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# time\n",
    "\n",
    "t = sympy.Symbol(\"t\")\n",
    "t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( x, \\quad \\theta\\right )$$"
      ],
      "text/plain": [
       "(x, theta)"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# generalized coordinates\n",
    "\n",
    "# x is cart rightward from O, theta is pendulum up, counter-clockwise\n",
    "x, theta = dynamicsymbols('x theta')\n",
    "x,theta"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( \\dot{x}, \\quad \\dot{\\theta}\\right )$$"
      ],
      "text/plain": [
       "(x', theta')"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# generalized speeds\n",
    "\n",
    "v,omega = dynamicsymbols('x theta',1)\n",
    "v,omega"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( m, \\quad M, \\quad l, \\quad g\\right )$$"
      ],
      "text/plain": [
       "(m, M, l, g)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# constants\n",
    "\n",
    "m,M,l,g = symbols(\"m M l g\")\n",
    "m,M,l,g"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### define kinetic energy T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "v_mx = v - cos(theta)*l*omega\n",
    "v_my = -sin(theta)*l*omega"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$l^{2} \\operatorname{sin}^{2}\\left(\\theta\\right) \\dot{\\theta}^{2} + \\left(- l \\operatorname{cos}\\left(\\theta\\right) \\dot{\\theta} + \\dot{x}\\right)^{2}$$"
      ],
      "text/plain": [
       "l**2*sin(theta)**2*theta'**2 + (-l*cos(theta)*theta' + x')**2"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v_m_sq = v_mx**2 + v_my**2\n",
    "v_m_sq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "T = sympy.Rational(1,2)*M*v**2 + sympy.Rational(1,2)*m*v_m_sq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\frac{M \\dot{x}^{2}}{2} + \\frac{m}{2} \\left(l^{2} \\operatorname{sin}^{2}\\left(\\theta\\right) \\dot{\\theta}^{2} + \\left(- l \\operatorname{cos}\\left(\\theta\\right) \\dot{\\theta} + \\dot{x}\\right)^{2}\\right)$$"
      ],
      "text/plain": [
       "M*x'**2/2 + m*(l**2*sin(theta)**2*theta'**2 + (-l*cos(theta)*theta' + x')**2)/2"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### define potential energy V"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "V = m*g*l*sympy.cos(theta)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$g l m \\operatorname{cos}\\left(\\theta\\right)$$"
      ],
      "text/plain": [
       "g*l*m*cos(theta)"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "V"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### define lagrangian"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\frac{M \\dot{x}^{2}}{2} - g l m \\operatorname{cos}\\left(\\theta\\right) + \\frac{m}{2} \\left(l^{2} \\operatorname{sin}^{2}\\left(\\theta\\right) \\dot{\\theta}^{2} + \\left(- l \\operatorname{cos}\\left(\\theta\\right) \\dot{\\theta} + \\dot{x}\\right)^{2}\\right)$$"
      ],
      "text/plain": [
       "M*x'**2/2 - g*l*m*cos(theta) + m*(l**2*sin(theta)**2*theta'**2 + (-l*cos(theta)*theta' + x')**2)/2"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "L = T-V\n",
    "L"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### use the Euler-Langrange equations to find equations of motion"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$l m \\operatorname{sin}\\left(\\theta\\right) \\dot{\\theta}^{2} - l m \\operatorname{cos}\\left(\\theta\\right) \\ddot{\\theta} + \\left(M + m\\right) \\ddot{x}$$"
      ],
      "text/plain": [
       "l*m*sin(theta)*theta'**2 - l*m*cos(theta)*theta'' + (M + m)*x''"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#this equals F\n",
    "(L.diff(v).diff(t) - L.diff(x)).expand().collect(v)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$l m \\left(- g \\operatorname{sin}\\left(\\theta\\right) + l \\ddot{\\theta} - \\operatorname{cos}\\left(\\theta\\right) \\ddot{x}\\right)$$"
      ],
      "text/plain": [
       "l*m*(-g*sin(theta) + l*theta'' - cos(theta)*x'')"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#this equals 0\n",
    "(L.diff(omega).diff(t) - L.diff(theta)).simplify()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kane's Method, using Sympy API"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from sympy import symbols, Matrix\n",
    "from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame\n",
    "from sympy.physics.mechanics import Point, Particle, KanesMethod\n",
    "import numpy as np\n",
    "\n",
    "from sympy.physics.vector import init_vprinting\n",
    "init_vprinting(use_latex='mathjax', pretty_print=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( \\theta, \\quad v, \\quad \\omega\\right )$$"
      ],
      "text/plain": [
       "(theta, v, omega)"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "theta, v, omega = dynamicsymbols('theta v omega')\n",
    "theta, v, omega"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( m, \\quad M, \\quad l, \\quad g\\right )$$"
      ],
      "text/plain": [
       "(m, M, l, g)"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "m, M, l, g = symbols('m M l g')\n",
    "m, M, l, g"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(P, I)"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# inertial reference frame\n",
    "I = ReferenceFrame('I')\n",
    "\n",
    "# pendulum reference frame\n",
    "P = ReferenceFrame('P')\n",
    "P,I"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "P.orient(I, 'Axis', (theta, I.z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(B, C)"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# inertial frame origin. this isn't required for finding equations of motion.\n",
    "#O = Point('O') \n",
    "B = Point('B') #pendulum bob\n",
    "C = Point('C') #pendulum cart\n",
    "B,C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$v\\mathbf{\\hat{i}_x} -  l \\dot{\\theta}\\mathbf{\\hat{p}_x}$$"
      ],
      "text/plain": [
       "v*I.x - l*theta'*P.x"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C.set_vel(I, v * I.x)\n",
    "\n",
    "B.set_pos(C, l*P.y)\n",
    "B.v2pt_theory(C, I, P)\n",
    "\n",
    "# optionally: define the origin point and find where the pendulum bob is\n",
    "#O.set_vel(I,0)\n",
    "#C.set_pos(O, x * I.x)\n",
    "#B.pos_from(O).express(I)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left [ \\omega - \\dot{\\theta}\\right ]$$"
      ],
      "text/plain": [
       "[omega - theta']"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "kd = [omega - theta.diff()]\n",
    "kd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "F = dynamicsymbols(\"F\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[(B, - g*m*I.y), (C, F*I.x)]"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "FL = [(B, -g*m*I.y),\n",
    "      (C, F*I.x)]\n",
    "FL"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#define a body, in this case a particle. It's at P and has a mass m.\n",
    "bob = Particle('bob', B, m)\n",
    "cart = Particle('cart', C, M)\n",
    "BL = [bob, cart]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# set up kane's method with frame, coordinates, speeds, and kinematical equations\n",
    "KM = KanesMethod(I, q_ind=[theta], u_ind=[v,omega], kd_eqs=kd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# find kane's equations given forces and bodies\n",
    "(fr, frstar) = KM.kanes_equations(FL, BL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}- l m \\omega^{2} \\operatorname{sin}\\left(\\theta\\right) + l m \\operatorname{cos}\\left(\\theta\\right) \\dot{\\omega} - \\left(M + m\\right) \\dot{v} + F\\\\g l m \\operatorname{sin}\\left(\\theta\\right) - l^{2} m \\dot{\\omega} + l m \\operatorname{cos}\\left(\\theta\\right) \\dot{v}\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-l*m*omega**2*sin(theta) + l*m*cos(theta)*omega' - (M + m)*v' + F],\n",
       "[             g*l*m*sin(theta) - l**2*m*omega' + l*m*cos(theta)*v']])"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# fun fact: fr + frstart = 0\n",
    "fr + frstar"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Hearteningly, this is the same result as the first section."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "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.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}