Simulates the lateral dynamics of an automobile

auto_lateral_dynamics_numeric.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Import Packages and Setup"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np  # arrays and vectorized computations\n",
    "import matplotlib.pyplot as plt  # plotting functions\n",
    "from matplotlib.patches import Circle, Rectangle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from resonance.linear_systems import MultiDoFLinearSystem"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# makes plots display in the notebook\n",
    "%matplotlib notebook"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Define Constants\n",
    "\n",
    "This sets the numeric values for the various constants to be approximately what a Honda Civic might have."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "speed = 20.0  # m/s\n",
    "\n",
    "car_width = 1.54  # meters\n",
    "car_length = 2.7  # meters\n",
    "\n",
    "tire_width = 0.22 # meters\n",
    "tire_diameter = 0.65  # meters\n",
    "\n",
    "mass = 2760 * 4.44 / 9.81  # kg (2760 lbs converted to kg)\n",
    "inertia = mass / 12 * (car_length**2 + car_width**2)  # kg m**2\n",
    "\n",
    "com_ratio = 0.2  # < 0.5 closer to front, > 0.5 closer to rear\n",
    "front_end = com_ratio * car_length  # meters\n",
    "rear_end = (1 - com_ratio) * car_length  # meters\n",
    "\n",
    "dmux_dalpha = 0.6 / np.deg2rad(5.0)  # (N/N) / rad\n",
    "Fz = mass * 9.81 #/ 4  # [N] weigt at each tire\n",
    "\n",
    "front_corner_coef = 0.6 * dmux_dalpha * Fz  # N/rad\n",
    "rear_corner_coef = 0.4 * dmux_dalpha * Fz  # N/rad"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if rear_end*rear_corner_coef < front_end*front_corner_coef:\n",
    "    print('oversteer')\n",
    "    u_crit = np.sqrt(-car_length**2*front_corner_coef*rear_corner_coef/mass/(rear_end*rear_corner_coef-front_end*front_corner_coef))\n",
    "    print('Critical speed: {:1.1f} m/s'.format(u_crit))\n",
    "elif rear_end*rear_corner_coef == front_end*front_corner_coef:\n",
    "    print('neutralsteer')\n",
    "else:\n",
    "    print('understeer')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check the understeer coefficient to see what kind of car you have:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "K = mass * (rear_end*rear_corner_coef - front_end*front_corner_coef) / car_length / front_corner_coef / rear_corner_coef\n",
    "K"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Create an Automobile System"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car = MultiDoFLinearSystem()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set the constants on the system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.constants['U'] = speed\n",
    "car.constants['m'] = mass\n",
    "car.constants['a'] = front_end\n",
    "car.constants['b'] = rear_end\n",
    "car.constants['I'] = inertia\n",
    "car.constants['Cf'] = front_corner_coef\n",
    "car.constants['Cr'] = rear_corner_coef"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.constants"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set the variables and their first derivatives. These will be used as initial conditions in the simulation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.coordinates['y'] = 0.0  # meters\n",
    "car.coordinates['psi'] = 0.0  # radians\n",
    "car.speeds['ydot'] = 0.0  # m/s\n",
    "car.speeds['psidot'] = 0.0  # rad/s"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Define the Linear Equations of Motion\n",
    "\n",
    "This requires a function that takes the constants as inputs and outputs M, C, and K matrices. The equations should be taken from the symbolics you determined in the prior notebook."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_mck(U, m, I, a, b, Cf, Cr):\n",
    "\n",
    "    M = np.array([[m, 0],\n",
    "                  [0, I]])\n",
    "    \n",
    "    C = np.array([[    2 * (Cf + Cr) / U, 2 * (a*Cf - b*Cr) / U],\n",
    "                  [2 * (a*Cf - b*Cr) / U, 2 * (a**2*Cf + b**2*Cr) / U]])\n",
    "    \n",
    "    K = np.array([[0, -2 * (Cf + Cr)],\n",
    "                  [0, -2 * (a*Cf - b*Cr)]])\n",
    "    \n",
    "    return M, C, K\n",
    "\n",
    "car.canonical_coeffs_func = calc_mck"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you can see the numerical values of M, C, K."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.canonical_coefficients()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "F = np.array([[2*front_corner_coef, 2 * rear_corner_coef],\n",
    "              [2*front_corner_coef*front_end, -2*rear_corner_coef*rear_end]])\n",
    "F"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simulate the Free Response wrt Initial Conditions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.coordinates['psi'] = np.deg2rad(5.0)  # set initial yaw angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.coordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.speeds"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = car.free_response(10.0)  # simulate for 5.0 seconds"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj[['y', 'psi', 'ydot', 'psidot']].plot(subplots=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the path."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj['x'] = traj.index * speed\n",
    "ax = traj.plot('x', 'y')\n",
    "ax.set_aspect('equal')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Add Steering Inputs\n",
    "\n",
    "$$\n",
    "\\delta_f = 5^\\circ \\cos(\\pi/8 t)\n",
    "$$\n",
    "\n",
    "$\\pi$ radians/second is $180^\\circ$ degrees/second\n",
    "\n",
    "$$\n",
    "\\delta_r = -0.2 \\delta_f\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_deltaf(time):\n",
    "    return np.deg2rad(10.0) * np.cos(np.deg2rad(180/8) * time)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.add_measurement('deltaf', calc_deltaf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_deltar(deltaf):\n",
    "    return -0.2 * deltaf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.add_measurement('deltar', calc_deltar)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.measurements"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now implement the forcing side of the equations of motion:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_steer(time, deltaf, deltar, a, b, Cf, Cr):\n",
    "    F1 = Cf*deltaf + Cr*deltar\n",
    "    F2 = Cf*a*deltaf - Cr*b*deltar\n",
    "    return F1, F2\n",
    "\n",
    "car.forcing_func = calc_steer"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Plot the Initial Configuration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.coordinates['psi'] = np.deg2rad(10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def centered_rectangle(xy, width, height, angle=0.0):\n",
    "    \"\"\"Returns the arguments for Rectangle given the x and y coordinates of the\n",
    "    center of the rectangle.\n",
    "\n",
    "    Parameters\n",
    "    ==========\n",
    "    xy : tuple of floats\n",
    "        The x and y coordinates of the center of the rectangle.\n",
    "    width : float\n",
    "        Width of the rectangle. When angle=0.0 this is along the x axis.\n",
    "    height : float\n",
    "        Height of the rectangle. When angle=0.0 this is along the y axis.\n",
    "    angle : float\n",
    "        Angle of rotation about the z axis in degrees.\n",
    "\n",
    "    Returns\n",
    "    =======\n",
    "    xy_ll : tuple of floats\n",
    "        The x and y coordinates of the lower left hand corner of the rectangle.\n",
    "    width : float\n",
    "        Width of the rectangle. When angle=0.0 this is along the x axis.\n",
    "    height : float\n",
    "        Height of the rectangle. When angle=0.0 this is along the y axis.\n",
    "    angle : float\n",
    "        Angle of rotation about the z axis in degrees.\n",
    "\n",
    "    \"\"\"\n",
    "    xc, yc = xy\n",
    "    theta = np.deg2rad(angle)\n",
    "    x_ll = xc - width/2 * np.cos(theta) + height/2 * np.sin(theta)\n",
    "    y_ll = yc - width/2 * np.sin(theta) - height/2 * np.cos(theta)\n",
    "    xy_ll = (x_ll, y_ll)\n",
    "    return xy_ll, width, height, angle\n",
    "\n",
    "def plot_config(y, psi, a, b, U, deltaf, deltar, time, time__hist, y__hist):\n",
    "    \n",
    "    fig, ax = plt.subplots(1, 1)\n",
    "\n",
    "    car_com_x = U * time\n",
    "    car_com_y = y\n",
    "\n",
    "    com = Circle((car_com_x, car_com_y), radius=0.1, color='black')\n",
    "\n",
    "    car_center_x = car_com_x - ((a + b) / 2 - a) * np.cos(psi)\n",
    "    car_center_y = car_com_y - ((a + b) / 2 - a) * np.sin(psi)\n",
    "\n",
    "    car = Rectangle(*centered_rectangle((car_center_x, car_center_y),\n",
    "                                        a + b, car_width, angle=np.rad2deg(psi)))\n",
    "    \n",
    "    flwheel = Rectangle(*centered_rectangle(\n",
    "        (car_com_x + a*np.cos(psi) + car_width / 2 * np.sin(psi),\n",
    "         car_com_y + a*np.sin(psi) - car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltaf)), color='green')\n",
    "    frwheel = Rectangle(*centered_rectangle(\n",
    "        (car_com_x + a*np.cos(psi) - car_width / 2 * np.sin(psi),\n",
    "         car_com_y + a*np.sin(psi) + car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltaf)), color='red')\n",
    "\n",
    "    rlwheel = Rectangle(*centered_rectangle(\n",
    "        (car_com_x - b*np.cos(psi) + car_width / 2 * np.sin(psi),\n",
    "         car_com_y - b*np.sin(psi) - car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltar)), color='orange')\n",
    "    rrwheel = Rectangle(*centered_rectangle(\n",
    "        (car_com_x - b*np.cos(psi) - car_width / 2 * np.sin(psi),\n",
    "         car_com_y - b*np.sin(psi) + car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltar)), color='purple')\n",
    "    \n",
    "    path = ax.plot(U * time__hist, y__hist, color='black')[0]\n",
    "\n",
    "    ax.add_patch(car)\n",
    "    ax.add_patch(com)\n",
    "    ax.add_patch(flwheel)\n",
    "    ax.add_patch(frwheel)\n",
    "    ax.add_patch(rlwheel)\n",
    "    ax.add_patch(rrwheel)\n",
    "\n",
    "    text = ax.text(U * time - 3*car_width, y + 3*car_width, 'Time = {:0.3f} s'.format(time))\n",
    "\n",
    "    ax.set_xlim((U * time - 4*car_length, U * time + 4*car_length))\n",
    "    ax.set_ylim((y + 4*car_width, y - 4*car_width))\n",
    "\n",
    "    ax.set_aspect('equal')\n",
    "    ax.grid(b=True)\n",
    "    ax.set_xlabel('Distance [m]')\n",
    "    ax.set_ylabel('Distance [m]')\n",
    "\n",
    "    return fig, ax, car, com, flwheel, frwheel, rlwheel, rrwheel, path, text\n",
    "\n",
    "car.config_plot_func = plot_config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.plot_configuration();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simulate the Forced Response"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.coordinates['psi'] = 0.0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = car.forced_response(30.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj.columns"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj[['y', 'psi', 'ydot', 'psidot', 'deltaf', 'deltar']].plot(subplots=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj['x'] = traj.index * speed\n",
    "ax = traj.plot('x', 'y')\n",
    "ax.set_aspect('equal')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Animate the Motion"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def update(y, psi, a, b, U, deltaf, deltar, time,\n",
    "           time__hist, y__hist,\n",
    "           ax, car, com, flwheel, frwheel, rlwheel, rrwheel, path, text):\n",
    "\n",
    "    car_com_x = U * time\n",
    "    car_com_y = y\n",
    "\n",
    "    com.center = (car_com_x, car_com_y)\n",
    "\n",
    "    car_center_x = car_com_x - ((a + b) / 2 - a) * np.cos(psi)\n",
    "    car_center_y = car_com_y - ((a + b) / 2 - a) * np.sin(psi)\n",
    "    car_center = (car_center_x, car_center_y)\n",
    "\n",
    "    xy, _, _, angle = centered_rectangle(car_center, car_length,\n",
    "                                         car_width, np.rad2deg(psi))\n",
    "    car.set_xy(xy)\n",
    "    car.angle = angle\n",
    "\n",
    "    res = centered_rectangle(\n",
    "        (car_com_x + a*np.cos(psi) + car_width / 2 * np.sin(psi),\n",
    "         car_com_y + a*np.sin(psi) - car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltaf))\n",
    "\n",
    "    flwheel.set_xy(res[0])\n",
    "    flwheel.angle = res[-1]\n",
    "  \n",
    "    res = centered_rectangle(\n",
    "        (car_com_x + a*np.cos(psi) - car_width / 2 * np.sin(psi),\n",
    "         car_com_y + a*np.sin(psi) + car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltaf))\n",
    "\n",
    "    frwheel.set_xy(res[0])\n",
    "    frwheel.angle = res[-1]\n",
    "\n",
    "    res = centered_rectangle(\n",
    "        (car_com_x - b*np.cos(psi) + car_width / 2 * np.sin(psi),\n",
    "         car_com_y - b*np.sin(psi) - car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltar))\n",
    "\n",
    "    rlwheel.set_xy(res[0])\n",
    "    rlwheel.angle = res[-1]\n",
    "\n",
    "    res = centered_rectangle(\n",
    "        (car_com_x - b*np.cos(psi) - car_width / 2 * np.sin(psi),\n",
    "         car_com_y - b*np.sin(psi) + car_width / 2 * np.cos(psi)),\n",
    "        tire_diameter, tire_width, angle=np.rad2deg(psi + deltar))\n",
    "\n",
    "    rrwheel.set_xy(res[0])\n",
    "    rrwheel.angle = res[-1]\n",
    "    \n",
    "    path.set_data(U * time__hist, y__hist)\n",
    "\n",
    "    text.set_text('Time = {:0.3f} s'.format(time))\n",
    "    text.set_position((U * time - 3*car_length, y + 3*car_width))\n",
    "\n",
    "    ax.set_xlim((U * time - 4*(a+b), U * time + 4*(a+b)))\n",
    "    ax.set_ylim((y + 4*car_width, y - 4*car_width))\n",
    "    \n",
    "    ax.set_aspect('equal')\n",
    "\n",
    "car.config_plot_update_func = update"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "car.animate_configuration(fps=10)"
   ]
  }
 ],
 "metadata": {
  "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.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}

environment.yml

name: resonance
channels:
  - conda-forge
dependencies:
  - resonance
  - numpy
  - matplotlib