Derives the equations of motion for the lateral dynamics of a car model using Lagrange's method.

auto_lateral_dynamics_symbolic.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Import SymPy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This command enables rich display of the mathematics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "me.init_vprinting()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Define Constants"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "m, I, a, b, tf, tr, Cf, Cr, U = sm.symbols('m, I, a, b, t_f, t_r, C_f, C_r, U', real=True, positive=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Can be used to write symbolic expressions (note `**` is used in Python for exponents not `^`):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "m * b**2 + I"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Define Variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = sm.symbols('t')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "y = sm.Function('y')(t)\n",
    "psi = sm.Function('psi')(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "y, psi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "y.diff(t)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Create and Orient Reference Frames"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "N = me.ReferenceFrame('N')  # newtonian RF"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = me.ReferenceFrame('A') # automobile RF"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "A.orient(N, 'axis', (psi, N.z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "A.dcm(N)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Define Velocity Vectors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_com = U * N.x + y.diff(t) * N.y\n",
    "V_com"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_com.magnitude()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_fl = V_com + me.cross(psi.diff(t) * N.z, a * A.x - tf * A.y)\n",
    "V_fl"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_fr = V_com + me.cross(psi.diff(t) * N.z, a * A.x + tf * A.y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_fl.express(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_fl.express(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_rl = V_com + me.cross(psi.diff(t) * N.z, -b * A.x - tr *A.y)\n",
    "V_rl.express(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_rr = V_com + me.cross(psi.diff(t) * N.z, -b * A.x + tr *A.y)\n",
    "V_rr.express(A)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Slip Angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "FL = me.ReferenceFrame('FL')\n",
    "FR = me.ReferenceFrame('FR')\n",
    "RL = me.ReferenceFrame('RL')\n",
    "RR = me.ReferenceFrame('RR')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "delf = sm.Function('delta_f')(t)\n",
    "delr = sm.Function('delta_r')(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "delr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "FL.orient(A, 'axis', (delf, N.z))\n",
    "FR.orient(A, 'axis', (delf, N.z))\n",
    "RL.orient(A, 'axis', (delr, N.z))\n",
    "RR.orient(A, 'axis', (delr, N.z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "FL.dcm(N).simplify()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "alphafl = sm.atan(V_fl.dot(FL.y) / V_fl.dot(FL.x))\n",
    "alphafl = alphafl.trigsimp()\n",
    "alphafl"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "alphafr = sm.atan(V_fr.dot(FL.y) / V_fr.dot(FL.x))\n",
    "alphafr = alphafr.trigsimp()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "alpharl = sm.atan(V_rl.dot(RL.y) / V_rl.dot(RL.x))\n",
    "alpharl = alpharl.trigsimp()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "alpharr = sm.atan(V_rr.dot(RL.y) / V_rr.dot(RL.x))\n",
    "alpharr = alpharr.simplify()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Kinetic Energy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = m / 2 * V_com.magnitude()**2 + I / 2 * psi.diff(t)**2\n",
    "T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Generalized Forces"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Xi_y = -Cf * alphafl - Cf * alphafr - Cr * alpharl - Cr * alpharr\n",
    "Xi_y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Xi_psi = -Cf * alphafl * a - Cf * alphafr * a + Cr * alpharl * b + Cr * alpharr * b\n",
    "Xi_psi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Create Lagrange's Equation For Each Variable\n",
    "\n",
    "$$\n",
    "0 = \\frac{d}{dt} \\frac{\\partial T}{\\partial \\dot{y}} - \\frac{\\partial T}{\\partial y}- \\Xi_y\n",
    "$$\n",
    "\n",
    "$$\n",
    "0 = \\frac{d}{dt} \\frac{\\partial T}{\\partial \\dot{\\psi}} - \\frac{\\partial T}{\\partial \\psi}- \\Xi_\\psi\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "zero_y = T.diff(y.diff(t)).diff(t) - T.diff(y) - Xi_y\n",
    "zero_y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "zero_psi = T.diff(psi.diff(t)).diff(t) - T.diff(psi) - Xi_psi\n",
    "zero_psi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Linearize Equations"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a column matrix containing all of the variables and their derivatives present in the nonlinear equations of motion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "v = sm.Matrix([y.diff().diff(), psi.diff().diff(),\n",
    "               y.diff(), psi.diff(),\n",
    "               y, psi, delf, delr])\n",
    "v"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "v0 = sm.zeros(len(v), 1)\n",
    "v0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "v_eq_sub = dict(zip(v, v0))\n",
    "v_eq_sub"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f = sm.Matrix([zero_y, zero_psi])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This following equation calculates the first two terms of the multivariate Taylor series expansion about $v_0$ using a matrix form that utilizes the Jacobian operator."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f_lin = f.xreplace(v_eq_sub) + f.jacobian(v).xreplace(v_eq_sub) * (v - v0)\n",
    "f_lin"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Find M, C, K, F Matrices"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M = f_lin.jacobian(sm.Matrix([y.diff().diff(), psi.diff().diff()]))\n",
    "M"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C = f_lin.jacobian(sm.Matrix([y.diff(), psi.diff()]))\n",
    "C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "K = f_lin.jacobian(sm.Matrix([y, psi]))\n",
    "K"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "F = -f_lin.jacobian(sm.Matrix([delf, delr]))\n",
    "F"
   ]
  }
 ],
 "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: sympy
channels:
  - conda-forge
dependencies:
  - sympy