dynamics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "d89f4213",
   "metadata": {},
   "source": [
    "## Dynamics\n",
    "We consider the control of a car whose state $z(t)$ (encapsulating position, velocity, etc of the car) at time $t$ is controlled by a control variable $u(t)$ (encapsulating acceleration, steering angle, etc) through dynamics of the form\n",
    "\n",
    "$$ \\dot z(t) = \\psi(z(t), u(t)),$$\n",
    "\n",
    "where $\\psi$ models the physics of the movement, as described below.\n",
    "\n",
    "The continuous-time representation of the car is not amenable to numerical computations. We consider the discrete-time\n",
    "representation of the movement given by some discretization of the continuous-time dynamics, which lead to a sequence of\n",
    "states $z_0, z_1, \\ldots $, called the trajectory, whose discrete-time dynamics are of the form\n",
    "\n",
    "$$ z_{t+1} = \\phi(z_t, u_t), $$\n",
    "\n",
    "for $u_0, u_1, \\ldots$ a sequence of controls applied to the system.\n",
    "\n",
    "The simplest discretization method is the Euler method, which for a time-step $dt$, reads $z_{t+1} = z_t + \\Delta \\psi(z_t, u_t),$\n",
    "i.e., $\\phi(z_t, u_t) = z_t +\\Delta \\psi(z_t, u_t)$ where $\\Delta$ is the discretization step. The Euler method is illustrated in the case of the simple model of a car. Alternatively, we can consider a Runge Kutta of order 4 (see, e.g. [[Süli and Mayers, 2003]](#refs)), which is illustrated in the realistic model of a car."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "278421fc",
   "metadata": {},
   "outputs": [],
   "source": [
    "import torch\n",
    "import time\n",
    "import math\n",
    "import pandas as pd\n",
    "import os\n",
    "from matplotlib import pyplot as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4c93cb0a",
   "metadata": {},
   "source": [
    "<a id='simple_model'></a>\n",
    "### Simple model of a car\n",
    "A simple model of a car is illustrated below.\n",
    "\n",
    "\n",
    "<center><img src=\"fig/simple_model.png\" alt=\"simple_model\" width=\"400\" /></center>\n",
    "\n",
    "The state of the car is described by $z(t) = (x(t), y(t), \\theta(t), v(t))$, where\n",
    "\n",
    "- the position $(x, y)$ of the car on the plane,\n",
    "- the yaw angle $\\theta$, i.e., the angle between the orientation of the car and the x-axis,\n",
    "- the velocity $v$ of the car.\n",
    "\n",
    "The car is controlled by $u(t) = (a(t), \\delta(t))$, where\n",
    "- its acceleration $a$,\n",
    "- the steering angle $\\delta$.\n",
    "\n",
    "The dynamics are, dropping the dependency w.r.t. time,\n",
    "\\begin{align}\n",
    "\\dot x &  = v \\cos(\\theta)  & \\dot \\theta &  = v \\tan(\\delta)/L  \\\\\n",
    "\\dot y  & = v \\sin(\\theta)  & \\dot v&  =  a,\n",
    "\\end{align}\n",
    "where $L$ is the length of the car.\n",
    "\n",
    "The following code instantiates this simple model of a car with an Euler discretization. The rendering is inspired by the rendering made in Open AI gym."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "19421ecf",
   "metadata": {},
   "outputs": [],
   "source": [
    "from envs.car import set_window_viewer, add_car_to_viewer\n",
    "\n",
    "class SimpleCar:\n",
    "    def __init__(self, dt=0.02, vinit=1., constrain_angle=math.pi/3):\n",
    "        super(SimpleCar, self).__init__()\n",
    "        self.dim_state, self.dim_ctrl = 4, 2\n",
    "        self.dt = dt\n",
    "        \n",
    "        self.constrain_angle = constrain_angle\n",
    "        \n",
    "        # Initial state of the system\n",
    "        self.init_state = self.state = torch.tensor([0, 0, 0., vinit])\n",
    "\n",
    "        # Dimensions of the car\n",
    "        self.carlength, self.carwidth = 0.09, 0.045\n",
    "\n",
    "        self.viewer = self.cartrans = None\n",
    "\n",
    "    # Continuous dynamics as defined above\n",
    "    def dyn(self, state, ctrl):\n",
    "        x, y, theta, v = state\n",
    "        a, delta = ctrl\n",
    "        \n",
    "        # We constrain the steering angle to be bounded through some non-linear transformation\n",
    "        delta = 2/math.pi*torch.arctan(delta)*self.constrain_angle\n",
    "        dx = v * torch.cos(theta)\n",
    "        dy = v * torch.sin(theta)\n",
    "        dtheta = v * torch.tan(delta)/self.carlength\n",
    "        dv = a\n",
    "        return torch.tensor([dx, dy, dtheta, dv])\n",
    "\n",
    "    # Discrete dynamics\n",
    "    def discrete_dyn(self, state, ctrl):\n",
    "        # Euler method\n",
    "        return state + self.dt * self.dyn(state, ctrl)\n",
    "\n",
    "    # Let the system make a step with the given control\n",
    "    def step(self, ctrl):\n",
    "        next_state = self.discrete_dyn(self.state, ctrl)\n",
    "        self.state = next_state\n",
    "        return next_state\n",
    "\n",
    "    def roll_out_cmd(self, cmd, reset=True):\n",
    "        if reset:\n",
    "            self.reset()\n",
    "        state = self.state\n",
    "        traj = [state]\n",
    "        for ctrl in cmd:\n",
    "            state = self.discrete_dyn(state, ctrl)\n",
    "            traj.append(state)\n",
    "        return traj\n",
    "    \n",
    "    def reset(self):\n",
    "        self.state = self.init_state\n",
    "   \n",
    "    def set_viewer(self, traj):\n",
    "        stack_traj = torch.stack(traj)\n",
    "        min_x, max_x = min(stack_traj[:, 0]), max(stack_traj[:, 0])\n",
    "        min_y, max_y = min(stack_traj[:, 1]), max(stack_traj[:, 1])\n",
    "        self.viewer = set_window_viewer(min_x, max_x, min_y, max_y, 2*self.carlength)\n",
    "        self.cartrans = add_car_to_viewer(self.viewer, self.carlength, self.carwidth)\n",
    "        \n",
    "    def render(self):\n",
    "        if self.viewer is None:\n",
    "            raise ValueError('Initilaze the viewer before rendering')\n",
    "        x, y, theta = self.state.numpy()[:3]   \n",
    "        self.cartrans.set_translation(x, y) # Translates the car with the new position      \n",
    "        self.cartrans.set_rotation(theta) # Update the orientation of the car\n",
    "        return self.viewer.render()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "93f748d4",
   "metadata": {},
   "source": [
    "We can now test the movement of our car. You can select a sequence of steering angles (``deltas``) and a sequence of accelerations (``accs``) in the code below to see the movement of the car. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "2d32aaa6",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "2024-08-01 16:04:48.979 Python[59711:1687881] ApplePersistenceIgnoreState: Existing state will not be touched. New state will be written to /var/folders/pf/znths45963l2918jylv3wzjr0134jq/T/org.python.python.savedState\n"
     ]
    }
   ],
   "source": [
    "dt = 0.02 # Discretization step\n",
    "horizon = 400 # Number of steps\n",
    "car = SimpleCar(dt)\n",
    "\n",
    "deltas = torch.tan(math.pi/6  * torch.ones(horizon)) # Constant steering angle\n",
    "accs = torch.linspace(0, 1, horizon) # Linearly increasing acceleration\n",
    "cmd = torch.stack((accs, deltas), dim=1) # Sequence of controls\n",
    "\n",
    "# Display the movement of the car\n",
    "car.reset()\n",
    "traj = car.roll_out_cmd(cmd)\n",
    "car.set_viewer(traj)\n",
    "car.render()\n",
    "for ctrl in cmd:\n",
    "    time.sleep(dt)\n",
    "    car.step(ctrl)\n",
    "    car.render()\n",
    "car.viewer.close()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0b6477b",
   "metadata": {},
   "source": [
    "<a id='realistic_model'></a>\n",
    "\n",
    "### Realistic model of a car\n",
    "A realistic model of a car considers the friction forces on the wheels to define how the car turns.  Specifically, we consider a bicycle model as presented by [[Liniger et al 2015]](#refs) to model a miniature car or [[Gerdts et al 2009]](#refs) to model a Volkswagen car. The model is illustrated in the figure below.\n",
    "\n",
    "<center><img src=\"fig/realistic_model.png\" alt=\"realistic model\" width=\"600\" /></center>\n",
    "\n",
    "In this model, the state of the car is defined by $z(t) = (x(t), y(t), \\theta(t), v_x(t), v_y(t), \\omega(t))$, where\n",
    "- the position $(x, y)$ of the car,\n",
    "- the yaw of the car $\\theta$\n",
    "- the longitudinal speed $v_x$,\n",
    "- the lateral speed $v_y$,\n",
    "- the yaw rate $\\omega$.\n",
    "\n",
    "The controls are given by $u(t) = (a(t), \\delta(t))$, where\n",
    "- the PWM duty cycle of the car denoted $a$, this duty cycle can be negative to take into account braking,\n",
    "- the steering angle $\\delta$.\n",
    "\n",
    "These controls act on the state through the following forces.\n",
    "- A longitudinal force on the rear wheels, denoted $F_{r, x}$ modeled using a motor model for the DC electric motor as well as a friction model for the rolling resistance and the drag:\n",
    "$$\n",
    "F_{r, x} = (C_{m1} -C_{m2} v_x) a - C_{r0} - C_{r2} v_x^2,\n",
    "$$\n",
    "where $C_{m1}, C_{m2}, C_{r0}, C_{r2}$ are constants estimated from experiments. \n",
    "- Lateral forces on the front and rear wheels, denoted $F_{f, y}, F_{y, r}$ respectively, modeled using a simplified Pacejka Tire Model: \n",
    "\\begin{align*}\n",
    "F_{f, y} & = D_f \\sin(C_f \\arctan(B_f \\alpha_f)) \\quad \\textrm{where} \\ \\alpha_f = \\delta - \\arctan\\left(\\frac{\\omega l_f + v_y}{v_x}\\right)  \\\\\n",
    "F_{r, y} & = D_r \\sin(C_r \\arctan(B_r \\alpha_r)) \\quad \\textrm{where} \\ \\alpha_r = \\arctan\\left(\\frac{\\omega l_r - v_y}{v_x}\\right),  \n",
    "\\end{align*}\n",
    "where $\\alpha_f$, $\\alpha_r$ are the slip angles on the front and rear wheels respectively, $l_f, l_r$ are the distance from the center of gravity to the front and the rear wheel and the constants $B_r, C_r, D_r, B_f, C_f, D_f$ define the exact shape of the semi-empirical curve, presented below\n",
    "\n",
    "<center><img src=\"fig/pacejka.png\" alt=\"pacejka tire model\" width=\"350\" /></center>\n",
    "\n",
    "The continuous time dynamics are then (dropping the dependency w.r.t. time for simplicity):\n",
    "\n",
    "\\begin{align*}\n",
    "\\dot x & = v_x \\cos(\\theta) - v_y\\sin(\\theta) & \\dot v_x & =\\frac{1}{m}(F_{r,x} - F_{f, y} \\sin (\\delta)) + v_y \\omega \\\\\n",
    "\\dot y & = v_x\\sin(\\theta) + v_y\\cos(\\theta) & \\dot v_y & = \\frac{1}{m}(F_{r, y} + F_{f, y} \\cos(\\delta)) - v_x \\omega \\\\\n",
    "\\dot \\theta & = \\omega & \\dot \\omega & = \\frac{1}{I_z}(F_{f, y} l_f \\cos(\\delta) - F_{r, y} l_r),\n",
    "\\end{align*}\n",
    "\n",
    "where $m$ is the mass of the car and $I_z$ is the inertia.\n",
    "\n",
    "The following code illustrates these dynamics. The parameters used in this model are the ones of [[Liniger et al 2015]](#refs). "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "368ab978",
   "metadata": {},
   "outputs": [],
   "source": [
    "from envs.car import set_window_viewer, add_car_to_viewer\n",
    "\n",
    "class RealisticCar:\n",
    "    def __init__(self, dt=0.02, vinit=1., constrain_angle=math.pi/3, acc_bounds=(-0.1, 1.)):\n",
    "        super(RealisticCar, self).__init__()\n",
    "        self.dim_state, self.dim_ctrl = 6, 2\n",
    "        self.dt = dt\n",
    "        self.constrain_angle, self.acc_bounds = constrain_angle, acc_bounds\n",
    "        \n",
    "        # Initial state of the system\n",
    "        self.init_state = torch.tensor([0, 0, 0, vinit, 0., 0.])\n",
    "        \n",
    "        # Constants used to define the dynamics are stored in a json file\n",
    "        self.dyn_csts = pd.read_json(os.path.join('envs/bicycle_model.json'), typ='series')\n",
    "        \n",
    "        # We use larger carlength and carwidth than the parameters of Liniger et al for better visualization\n",
    "        self.carlength, self.carwidth = 1.5 * self.dyn_csts['car_l'], 1.5 * self.dyn_csts['car_w']\n",
    "\n",
    "        self.viewer = self.cartrans = None\n",
    "\n",
    "    # Continuous dynamics as defined above\n",
    "    def dyn(self, state, ctrl):\n",
    "        x, y, theta, vx, vy, vtheta = state\n",
    "        a, delta = ctrl\n",
    "        \n",
    "        # We constrain the steering angle to be bounded through some non-linear transformation\n",
    "        delta = 2/math.pi*torch.arctan(delta)*self.constrain_angle\n",
    "        \n",
    "        # Similarly we constrain the acceleration to some bounds taken from Liniger et al \n",
    "        gap = self.acc_bounds[1] - self.acc_bounds[0]\n",
    "        a = gap*torch.sigmoid(4 * a / gap) + self.acc_bounds[0]\n",
    "        \n",
    "        Cm1, Cm2, Cr0, Cr2,\\\n",
    "        Br, Cr, Dr, Bf, Cf, Df,\\\n",
    "        m, Iz, lf, lr = [self.dyn_csts[key] for key in ['Cm1', 'Cm2', 'Cr0', 'Cr2',\n",
    "                                                        'Br', 'Cr', 'Dr', 'Bf', 'Cf', 'Df',\n",
    "                                                        'm', 'Iz', 'lf', 'lr']]\n",
    "\n",
    "        # Note that the angles alphaf and alphar must be computed using arctan2\n",
    "        alphaf = delta - torch.atan2(vtheta*lf + vy, vx) \n",
    "        alphar = torch.atan2(vtheta*lr - vy, vx)\n",
    "        Fry = Dr*torch.sin(Cr*torch.atan(Br*alphar))\n",
    "        Ffy = Df*torch.sin(Cf*torch.atan(Bf*alphaf))\n",
    "        Frx = (Cm1 - Cm2*vx)*a - Cr0 - Cr2*vx**2\n",
    "        dx = torch.cos(theta)*vx - torch.sin(theta)*vy\n",
    "        dy = torch.sin(theta)*vx + torch.cos(theta)*vy\n",
    "        dtheta = vtheta\n",
    "        dvx = (Frx - Ffy*torch.sin(delta) + m*vy*vtheta)/m\n",
    "        dvy = (Fry + Ffy*torch.cos(delta) - m*vx*vtheta)/m\n",
    "        dvtheta = (Ffy*lf*torch.cos(delta) - Fry*lr)/Iz\n",
    "        return torch.stack([dx, dy, dtheta, dvx, dvy, dvtheta])\n",
    "\n",
    "    # Discrete dynamics\n",
    "    def discrete_dyn(self, state, ctrl):\n",
    "        # Runge Kutta method of order 4\n",
    "        # We consider a constant control variable in this discretization\n",
    "        dt = self.dt\n",
    "        k1 = self.dyn(state, ctrl)\n",
    "        k2 = self.dyn(state + dt*k1/2, ctrl)\n",
    "        k3 = self.dyn(state + dt*k2/2, ctrl)\n",
    "        k4 = self.dyn(state + dt*k3, ctrl)\n",
    "        return state + dt*(k1 + 2*k2 + 2*k3 + k4)/6\n",
    "\n",
    "    # Let the system make a step with the given control\n",
    "    def step(self, ctrl):\n",
    "        next_state = self.discrete_dyn(self.state, ctrl)\n",
    "        self.state = next_state\n",
    "        return next_state\n",
    "\n",
    "    def roll_out_cmd(self, cmd, reset=True):\n",
    "        if reset:\n",
    "            self.reset()\n",
    "        state = self.state\n",
    "        traj = [state]\n",
    "        for ctrl in cmd:\n",
    "            state = self.discrete_dyn(state, ctrl)\n",
    "            traj.append(state)\n",
    "        return traj\n",
    "\n",
    "    def reset(self):\n",
    "        self.state = self.init_state\n",
    "        \n",
    "    def set_viewer(self, traj):\n",
    "        stack_traj = torch.stack(traj)\n",
    "        min_x, max_x = min(stack_traj[:, 0]), max(stack_traj[:, 0])\n",
    "        min_y, max_y = min(stack_traj[:, 1]), max(stack_traj[:, 1])\n",
    "        self.viewer = set_window_viewer(min_x, max_x, min_y, max_y, 2*self.carlength)\n",
    "        self.cartrans = add_car_to_viewer(self.viewer, self.carlength, self.carwidth)\n",
    "        \n",
    "    def render(self):\n",
    "        if self.viewer is None:\n",
    "            raise ValueError('Initiliaze the viewer before rendering')\n",
    "        x, y, theta = self.state.numpy()[:3]   \n",
    "        self.cartrans.set_translation(x, y) # Translates the car with the new position      \n",
    "        self.cartrans.set_rotation(theta) # Update the orientation of the car\n",
    "        return self.viewer.render()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d5d0314d",
   "metadata": {},
   "source": [
    "As for the simple model of the car, we can test the movement of the car for a sequence of steering angles (``deltas``) and a sequence of accelerations (``accs``). Note the difference of behavior in the case of a constant steering angle and a linearly increasing acceleration: in the case of a realistic car, the trajectory is not a simple circle, and we see the car drift. Note also that the model is not realistic for low speeds, therefore we initialize the car with a constant longitudinal speed (``vinit`` in the definition of the model) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "de6a6040",
   "metadata": {},
   "outputs": [],
   "source": [
    "dt = 0.02 # Discretization step\n",
    "horizon = 400 # Number of steps\n",
    "\n",
    "car = RealisticCar(dt)\n",
    "\n",
    "# To better observe the drifting of the car, change the bounds on the acceleration by uncommenting the following\n",
    "# car = RealisticCar(dt, acc_bounds=(-2., 2.))\n",
    "\n",
    "accs = torch.linspace(0, 2, horizon) # Linearly increasing acceleration\n",
    "deltas = torch.tan(math.pi/6*torch.ones(horizon)) # Constant steering angle\n",
    "cmd = torch.stack((accs, deltas), dim=1) # Sequence of controls\n",
    "\n",
    "traj = car.roll_out_cmd(cmd)\n",
    "car.set_viewer(traj)\n",
    "car.render()\n",
    "for ctrl in cmd:\n",
    "    time.sleep(dt)\n",
    "    car.step(ctrl)\n",
    "    car.render()\n",
    "car.viewer.close()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c3cc060a",
   "metadata": {},
   "source": [
    "Now that we have the dynamics we need to define the costs, see the notebook [Costs](costs.ipynb)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cb19cbfb",
   "metadata": {},
   "source": [
    "<a id='refs'></a>\n",
    "### References\n",
    "**Optimization‐based autonomous racing of 1: 43 scale RC cars.**  \n",
    "Alexander Liniger, Alexander Domahidi, and Manfred Morari.  \n",
    "*Optimal Control Applications and Methods, 2015.*  \n",
    "\n",
    "**Generating Locally Optimal Trajectories for an Automatically Driven Car.**  \n",
    "Matthias Gerdts, Simon Karrenberg, Bernhard Müller-Beßler and Gregor Stock.  \n",
    "*Optimization and Engineering, 2009.*  \n",
    "\n",
    "**An Introduction to Numerical Analysis.**  \n",
    "Endre Süli and David Mayers.  \n",
    "*Cambridge University Press, 2003.*  "
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}