{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simple Model of a Car on a Bumpy Road\n",
    "\n",
    "This notebook allows you to compute and visualize the car model presented in Example 2.4.2 the book.\n",
    "\n",
    "The road is described as:\n",
    "\n",
    "$$y(t) = Ysin\\omega_b t$$\n",
    "\n",
    "And $\\omega_b$ is a function of the car's speed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def x_h(t, wn, zeta, x0, xd0):\n",
    "    \"\"\"Returns the transient vertical deviation from the equilibrium of the mass.\n",
    "    \n",
    "    Parameters\n",
    "    ==========\n",
    "    t : ndarray, shape(n,)\n",
    "        An array of monotonically increasing values for time.\n",
    "    zeta : float\n",
    "        The damping ratio of the system.\n",
    "    x0 : float\n",
    "        The initial displacement from the equilibrium.\n",
    "    xd0 : float\n",
    "        The initial velocity of the mass.\n",
    "        \n",
    "    Returns\n",
    "    ========\n",
    "    x_h : ndarray, shape(n,)\n",
    "        An array containing the displacement from equilibrium as a function of time.\n",
    "    \n",
    "    \"\"\"\n",
    "    \n",
    "    wd = wn * np.sqrt(1 - zeta**2)\n",
    "    \n",
    "    A = np.sqrt(x0**2 + ((xd0 + zeta * wn * x0) / wd)**2)\n",
    "    \n",
    "    phi = np.arctan2(x0 * wd, xd0 + zeta * wn * x0)\n",
    "    \n",
    "    return A * np.exp(-zeta * wn * t) * np.sin(wd * t + phi)\n",
    "\n",
    "def x_p(t, wn, zeta, Y, wb):\n",
    "    \"\"\"Returns the steady state vertical deviation from the equilibrium of the mass.\n",
    "    \n",
    "    Parameters\n",
    "    ==========\n",
    "    t : ndarray, shape(n,)\n",
    "        An array of monotonically increasing values for time.\n",
    "    wn : float\n",
    "        The natural frequency of the system in radians per second.\n",
    "    zeta : float\n",
    "        The damping ratio of the system.\n",
    "    Y : float\n",
    "        The amplitude of the road bumps in meters.\n",
    "    wb : float\n",
    "        The frequency of the road bumps in radians per second.\n",
    "    \n",
    "\n",
    "    Returns\n",
    "    ========\n",
    "    x_p : ndarray, shape(n,)\n",
    "        An array containing the displacement from equilibrium as a function of time.\n",
    "    \n",
    "    \"\"\"\n",
    "    \n",
    "    theta1 = np.arctan2(2 * zeta * wn * wb, (wn**2 - wb**2))\n",
    "    \n",
    "    theta2 = np.arctan2(wn, 2 * zeta * wb)\n",
    "    \n",
    "    amp = wn * Y * ((wn**2 + (2 * zeta * wb)**2) / ((wn**2 - wb**2)**2 + (2 * zeta * wn * wb)**2))**0.5\n",
    "    \n",
    "    return amp * np.cos(wb * t - theta1 - theta2)\n",
    "\n",
    "def compute_force(t, m, c, k, Y, wb):\n",
    "    \"\"\"Returns the total force acting on the mass.\n",
    "    \n",
    "    Parameters\n",
    "    ==========\n",
    "    t : ndarray, shape(n,)\n",
    "        An array of monotonically increasing values for time.\n",
    "    m : float\n",
    "        The mass of the vehicle in kilograms.\n",
    "    c : float\n",
    "        The damping coefficient in Newton seconds per meter.\n",
    "    k : float\n",
    "        The spring stiffness in Newtons per meter.\n",
    "    Y : float\n",
    "        The amplitude of the road bumps in meters.\n",
    "    wb : float\n",
    "        The frequency of the road bumps in radians per second.\n",
    " \n",
    "        \n",
    "    Returns\n",
    "    ========\n",
    "    f : ndarray, shape(n,)\n",
    "        An array containing the acting on the mass as a function of time.\n",
    "    \n",
    "    \"\"\"\n",
    "    \n",
    "    wn = np.sqrt(k / m)\n",
    "    \n",
    "    zeta = c / 2 / m / wn    \n",
    "    \n",
    "    r = wb / wn\n",
    "    \n",
    "    amp = k * Y * r**2 * np.sqrt((1 + (2 * zeta * r)**2) / ((1 - r**2)**2 + (2 * zeta * r)**2))\n",
    "    \n",
    "    theta1 = np.arctan2(2 * zeta * wn * wb, (wn**2 - wb**2))\n",
    "    \n",
    "    theta2 = np.arctan2(wn, 2 * zeta * wb)\n",
    "    \n",
    "    return -amp * np.cos(wb * t - theta1 - theta2) \n",
    "\n",
    "def compute_trajectory(t, m, c, k, Y, wb, x0, xd0):\n",
    "    \"\"\"Returns the combined transient and steady state deviation of the mass from equilibrium.\n",
    "    \n",
    "    Parameters\n",
    "    ==========\n",
    "    t : ndarray, shape(n,)\n",
    "        An array of monotonically increasing values for time.\n",
    "    m : float\n",
    "        The mass of the vehicle in kilograms.\n",
    "    c : float\n",
    "        The damping coefficient in Newton seconds per meter.\n",
    "    k : float\n",
    "        The spring stiffness in Newtons per meter.\n",
    "    Y : float\n",
    "        The amplitude of the road bumps in meters.\n",
    "    wb : float\n",
    "        The frequency of the road bumps in radians per second.x0 : float\n",
    "        The initial displacement.\n",
    "    xd0 : float\n",
    "        The initial velocity of the mass.\n",
    "        \n",
    "    Returns\n",
    "    ========\n",
    "    x_h : ndarray, shape(n,)\n",
    "        An array containing the displacement from equilibrium as a function of time.\n",
    "        \n",
    "    \n",
    "    \"\"\"\n",
    "    \n",
    "    wn = np.sqrt(k / m)\n",
    "    \n",
    "    zeta = c / 2 / m / wn\n",
    "    \n",
    "    return x_h(t, wn, zeta, x0, xd0) + x_p(t, wn, zeta, Y, wb)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now start with the parameters given in the book."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Y = 0.01  # m\n",
    "v = 20  # km/h\n",
    "m = 1007  # kg\n",
    "k = 4e4  # N/m\n",
    "c = 20e2  # Ns/m\n",
    "x0 = -0.05  # m\n",
    "xd0 = 0  # m/s\n",
    "bump_distance = 6  # m"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The bump frequency is a function of the distance between the bumps and the speed of the vehicle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "wb = v / bump_distance * 1000 / 3600 * 2 * np.pi  # rad /s"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is worth noting what the frequency ratio is:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "r = np.sqrt(k / m) / wb\n",
    "r"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now pick some time values and compute the displacement and the force trajectories."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = np.linspace(0, 20, num=500)\n",
    "\n",
    "x = compute_trajectory(t, m, c, k, Y, wb, x0, xd0)\n",
    "\n",
    "f = compute_force(t, m, c, k, Y, wb)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the trajectories."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "%matplotlib notebook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig, axes = plt.subplots(2, 1, sharex=True)\n",
    "axes[0].plot(t, x)\n",
    "axes[1].plot(t, f / k)\n",
    "axes[1].set_xlabel('Time [s]')\n",
    "axes[0].set_ylabel('$x(t)$')\n",
    "axes[1].set_ylabel('F / k [m]')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now animate the simulation of the model showing the motion and a vector that represents the force per stiffness value."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from matplotlib.patches import Rectangle\n",
    "import matplotlib.animation as animation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "fig, ax = plt.subplots(1, 1)\n",
    "\n",
    "ax.set_ylim((-0.1, 0.6))\n",
    "ax.set_ylabel('Height [m]')\n",
    "#ax.set_aspect('equal')\n",
    "\n",
    "xeq = 0.1  # m\n",
    "view_width = 4  # m\n",
    "rect_width = 1.0  # m\n",
    "rect_height = rect_width / 4  # m\n",
    "bump_distance = 6 # m\n",
    "lat_pos = 0\n",
    "\n",
    "lat = np.linspace(lat_pos - view_width / 2, lat_pos + view_width / 2, num=100)\n",
    "\n",
    "ax.set_xlim((lat[0], lat[-1]))\n",
    "\n",
    "rect = Rectangle(\n",
    "                 (-rect_width / 2, xeq + x0),  # (x,y)\n",
    "                 rect_width,  # width\n",
    "                 rect_height,  # height\n",
    "                )\n",
    "\n",
    "car = ax.add_patch(rect)\n",
    "\n",
    "road = ax.plot(lat, Y * np.sin(2 * np.pi / bump_distance * lat), color='black')[0]\n",
    "\n",
    "suspension = ax.plot([lat_pos, lat_pos],\n",
    "                     [Y * np.sin(2 * np.pi / bump_distance * lat_pos), xeq + x0],\n",
    "                     linewidth='4', marker='o', color='yellow')[0]\n",
    "force_vec = ax.plot([lat_pos, lat_pos],\n",
    "                    [xeq + x0 + rect_height / 2, xeq + x0 + rect_height / 2 + 0.2],\n",
    "                    'r', linewidth=4)[0]\n",
    "\n",
    "def kph2mps(speed):\n",
    "    # km   1 hr     1 min    1000 m\n",
    "    # -- * ------ * ------ * ------\n",
    "    # hr   60 min   60 sec   1 km\n",
    "    return speed * 1000 / 3600\n",
    "\n",
    "def animate(i):\n",
    "    # update the data for all the drawn elements in this function\n",
    "    \n",
    "    lat_pos = kph2mps(v) * t[i]\n",
    "        \n",
    "    ax.set_xlim((lat_pos - view_width / 2, lat_pos + view_width / 2))\n",
    "    \n",
    "    rect.set_xy([lat_pos - rect_width / 2, xeq + x[i]])\n",
    "    \n",
    "    road.set_xdata(lat + lat_pos)\n",
    "    road.set_ydata(Y * np.sin(2 * np.pi / bump_distance * (lat + lat_pos)))\n",
    "    \n",
    "    suspension.set_xdata([lat_pos, lat_pos])\n",
    "    suspension.set_ydata([Y * np.sin(2 * np.pi / bump_distance * lat_pos), xeq + x[i]])\n",
    "    \n",
    "    force_vec.set_xdata([lat_pos, lat_pos])\n",
    "    force_vec.set_ydata([xeq + x[i] + rect_height / 2,\n",
    "                         xeq + x[i] + rect_height / 2 + f[i] / k])\n",
    "\n",
    "ani = animation.FuncAnimation(fig, animate, frames=len(t), interval=25)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Questions\n",
    "\n",
    "\n",
    "Explore the model and see if you can select a damping value that keeps the car pretty stable as it traverses the road.\n",
    "\n",
    "What happens if you change the driving speed? Do the values of $k$ and $c$ work well for all speeds?\n",
    "\n",
    "Can you detect the difference in displacement transmissibility and force transmissibility for different driving speeds?"
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python 3",
   "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.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}