{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "f8bfc15c",
   "metadata": {},
   "source": [
    "# Linear Quadratic Regulator Example\n",
    "\n",
    "Richard M. Murray, 25 Jan 2022\n",
    "\n",
    "This notebook contains an example of LQR control applied to the PVTOL system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c120d65c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "77e2ed47",
   "metadata": {},
   "source": [
    "## System description\n",
    "\n",
    "We use the PVTOL dynamics from the textbook, which are contained in the file `pvtol`.  The vehicle model is both an I/O system model and a flat system model (for the case when the viscous damping coefficient $c$ is zero)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7adc6cf1",
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import YouTubeVideo\n",
    "display(YouTubeVideo('ZFb5kFpgCm4', width=640, height=480))\n",
    "\n",
    "from pvtol import pvtol, plot_results\n",
    "print(pvtol)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea50d7cd",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Find the equilibrium point corresponding to hover\n",
    "xeq, ueq = ct.find_eqpt(pvtol, np.zeros(6), np.zeros(2), np.zeros(6), iy=[1, 2])\n",
    "# print(ct.StateSpace.__str__(pvtol.linearize(xeq, ueq)))\n",
    "ueq"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7cb8840b",
   "metadata": {},
   "source": [
    "## Linear Quadratic Regulator Design\n",
    "\n",
    "Linearizing the system around an equilibrium point, we can apply LQR to obtain a stabilizing compensator."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b3cc8857",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get the linearized dynamics\n",
    "sys = pvtol.linearize(xeq, ueq)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5cfa1ba7",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start with a diagonal weighting\n",
    "Qx1 = np.diag([1, 1, 1, 1, 1, 1])\n",
    "Qu1a = np.diag([1, 1])\n",
    "K, X, E = ct.lqr(sys, Qx1, Qu1a)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "863d07de",
   "metadata": {},
   "source": [
    "To create a controller for the system, we need to create an I/O system that takes in the desired trajectory $(x_\\text{d}, u_\\text{d})$ and the current state $x$ and generates the control law\n",
    "\n",
    "$$\n",
    "u = u_\\text{d} - K (x - x_\\text{d})\n",
    "$$\n",
    "\n",
    "The function `create_statefbk_iosystem()` from the `ctrlutil` package does this.  (Take a look inside [`ctrlutil.py`](/edit/python/ctrlutil.py) to see what is going on.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c19bfdc5",
   "metadata": {},
   "outputs": [],
   "source": [
    "import ctrlutil as ct_\n",
    "print(ct_.create_statefbk_iosystem.__doc__)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5db704e6",
   "metadata": {},
   "outputs": [],
   "source": [
    "control, pvtol_closed = ct_.create_statefbk_iosystem(pvtol, K)\n",
    "print(control, \"\\n\")\n",
    "print(pvtol_closed)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bedcb0c0",
   "metadata": {},
   "source": [
    "## Closed loop system simulation\n",
    "\n",
    "We now generate a trajectory for the system and track that trajectory.\n",
    "\n",
    "For this simple example, we take the system input to be a \"step\" input that moves the system 1 meter to the right.  More complex trajectories (eg, using the results from HW #3) could also be used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a497aa2c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate a step response by setting xd, ud\n",
    "Tf = 15\n",
    "T = np.linspace(0, Tf, 100)\n",
    "xd = np.outer(np.array([1, 0, 0, 0, 0, 0]), np.ones_like(T))\n",
    "ud = np.outer(ueq, np.ones_like(T))\n",
    "ref = np.vstack([xd, ud])\n",
    "\n",
    "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f014e660",
   "metadata": {},
   "source": [
    "The limitations of the linear controlller can be seen if we take a larger step, say 10 meters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a141f100",
   "metadata": {},
   "outputs": [],
   "source": [
    "xd = np.outer(np.array([10, 0, 0, 0, 0, 0]), np.ones_like(T))\n",
    "ref = np.vstack([xd, ud])\n",
    "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8adb6ff4",
   "metadata": {},
   "source": [
    "A better trajectory can be obtained by using a (nonfeasible) trajectory that goes from 0 to 10 in 10 seconds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a075a0a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "xf = np.array([10, 0, 0, 0, 0, 0])\n",
    "xd = np.array([xf/10 * t if t < 10 else xf for t in T]).T\n",
    "ref = np.vstack([xd, ud])\n",
    "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ae2f5fb2",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}