{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "numerous-rochester",
   "metadata": {},
   "source": [
    "# Introduction to the Python Control Systems Library (python-control)\n",
    "\n",
    "## Input/Output Systems"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "69bdd3af",
   "metadata": {},
   "source": [
    "Richard M. Murray, 13 Nov 2021 (updated 5 Jan 2023)\n",
    "\n",
    "This notebook contains an introduction to the basic operations in the Python Control Systems Library (python-control), a Python package for control system design.  This notebook is focused on state space control design for a kinematic car, including trajectory generation and gain-scheduled feedback control.  This illustrates the use of the input/output (I/O) system class, which can be used to construct models for nonlinear control systems."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "macro-vietnamese",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import the packages needed for the examples included in this notebook\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import control as ct\n",
    "print(\"python-control version:\", ct.__version__)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "distinct-communist",
   "metadata": {},
   "source": [
    "### Installation hints\n",
    "\n",
    "If you get an error importing the `control` package, it may be that it is not in your current Python path.  You can fix this by setting the PYTHONPATH environment variable to include the directory where the python-control package is located.  If you are invoking Jupyter from the command line, try using a command of the form\n",
    "\n",
    "    PYTHONPATH=/path/to/control jupyter notebook\n",
    "    \n",
    "If you are using [Google Colab](https://colab.research.google.com), use the following command at the top of the notebook to install the `control` package:\n",
    "\n",
    "    !pip install control\n",
    "    \n",
    "For the examples below, you will need version 0.9.3 or higher of the python-control toolbox.  You can find the version number using the command\n",
    "\n",
    "    print(ct.__version__)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5dad04d8",
   "metadata": {},
   "source": [
    "### More information on Python, NumPy, python-control\n",
    "\n",
    "* [Python tutorial](https://docs.python.org/3/tutorial/)\n",
    "* [NumPy tutorial](https://numpy.org/doc/stable/user/quickstart.html)\n",
    "* [NumPy for MATLAB users](https://numpy.org/doc/stable/user/numpy-for-matlab-users.html), \n",
    "* [Python Control Systems Library (python-control) documentation](https://python-control.readthedocs.io/en/0.9.3.post2/)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "novel-geology",
   "metadata": {},
   "source": [
    "## System Definiton\n",
    "\n",
    "We now define the dynamics of the system that we are going to use for the control design.  The dynamics of the system will be of the form\n",
    "\n",
    "$$\n",
    "\\dot x = f(x, u), \\qquad y = h(x, u)\n",
    "$$\n",
    "\n",
    "where $x$ is the state vector for the system, $u$ represents the vector of inputs, and $y$ represents the vector of outputs.\n",
    "\n",
    "The python-control package allows definition of input/output sytems using the `InputOutputSystem` class and its various subclasess, including the `NonlinearIOSystem` class that we use here.  A `NonlinearIOSystem` object is created by defining the update law ($f(x, u)$) and the output map ($h(x, u)$).\n",
    "\n",
    "For the example in this notebook, we will be controlling the steering of a vehicle, using a \"bicycle\" model for the dynamics of the vehicle.  A more complete description of the dynamics of this system are available in [Example 3.11](https://fbswiki.org/wiki/index.php/System_Modeling) of [_Feedback Systems_](https://fbswiki.org/wiki/index.php/FBS) by Astrom and Murray (2020)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "sufficient-douglas",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define the update rule for the system, f(x, u)\n",
    "# States: x, y, theta (postion and angle of the center of mass)\n",
    "# Inputs: v (forward velocity), delta (steering angle)\n",
    "def vehicle_update(t, x, u, params):\n",
    "    # Get the parameters for the model\n",
    "    a = params.get('refoffset', 1.5)        # offset to vehicle reference point\n",
    "    b = params.get('wheelbase', 3.)         # vehicle wheelbase\n",
    "    maxsteer = params.get('maxsteer', 0.5)  # max steering angle (rad)\n",
    "\n",
    "    # Saturate the steering input\n",
    "    delta = np.clip(u[1], -maxsteer, maxsteer)\n",
    "    alpha = np.arctan2(a * np.tan(delta), b)\n",
    "\n",
    "    # Return the derivative of the state\n",
    "    return np.array([\n",
    "        u[0] * np.cos(x[2] + alpha),    # xdot = cos(theta + alpha) v\n",
    "        u[0] * np.sin(x[2] + alpha),    # ydot = sin(theta + alpha) v\n",
    "        (u[0] / a) * np.sin(alpha)      # thdot = v sin(alpha) / a\n",
    "    ])\n",
    "\n",
    "# Define the readout map for the system, h(x, u)\n",
    "# Outputs: x, y (planar position of the center of mass)\n",
    "def vehicle_output(t, x, u, params):\n",
    "    return x\n",
    "\n",
    "# Default vehicle parameters (including nominal velocity)\n",
    "vehicle_params={'refoffset': 1.5, 'wheelbase': 3, 'velocity': 15, \n",
    "                'maxsteer': 0.5}\n",
    "\n",
    "# Define the vehicle steering dynamics as an input/output system\n",
    "vehicle = ct.NonlinearIOSystem(\n",
    "    vehicle_update, vehicle_output, states=3, name='vehicle',\n",
    "    inputs=['v', 'delta'], outputs=['x', 'y', 'theta'], params=vehicle_params)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "intellectual-democrat",
   "metadata": {},
   "source": [
    "## Open loop simulation\n",
    "\n",
    "After these operations, the `vehicle` object references the nonlinear model for the system.  This system can be simulated to compute a trajectory for the system.  Here we command a velocity of 10 m/s and turn the wheel back and forth at one Hertz."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "likely-hindu",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define the time interval that we want to use for the simualation\n",
    "timepts = np.linspace(0, 10, 1000)\n",
    "\n",
    "# Define the inputs\n",
    "U = [\n",
    "    10 * np.ones_like(timepts),          # velocity\n",
    "    0.1 * np.sin(timepts * 2*np.pi)      # steering angle\n",
    "]\n",
    "\n",
    "# Simulate the system dynamics, starting from the origin\n",
    "time, outputs = ct.input_output_response(vehicle, timepts, U, 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dutch-charm",
   "metadata": {},
   "source": [
    "We plot the results using standard `matplotlib` commands:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "piano-algeria",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create a figure to plot the results\n",
    "fig, ax = plt.subplots(2, 1)\n",
    "\n",
    "# Plot the results in the xy plane\n",
    "ax[0].plot(outputs[0], outputs[1])\n",
    "ax[0].set_xlabel(\"$x$ [m]\")\n",
    "ax[0].set_ylabel(\"$y$ [m]\")\n",
    "\n",
    "# Plot the inputs\n",
    "ax[1].plot(timepts, U[0])\n",
    "ax[1].set_ylim(0, 12)\n",
    "ax[1].set_xlabel(\"Time $t$ [s]\")\n",
    "ax[1].set_ylabel(\"Velocity $v$ [m/s]\")\n",
    "ax[1].yaxis.label.set_color('blue')\n",
    "\n",
    "rightax = ax[1].twinx()       # Create an axis in the right\n",
    "rightax.plot(timepts, U[1], color='red')\n",
    "rightax.set_ylim(None, 0.5)\n",
    "rightax.set_ylabel(\"Steering angle $\\phi$ [rad]\")\n",
    "rightax.yaxis.label.set_color('red')\n",
    "\n",
    "fig.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "alone-worry",
   "metadata": {},
   "source": [
    "Notice that there is a small drift in the $y$ position despite the fact that the steering wheel is moved back and forth symmetrically around zero.  Exercise: explain what might be happening."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "portable-rubber",
   "metadata": {},
   "source": [
    "## Linearize the system around a trajectory\n",
    "\n",
    "We choose a straight path along the $x$ axis at a speed of 10 m/s as our desired trajectory and then linearize the dynamics around the initial point in that that trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "surprising-algorithm",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create the desired trajectory \n",
    "Ud = np.array([10 * np.ones_like(timepts), np.zeros_like(timepts)])\n",
    "Xd = np.array([10 * timepts, 0 * timepts, np.zeros_like(timepts)])\n",
    "\n",
    "# Now linizearize the system around this trajectory\n",
    "linsys = vehicle.linearize(Xd[:, 0], Ud[:, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "protecting-committee",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Check on the eigenvalues of the open loop system\n",
    "np.linalg.eigvals(linsys.A)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "trying-stereo",
   "metadata": {},
   "source": [
    "We see that all eigenvalues are zero, corresponding to a single integrator in the $x$ (longitudinal) direction and a double integrator in the $y$ (lateral) direction."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "pressed-delta",
   "metadata": {},
   "source": [
    "## Compute a state space (LQR) control law\n",
    "\n",
    "We can now compute a feedback controller around the trajectory.  We choose a simple LQR controller here, but any method can be used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "auburn-caribbean",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute LQR controller\n",
    "K, S, E = ct.lqr(linsys, np.diag([1, 1, 1]), np.diag([1, 1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "independent-lafayette",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Check on the eigenvalues of the closed loop system\n",
    "np.linalg.eigvals(linsys.A - linsys.B @ K)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "handmade-moral",
   "metadata": {},
   "source": [
    "The closed loop eigenvalues have negative real part, so the closed loop (linear) system will be stable about the operating trajectory."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "handy-virgin",
   "metadata": {},
   "source": [
    "## Create a controller with feedforward and feedback\n",
    "\n",
    "We now create an I/O system representing the control law.  The controller takes as an input the desired state space trajectory $x_\\text{d}$ and the nominal input $u_\\text{d}$.  It outputs the control law\n",
    "\n",
    "$$\n",
    "u = u_\\text{d} - K(x - x_\\text{d}).\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "negative-scope",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define the output rule for the controller\n",
    "# States: none (=> no update rule required)\n",
    "# Inputs: z = [xd, ud, x]\n",
    "# Outputs: v (forward velocity), delta (steering angle)\n",
    "def control_output(t, x, z, params):\n",
    "    # Get the parameters for the model\n",
    "    K = params.get('K', np.zeros((2, 3)))   # nominal gain\n",
    "    \n",
    "    # Split up the input to the controller into the desired state and nominal input\n",
    "    xd_vec = z[0:3]    # desired state ('xd', 'yd', 'thetad')\n",
    "    ud_vec = z[3:5]    # nominal input ('vd', 'deltad')\n",
    "    x_vec = z[5:8]     # current state ('x', 'y', 'theta')\n",
    "    \n",
    "    # Compute the control law\n",
    "    return ud_vec - K @ (x_vec - xd_vec)\n",
    "\n",
    "# Define the controller system\n",
    "control = ct.NonlinearIOSystem(\n",
    "    None, control_output, name='control',\n",
    "    inputs=['xd', 'yd', 'thetad', 'vd', 'deltad', 'x', 'y', 'theta'], \n",
    "    outputs=['v', 'delta'], params={'K': K})"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "affected-motor",
   "metadata": {},
   "source": [
    "Because we have named the signals in both the vehicle model and the controller in a compatible way, we can use the autoconnect feature of the `interconnect()` function to create the closed loop system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "stock-regression",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Build the closed loop system\n",
    "vehicle_closed = ct.interconnect(\n",
    "    (vehicle, control),\n",
    "    inputs=['xd', 'yd', 'thetad', 'vd', 'deltad'],\n",
    "    outputs=['x', 'y', 'theta']\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "hispanic-monroe",
   "metadata": {},
   "source": [
    "## Closed loop simulation\n",
    "\n",
    "We now command the system to follow in trajectory and use the linear controller to correct for any errors. \n",
    "\n",
    "The desired trajectory is a given by a longitudinal position that tracks a velocity of 10 m/s for the first 5 seconds and then increases to 12 m/s and a lateral position that varies sinusoidally by $\\pm 0.5$ m around the centerline.  The nominal inputs are not modified, so that feedback is required to obtained proper trajectory tracking."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "american-return",
   "metadata": {},
   "outputs": [],
   "source": [
    "Xd = np.array([\n",
    "    10 * timepts + 2 * (timepts-5) * (timepts > 5), \n",
    "    0.5 * np.sin(timepts * 2*np.pi), \n",
    "    np.zeros_like(timepts)\n",
    "])\n",
    "\n",
    "Ud = np.array([10 * np.ones_like(timepts), np.zeros_like(timepts)])\n",
    "\n",
    "# Simulate the system dynamics, starting from the origin\n",
    "resp = ct.input_output_response(\n",
    "    vehicle_closed, timepts, np.vstack((Xd, Ud)), 0)\n",
    "time, outputs = resp.time, resp.outputs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "indirect-longitude",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the results in the xy plane\n",
    "plt.plot(Xd[0], Xd[1], 'b--')      # desired trajectory\n",
    "plt.plot(outputs[0], outputs[1])   # actual trajectory\n",
    "plt.xlabel(\"$x$ [m]\")\n",
    "plt.ylabel(\"$y$ [m]\")\n",
    "plt.ylim(-1, 2)\n",
    "\n",
    "# Add a legend\n",
    "plt.legend(['desired', 'actual'], loc='upper left')\n",
    "\n",
    "# Compute and plot the velocity\n",
    "rightax = plt.twinx()       # Create an axis in the right\n",
    "rightax.plot(Xd[0, :-1], np.diff(Xd[0]) / np.diff(timepts), 'r--')\n",
    "rightax.plot(outputs[0, :-1], np.diff(outputs[0]) / np.diff(timepts), 'r-')\n",
    "rightax.set_ylim(0, 13)\n",
    "rightax.set_ylabel(\"$x$ velocity [m/s]\")\n",
    "rightax.yaxis.label.set_color('red')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "weighted-directory",
   "metadata": {},
   "source": [
    "We see that there is a small error in each axis.  By adjusting the weights in the LQR controller we can adjust the steady state error (try it!)"
   ]
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}