{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Mass-spring-damper\n",
    "\n",
    "In this tutorial, we will describe the mechanics and control of the one degree of freedom translational mass-spring-damper system subject to a control input force. We will first derive the dynamic equations by hand. Then, we will derive them using the `sympy.mechanics` python package.\n",
    "\n",
    "The system on which we will work is depicted below:\n",
    "\n",
    "![mass-spring-damper system](http://ctms.engin.umich.edu/CTMS/Content/Introduction/System/Modeling/figures/mass_spring_damper.png)\n",
    "\n",
    "Note that in what follows, we use the notation $u(t) = F$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Mechanics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Deriving the dynamical equations by hand\n",
    "\n",
    "#### 1.1 By using Newton equations\n",
    "\n",
    "Using Newton's law, we have:\n",
    "\n",
    "\\begin{align}\n",
    "    m \\ddot{x}(t) &= \\sum F_{ext} \\\\\n",
    "    &= - b \\dot{x}(t) - k x(t) + u(t)\n",
    "\\end{align}\n",
    "\n",
    "#### 1.2 By using the Lagrange Method\n",
    "\n",
    "Let's first derive the kinematic and potential energies.\n",
    "\n",
    "\\begin{equation}\n",
    "    T = \\frac{1}{2} m \\dot{x} \\\\\n",
    "    V = - \\int \\vec{F} . \\vec{dl} = - \\int (-kx \\vec{1_x}) . dx \\vec{1_x} = \\frac{k x^2}{2}\n",
    "\\end{equation}\n",
    "\n",
    "The Lagrangian is then given by:\n",
    "\\begin{equation}\n",
    "    \\mathcal{L} = T - V = \\frac{1}{2} m \\dot{x} - \\frac{k x^2}{2}\n",
    "\\end{equation}\n",
    "\n",
    "Using the Lagrange's equations we can derive the dynamics of the system:\n",
    "\n",
    "\\begin{equation}\n",
    "    \\frac{d}{dt} \\frac{\\partial \\mathcal{L}}{\\partial \\dot{q}} - \\frac{\\partial \\mathcal{L}}{\\partial q} = Q\n",
    "\\end{equation}\n",
    "\n",
    "where $q$ are the generalized coordinates (in this case $x$), and $Q$ represents the non-conservative forces (input force, dragging or friction forces, etc).\n",
    "\n",
    "* $\\frac{d}{dt} \\frac{\\partial \\mathcal{L}}{\\partial \\dot{x}} = \\frac{d}{dt} m \\dot{x}(t) = m \\ddot{x}(t) $\n",
    "* $\\frac{\\partial \\mathcal{L}}{\\partial x} = - k x(t) $\n",
    "* $Q = - b \\dot{x}(t) + u(t) $\n",
    "\n",
    "which when putting everything back together gives us:\n",
    "\n",
    "\\begin{equation}\n",
    "    m \\ddot{x}(t) + b \\dot{x}(t) + k x(t) = u(t)\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Deriving the dynamical equations using sympy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "import sympy.physics.mechanics as mechanics\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import pprint"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡                              2             ⎤\n",
      "⎢  d                          d              ⎥\n",
      "⎢b⋅──(q(t)) + 1.0⋅k⋅q(t) + m⋅───(q(t)) - u(t)⎥\n",
      "⎢  dt                          2             ⎥\n",
      "⎣                            dt              ⎦\n"
     ]
    }
   ],
   "source": [
    "# define variables \n",
    "q = mechanics.dynamicsymbols('q')\n",
    "dq = mechanics.dynamicsymbols('q', 1)\n",
    "u = mechanics.dynamicsymbols('u')\n",
    "\n",
    "# define constants\n",
    "m, k, b = sympy.symbols('m k b')\n",
    "\n",
    "# define the inertial frame\n",
    "N = mechanics.ReferenceFrame('N')\n",
    "\n",
    "# define a particle for the mass\n",
    "P = mechanics.Point('P')\n",
    "P.set_vel(N, dq * N.x) # go in the x direction\n",
    "Pa = mechanics.Particle('Pa', P, m)\n",
    "\n",
    "# define the potential energy for the particle (the kinematic one is derived automatically)\n",
    "Pa.potential_energy = k * q**2 / 2.0\n",
    "\n",
    "# define the Lagrangian and the non-conservative force applied on the point P\n",
    "L = mechanics.Lagrangian(N, Pa)\n",
    "force = [(P, -b * dq * N.x + u * N.x)]\n",
    "\n",
    "# Lagrange equations \n",
    "lagrange = mechanics.LagrangesMethod(L, [q], forcelist = force, frame = N)\n",
    "pprint(lagrange.form_lagranges_equations())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Laplace transform and transfer function"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Applying the Laplace transform on the dynamic equation:\n",
    "\n",
    "\\begin{equation}\n",
    "    m \\ddot{x}(t) + b \\dot{x}(t) + k x(t) = u(t) \\stackrel{L}{\\rightarrow} m s^2 X(s) + b s X(s) + k X(s) = U(s)\n",
    "\\end{equation}\n",
    "\n",
    "The transfer equation is given by:\n",
    "\n",
    "\\begin{equation}\n",
    "    H(s) = \\frac{X(s)}{U(s)} = \\frac{1}{m s^2 + b s + k}\n",
    "\\end{equation}\n",
    "\n",
    "By calculating the pole:\n",
    "\n",
    "\\begin{equation}\n",
    "    m s^2 + b s + k = 0 \\Leftrightarrow s = \\frac{-b}{2m} \\pm \\sqrt{\\left(\\frac{b}{2m}\\right)^2 - \\frac{k}{m}}\n",
    "\\end{equation}\n",
    "\n",
    "Note that $b, k, m > 0$ because they represent real physical quantities."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### LTI system\n",
    "\n",
    "We can rewrite the above equation as a first-order system of equations. Let's first define the state vector $\\pmb{x} = \\left[ \\begin{array}{c} x(t) \\\\ \\dot{x}(t) \\end{array} \\right]$ and the control vector $\\pmb{u} = \\left[ \\begin{array}{c} u(t) \\end{array} \\right]$, then we can rewrite the above equation in the form $\\pmb{\\dot{x}} = \\pmb{Ax} + \\pmb{Bu}$, as below:\n",
    "\n",
    "\\begin{equation}\n",
    "    \\left[ \\begin{array}{c} \\dot{x}(t) \\\\ \\ddot{x}(t) \\end{array} \\right] = \\left[ \\begin{array}{cc} 0 & 1 \\\\ -\\frac{k}{m} & -\\frac{b}{m} \\end{array} \\right] \\left[ \\begin{array}{c} x(t) \\\\ \\dot{x}(t) \\end{array} \\right] + \\left[ \\begin{array}{c} 0 \\\\ \\frac{1}{m} \\end{array} \\right] \\left[ \\begin{array}{c} u(t) \\end{array} \\right]\n",
    "\\end{equation}\n",
    "\n",
    "If there is no $u(t)$, i.e. $u(t) = 0 \\; \\forall t$, then we have $\\pmb{\\dot{x}} = \\pmb{Ax}$. The solution to this system of equation is $\\pmb{x}(t) = e^{\\pmb{A}t} \\pmb{x}_0$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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": 2
}