tut0_introduction.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c4c5436b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# install dependencies\n",
    "!pip install casadi"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0fce8dca",
   "metadata": {
    "id": "0fce8dca"
   },
   "source": [
    "# Safe Autonomy Tutorial: 2D Drone Example\n",
    "\n",
    "In this tutorial, you will be getting comfortable with Colab notebooks and do some basic exercises in python to get back into the swing of things! You will also get an introduction to the running example used throughout the course, taking a look at the dynamics model of a 2D drone and run some initial simulations."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4227c7f4",
   "metadata": {},
   "source": [
    "## Exercise 0\n",
    "\n",
    "\n",
    "* Take a look at the [Basic Colab Feature Overview](https://colab.research.google.com/notebooks/basic_features_overview.ipynb). You will probably not need all of the mentioned features.\n",
    "* The following exercises and tutorials will require basic python knowledge. If you need a quick refresher, check out this [Python Tutorial](https://colab.research.google.com/github/cs231n/cs231n.github.io/blob/master/python-colab.ipynb).\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5667e63a",
   "metadata": {},
   "source": [
    "## Exercise 1 - Python Warm-up\n",
    "\n",
    "The Fibonacci numbers are the sequence of numbers $\\{F_n\\}_{n=1}^\\infty$ defined by the linear recurrence equation\n",
    "\n",
    "$$F_n = F_{n-1} + F_{n-2}$$\n",
    "\n",
    "with $F_1 = F_2 = 1$, and conventionally defining $F_0 = 0$. We can thus calculate some of the first few Fibonacci numbers as 0, 1, 1, 2,\n",
    "3, 5, 8, 13, 21, ... and so forth.\n",
    "\n",
    "1. Create a code cell under this text cell.\n",
    "2. Write a recursive python function that returns the n-th fibonnaci number, where n is given as an input parameter.\n",
    "3. Print some example results using your function.\n",
    "\n",
    "### Voluntary Bonus\n",
    "When computing all fibonacci numbers from 0 to n, it is not very efficient to call the recursive function multiple times. Write a more efficient version by either caching previous results and/or generating the numbers in an iterative fashion."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f42496d2",
   "metadata": {},
   "source": [
    "## Exercise 2 - Plotting\n",
    "\n",
    "[matplotlib](https://matplotlib.org/) is a powerful python library that features a multitude of plotting functionalities. For examples on how to use it, check the tutorials mentioned in exercise 0.\n",
    "\n",
    "\n",
    "1. Plot the graph of the first 100 fibonacci numbers, with $n$ on the x-axis and $F_n$ on the y-axis.\n",
    "2. Make sure to label all axes and give your plot a heading.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e7d76208",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "\n",
    "# PUT YOUR CODE HERE"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "05bf5c60",
   "metadata": {
    "id": "05bf5c60"
   },
   "source": [
    "## Continuous-time dynamics"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1af77ca8",
   "metadata": {
    "id": "1af77ca8"
   },
   "source": [
    "In this section, we're gonna have a look at the dynamics model of a 2D drone which is typically needed for verification or controller synthesis which is a big part of this course. The true dynamics of a drone, or any other classic robotic system, can be obtained by using Newton and Euler Equations. For now,\n",
    "we consider a simplified kinematic model of the drone:\n",
    "$$\n",
    "\\begin{align}\n",
    "\\label{eq:dynamics}\\tag{1}\n",
    "    \\begin{bmatrix}\n",
    "    \\dot{x}\\\\\n",
    "    \\dot{y}\\\\\n",
    "    \\dot{\\theta}\n",
    "    \\end{bmatrix} = \\begin{bmatrix}\n",
    "    - u_1 sin(\\theta)\\\\\n",
    "    u_1 cos(\\theta)\\\\\n",
    "    u_2\n",
    "    \\end{bmatrix}\n",
    "\\end{align}\n",
    "$$\n",
    "Note that our dynamical system $\\dot{\\mathbf{x}} = \\mathbf{f} (\\mathbf{x}, \\mathbf{u})$ is defined by a three dimensional state vector $\\mathbf{x} \\in \\mathbb{R}^3$ and a two dimensional control input $\\mathbf{u} \\in \\mathbb{R}^2$. Here, the control input $u_1$ corresponds to the velocity in the direction of the drones heading and $u_2$ allows us to control the drone's angular velocity."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fb9f3c02",
   "metadata": {
    "id": "fb9f3c02"
   },
   "source": [
    "Let's import some packages and define the dynamics of the Drone as a callable function!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "db1b08d8",
   "metadata": {
    "id": "db1b08d8"
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from math import *\n",
    "\n",
    "\n",
    "def dynamics(x, u):\n",
    "    \"\"\"\n",
    "    Inputs: current state of the system x, control input u\n",
    "    Output: derivative of the state vector\n",
    "    \"\"\"\n",
    "    # PUT YOUR CODE HERE\n",
    "#     dx = ...\n",
    "    return dx"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "734984e2",
   "metadata": {
    "id": "734984e2"
   },
   "source": [
    "Next, let us explore how our dynamical system behaves under a certain control input. Let's consider the case where the drone is oriented in an upright position and the thrusters are generating a sinusoidal velocity profile. In this case we will define our control input as"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b89da51f",
   "metadata": {
    "id": "b89da51f"
   },
   "outputs": [],
   "source": [
    "def control_input(x, t):\n",
    "    return np.array([0.5 * sin(5 * t), 0.0])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "13dcaf05",
   "metadata": {
    "id": "13dcaf05"
   },
   "source": [
    "What we are interested in now is how our dynamical system in (1) evolves over time given our control signal $\\mathbf{u}(t)$. To do so, we need to solve the ordinary differential equation (1) which is essentially an initial value problem (IVP)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e6da070e",
   "metadata": {
    "colab": {
     "base_uri": "https://localhost:8080/"
    },
    "id": "e6da070e",
    "outputId": "e9211943-b804-4290-c5a3-785da804daf8"
   },
   "outputs": [],
   "source": [
    "from scipy.integrate import solve_ivp\n",
    "\n",
    "\n",
    "# Define controlled vector field F(x)\n",
    "F = lambda t, x: dynamics(x, control_input(x, t))\n",
    "\n",
    "# define time span\n",
    "tspan = [0., 5.]\n",
    "\n",
    "# define initial state\n",
    "x0 = np.array([0., 0., 0.])\n",
    "\n",
    "# solve IVP\n",
    "sol = solve_ivp(F, tspan, x0, t_eval=np.arange(tspan[0], tspan[1], 0.01))\n",
    "\n",
    "print(\"Solution shape: \" + str(sol.y.shape))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c9d235c6",
   "metadata": {
    "id": "c9d235c6"
   },
   "source": [
    "Now, we can have a look at the state evolution over time. We are expecting to see a sinusoidal movement in $y$-direction, while the other states remain zero."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d9f627a",
   "metadata": {
    "colab": {
     "base_uri": "https://localhost:8080/",
     "height": 351
    },
    "id": "7d9f627a",
    "outputId": "f1107ea1-5d39-4945-f8f2-02fd78c858a6"
   },
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "x_data = sol.y[0, :]\n",
    "y_data = sol.y[1, :]\n",
    "theta_data = sol.y[2, :]\n",
    "t = sol.t\n",
    "\n",
    "fig = plt.figure(figsize=(10,5))\n",
    "ax1 = fig.add_subplot(121)\n",
    "ax1.plot(t, x_data)\n",
    "ax1.plot(t, y_data)\n",
    "ax1.set_xlabel(\"time in [s]\")\n",
    "ax1.set_ylabel(\"position in [m]\")\n",
    "\n",
    "# Extract controls\n",
    "u1 = [control_input(sol.y[:, i], t[i])[0] for i in range(len(t))]\n",
    "u2 = [control_input(sol.y[:, i], t[i])[0] for i in range(len(t))]\n",
    "\n",
    "ax2 = fig.add_subplot(122)\n",
    "ax2.plot(t, u1)\n",
    "ax2.plot(t, u2)\n",
    "ax2.plot(t, theta_data)\n",
    "ax2.set_xlabel(\"time in [s]\")\n",
    "ax2.set_ylabel(\"Control input\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1d721ba9",
   "metadata": {
    "id": "1d721ba9"
   },
   "source": [
    "We can also create a small visualization of the drone as follows"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3c40c5e4",
   "metadata": {
    "colab": {
     "base_uri": "https://localhost:8080/",
     "height": 684
    },
    "id": "3c40c5e4",
    "outputId": "a826bbcd-30e9-4b0f-bed9-4f09354d394f"
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "from matplotlib.animation import FuncAnimation\n",
    "from IPython.display import HTML\n",
    "\n",
    "\n",
    "def animation_func(i, data):\n",
    "    x = data[0]\n",
    "    y = data[1]\n",
    "    theta = data[2]\n",
    "    # Drone dimension: width l\n",
    "    l = 0.3\n",
    "    p1 = [x[i] + cos(theta[i]) * l/2, y[i] + sin(theta[i]) * l/2]\n",
    "    p2 = [x[i] - cos(theta[i]) * l/2, y[i] - sin(theta[i]) * l/2]\n",
    "    line1.set_xdata([p1[0], p2[0]])\n",
    "    line1.set_ydata([p1[1], p2[1]])\n",
    "    # Plot trajectory\n",
    "    pt, = ax.plot([x[i]], [y[i]], \"g.\", markersize=1)\n",
    "    \n",
    "    return line1, pt\n",
    "    \n",
    "# print(len(t))\n",
    "fig = plt.figure(figsize=(7,5))\n",
    "ax = fig.add_subplot(111)\n",
    "ax.set_xlim([-0.5, 0.5])\n",
    "ax.set_ylim([-0.5, 0.5])\n",
    "line1, = ax.plot([-0.15, 0.15], [0., 0.], 'ko-', lw=1.5, markersize=5)\n",
    "\n",
    "animation = FuncAnimation(fig, animation_func, fargs=((x_data, y_data, theta_data),),  \n",
    "                          frames=len(t), interval=20, blit=True)\n",
    "\n",
    "# render in google colab \n",
    "HTML(animation.to_html5_video())\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8524d9c6",
   "metadata": {
    "id": "8524d9c6"
   },
   "source": [
    "## Discrete-time Dynamics"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ec732e8c",
   "metadata": {
    "id": "ec732e8c"
   },
   "source": [
    "Continuous-time dynamics are most often used on a rather lower level of a robotic system, as for example in control design. The true dynamical system will of course evolve in continuous time but in order to create sophisticated planning algorithms, we are interested in abstractions of the system which simplify our calculations. A very common approach is to consider the discrete-time dynamics of a robot in which we only consider the state of the robot at specific points in time, i.e. $\\mathbf{x}_k = \\mathbf{x}(t_k)$. Another important argument why one should consider the discrete time dynamics is that our planning & control algorithms are implemented on computers & micro-controllers which cannot generate continuous time signals which means that our inputs to the robot are always on a discrete level."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "66751fcc",
   "metadata": {
    "id": "66751fcc"
   },
   "source": [
    "In many applications, we start to model our robotic system in continuous time $\\dot{\\mathbf{x}} = \\mathbf{f}(\\mathbf{x}, \\mathbf{u})$ using, e.g. Newton Euler equations, and discretize the dynamics using an explicit Euler scheme where we leverage finite differences to approximate \n",
    "$$\n",
    "    \\dot{\\mathbf{x}} \\approx \\frac{\\mathbf{x}_{k+1} - \\mathbf{x}_{k}}{\\Delta t}, \\hspace{1cm} \\Delta t = t_{k+1} -t_k\n",
    "$$\n",
    "where $\\Delta t$ is a constant sampling time. Therefore, we can obtain the time-discretized dynamics as\n",
    "$$\n",
    "    \\mathbf{x}_{k+1} = \\mathbf{x}_{k} + \\Delta t \\cdot \\mathbf{f}(\\mathbf{x}_k, \\mathbf{u}_k)\n",
    "$$\n",
    "and hence our discrete-time drone dynamics are given by\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "    x_{k+1}\\\\\n",
    "    y_{k+1}\\\\\n",
    "    \\theta_{k+1}\n",
    "    \\end{bmatrix} = \\begin{bmatrix}\n",
    "    x_k - u_1 \\Delta t sin(\\theta)\\\\\n",
    "    y_k + u_1 \\Delta t cos(\\theta)\\\\\n",
    "     \\theta _k + u_2 \\Delta t\n",
    "    \\end{bmatrix}.\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5360a8a4",
   "metadata": {
    "id": "5360a8a4"
   },
   "source": [
    "The great advantage of using discrete-time dynamics is that we can easily simulate our system since we don't need to solve a differential equation anymore."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df7c5adc",
   "metadata": {
    "id": "df7c5adc"
   },
   "outputs": [],
   "source": [
    "def discrete_dynamics(x, u):\n",
    "    \"\"\"\n",
    "    Inputs: current state of the system x, control input u\n",
    "    Output: next state vector\n",
    "    \"\"\"\n",
    "    dt = 0.1\n",
    "    # PUT YOUR CODE HERE\n",
    "#     x_next = ...\n",
    "    return x_next"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5f857fc3",
   "metadata": {
    "id": "5f857fc3"
   },
   "source": [
    "Now, we will solve our first planning problem in which we want to find a path to a desired goal position. To do so, we utilize trajectory optimization concepts. The problem we want to solve can be mathematically formulated as\n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "\t\\min_{\\mathbf{u}_{1:N-1}, \\mathbf{x}_{1:N}} \\quad &\\sum_{t=1}^{N} J(\\mathbf{x}_t, \\mathbf{u}_t)\\\\\n",
    "\t\\textrm{s.t.  } \\quad  &\\mathbf{x}_{t+1} = \\mathbf{f}\\left(\\mathbf{x}_t, \\mathbf{u}_t\\right) \\hspace{0.5cm} &&\\forall t = 1, \\cdots, N-1\\\\\n",
    "\t& \\mathbf{x}_N \\in \\mathcal{X}_g, \\mathbf{x}_0 = \\mathbf{x}(t_0)\\\\\n",
    "    \\quad &0 \\leq u_1 \\leq 0.5 &&\\forall t=1, \\cdots, N-1.\n",
    "    \\end{aligned}\n",
    "$$\n",
    "To formulate and solve the optimization problem, we make use of an efficient open source nonlinear optimization tool called CasADi. This toolbox allows us to easily formulate and solve an optimization problem without diving into the theory of optimization. Some of the structure is already given in the following codeblock, what you need to do is to define the constraints of the optimization problem."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ba9aefb7",
   "metadata": {
    "colab": {
     "base_uri": "https://localhost:8080/"
    },
    "id": "ba9aefb7",
    "outputId": "63151606-2551-4442-9ed7-f41b159e9c82"
   },
   "outputs": [],
   "source": [
    "import casadi\n",
    "\n",
    "opti = casadi.Opti()\n",
    "\n",
    "# Trajectory length\n",
    "N = 100\n",
    "dt = 0.1\n",
    "\n",
    "# Initial & Goal states\n",
    "x0 = [0., 0., 0.]\n",
    "xG = [0.8, 0.3, 0.]\n",
    "\n",
    "\n",
    "# state trajectory\n",
    "X = opti.variable(3, N)\n",
    "\n",
    "# controls\n",
    "U = opti.variable(2,N)\n",
    "\n",
    "opti.minimize(sum([(X[0, i] - xG[0])**2 + (X[1, i] - xG[1])**2 + (X[2, i] - xG[2])**2 + 5*U[0, i]**2 + 2*U[1, i]**2 for i in range(N)]))\n",
    "\n",
    "# Dynamics constraints\n",
    "for k in range(N-1):\n",
    "    opti.subject_to(X[0, k+1] - X[0, k] - U[0, k] * dt * casadi.sin(X[2, k]) == 0)\n",
    "    opti.subject_to(X[1, k+1] - X[1, k] + U[0, k] * dt * casadi.cos(X[2, k]) == 0)\n",
    "    opti.subject_to(X[2, k+1] - X[2, k] + U[1, k] * dt == 0)\n",
    "    \n",
    "# Initial State Constraints\n",
    "# ...\n",
    "\n",
    "# Goal Constraints\n",
    "# ...\n",
    "\n",
    "# Actuator saturation constraints\n",
    "# ...\n",
    "\n",
    "opti.solver('ipopt')\n",
    "sol = opti.solve()\n",
    "\n",
    "traj = sol.value(X)\n",
    "controls = sol.value(U)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "71698acc",
   "metadata": {
    "id": "71698acc"
   },
   "source": [
    "After solving the optimization problem, we can visualize our drone again:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b534b4b8",
   "metadata": {
    "colab": {
     "base_uri": "https://localhost:8080/",
     "height": 351
    },
    "id": "b534b4b8",
    "outputId": "b7b65a57-6d19-44f4-e5ab-05b54a2c271f"
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(10,5))\n",
    "ax1 = fig.add_subplot(121)\n",
    "ax1.plot(traj[0, :], traj[1, :])\n",
    "ax1.set_xlabel(\"x in [m]\")\n",
    "ax1.set_ylabel(\"y in [m]\")\n",
    "\n",
    "# Extract controls\n",
    "u1 = controls[0, :]\n",
    "u2 = controls[1, :]\n",
    "t = [i * dt for i in range(N)]\n",
    "ax2 = fig.add_subplot(122)\n",
    "ax2.plot(t, u1)\n",
    "ax2.plot(t, u2)\n",
    "ax2.set_xlabel(\"time in [s]\")\n",
    "ax2.set_ylabel(\"Control input\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "684ad270",
   "metadata": {
    "colab": {
     "base_uri": "https://localhost:8080/",
     "height": 688
    },
    "id": "684ad270",
    "outputId": "133f8495-5337-4d5f-b83e-f588feaad0df"
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(7,5))\n",
    "ax = fig.add_subplot(111)\n",
    "ax.set_xlim([-0.2, 1.])\n",
    "ax.set_ylim([-0.4, 1.])\n",
    "line1, = ax.plot([x0[0]-0.15, x0[0] + 0.15], [0., 0.], 'ko-', lw=1.5, markersize=5)\n",
    "\n",
    "x_data = traj[0, :]\n",
    "y_data = traj[1, :]\n",
    "theta_data = traj[2, :]\n",
    "animation = FuncAnimation(fig, animation_func, fargs=((x_data, y_data, theta_data),),  \n",
    "                          frames=len(t), interval=20, blit=True)\n",
    "\n",
    "HTML(animation.to_html5_video())"
   ]
  }
 ],
 "metadata": {
  "colab": {
   "provenance": []
  },
  "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.8.10"
  },
  "vscode": {
   "interpreter": {
    "hash": "4c4b768e6b91420be1aa0f18e0e2b239a7d15704d4d1fda8bee26b340664750a"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}