mpc_with_koopman_op.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Model-Based Control using Koopman Operators\n",
    "\n",
    "The Koopman operator $\\mathcal{K}$ is an infinite dimensional linear operator that directly acts on the functions of state \n",
    "$$\n",
    "    \\mathcal{K}g = g \\circ F,\n",
    "$$\n",
    "where $\\circ$ is the composition operator such that\n",
    "$$\n",
    "    \\mathcal{K}g(x(t_i)) = g(F(x(t_i))) = g(x(t_{i+1})).\n",
    "$$\n",
    "That is, the Koopman operator $\\mathcal{K}$ takes *any* observation of state $g(x(t_i))$ at time $t_i$ and evolves the function of state subject to its dynamics forward in time *linearly*. Thus, a nonlinear dynamical system can be represented by a linear Koopman operator in a lifted function space where the observations of state evolve linearly. The contribution in robotics is that now a highly nonlinear robotic system can be represented as a linear dynamical system which contains all the nonlinear information (as opposed to a Taylor expansion of the dynamics centered at an equilibrium state).\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Computing the Koopman operator with Control inputs\n",
    "The Koopman operator $\\mathcal{K}$ is infeasible to compute in the infinite dimensional space. A finite subspace approximation to the operator $\\mathfrak{K} \\in \\mathbb{R}^c \\times \\mathbb{R}^c$ acting on $\\mathcal{C} \\subset \\mathbb{C}$ is used instead. Here, we define a subset of function observables (or observations of state)\n",
    "$z(x) : \\mathbb{R}^n \\in \\mathbb{R}^c \\subset \\mathcal{C}$. The output dimension of $z(x)$ will now define the finite subspace that spance $\\mathcal{C} \\subset \\mathbb{C}$.\n",
    "The operator $\\mathfrak{K}$ acting on $z(x(t_i))$ is then represented in discrete time as\n",
    "$$\n",
    "    z(x(t_{i+1})) = \\mathfrak{K} z(x(t_i)) + r(x(t_i))\n",
    "$$\n",
    "where $r(x) \\in \\mathbb{C}$ is the residual function error. Adding control to the Koopman operator representation is done by augmenting the state $x(t)$ with the input control to the system $u(t)$ where now the function observables become $z(x, u)$ and the approximate Koopman operator is given by \n",
    "$$\n",
    "    z(x(t_{i+1}), u(t_{i+1})) = \\mathfrak{K} z(x(t_i), u(t_i)) + r(x(t_i), u(t_i))\n",
    "$$\n",
    "In principle, as $c \\to \\infty$, the residual error goes to zero; however, it is sometimes possible to find $c < \\infty$ such that $r(x) = 0$. This is known as a Koopman invariant subspace, but for our purposes we won't go into too much detail.\n",
    "\n",
    "The continuous time Koopman operator is calculated by taking the matrix logarithm as $t_{i+1} - t_i \\to 0$ where we overload the notation for the Koopman operator:\n",
    "$$\n",
    "\\dot{z}(x(t), u(t)) = \\mathfrak{K} z(x(t), u(t)) + r(x(t), u(t)).\n",
    "$$\n",
    "\n",
    "We are first going to write a class for the Van der Pol dynamical system which has the following differential equation:\n",
    "$$\n",
    "    \\frac{d}{dt} \\begin{bmatrix} x_1 \\\\ x_2  \\end{bmatrix} = \\begin{bmatrix} x_2 \\\\ -x_1 + \\epsilon (1 - x_1^2)x_2 + u \\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:34.488654Z",
     "start_time": "2019-04-17T19:01:34.402294Z"
    }
   },
   "outputs": [],
   "source": [
    "import autograd.numpy as np\n",
    "from autograd import jacobian\n",
    "from scipy.linalg import logm\n",
    "import scipy.linalg\n",
    "\n",
    "class VanDerPol(object):\n",
    "    def __init__(self, eps=1.0, dt=0.01):\n",
    "        self.eps = eps\n",
    "        self.dt = dt\n",
    "        self.dfdx = jacobian(self.f, argnum=0)\n",
    "        self.dfdu = jacobian(self.f, argnum=1)\n",
    "        self.num_states = 2\n",
    "        self.num_actions = 2\n",
    "        self.reset()\n",
    "    def reset(self):\n",
    "        self.state = np.random.uniform(-3., 3., \n",
    "                                       size=(self.num_states,))\n",
    "        return self.state.copy() # return a copy\n",
    "    def sample_action(self):\n",
    "        return np.random.uniform(-3., 3.,\n",
    "                                size=(self.num_actions,))\n",
    "    def f(self, x, u):\n",
    "        xdot = np.array([\n",
    "            x[1],\n",
    "            -x[0] + self.eps * (1 - x[0]**2) * x[1] + u[0]\n",
    "        ])\n",
    "        return xdot\n",
    "    def get_linearization(self, x, u):\n",
    "        return self.dfdx(x, u), self.dfdu(x, u)\n",
    "    def step(self, u): ## RK4 step\n",
    "        k1 = self.f(self.state, u) * self.dt\n",
    "        k2 = self.f(self.state + k1/2.0, u) * self.dt\n",
    "        k3 = self.f(self.state + k2/2.0, u) * self.dt\n",
    "        k4 = self.f(self.state + k3, u) * self.dt\n",
    "        self.state = self.state + (k1 + 2 * (k2 + k3) + k4)/6\n",
    "#         self.state = self.state + self.f(self.state, u) * self.dt # euler integration\n",
    "        return self.state.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the Koopman operator system, we are going to use the following basis functions:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:35.564909Z",
     "start_time": "2019-04-17T19:01:35.561202Z"
    }
   },
   "outputs": [],
   "source": [
    "num_x_obs = 4\n",
    "num_u_obs = 1\n",
    "num_obs = num_x_obs + num_u_obs\n",
    "def z(x, u):\n",
    "    return np.array([x[0], x[1], x[0]**2, (x[0]**2)*x[1], u[0]])\n",
    "\n",
    "zdu = jacobian(z, argnum=1) ## needed for functions where the control is not seperable \n",
    "vdp = VanDerPol() # create the dynamic system"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Provided a data set $\\mathcal{D} = \\{(x(t_m), u(t_m), x(t_{m+1}), u(t_{m+1})) \\}_{m=0}^{M-1}$, we can compute the approximate Koopman operator $\\mathfrak{K}$ using least-squares minimization over the matrix $\\mathfrak{K}$:\n",
    "$$\n",
    "    \\min_{\\mathfrak{K}} \\frac{1}{2 M} \\sum_{m=0}^{M-1} \\Vert z(x(t_{m+1}), u(t_{m+1})) - \\mathfrak{K} z(x(t_m), u(t_m)) \\Vert^2.\n",
    "$$\n",
    "Since this optimization is convex in $\\mathfrak{K}$, the solution is given by\n",
    "$$\n",
    "    \\mathfrak{K} = A G^\\dagger\n",
    "$$        \n",
    "where $\\dagger$ denotes the Moore-Penrose pseudoinverse and\n",
    "$$\n",
    "A = \\frac{1}{M} \\sum_{m=0}^{M-1} z(x(t_{m+1}), u(t_{m+1})) z(x(t_m), u(t_m))^\\top, \\\\\n",
    "            G = \\frac{1}{M} \\sum_{m=0}^{M-1} z(x(t_m), u(t_m)) z(x(t_m), u(t_m))^\\top .\n",
    "$$\n",
    "The continuous time operator is then given by $\\log(\\mathfrak{K})/t_s$.\n",
    "Note that we can solve the optimization using gradient descent methods or other\n",
    "optimization methods.\n",
    "\n",
    "In the next cell, we will collect some data and learn a Koopman operator for the functions of state described previously."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:36.951096Z",
     "start_time": "2019-04-17T19:01:36.663506Z"
    }
   },
   "outputs": [],
   "source": [
    "np.random.seed(203) ## set the seed for reproducibility \n",
    "num_trials = 10 ## number of resets\n",
    "horizon = 200 ## how long we simulate the system for\n",
    "M = num_trials * horizon ## M sized data\n",
    "\n",
    "A = np.zeros((num_obs, num_obs))\n",
    "G = np.zeros((num_obs, num_obs))\n",
    "\n",
    "## loop to collect data\n",
    "for k in range(num_trials):\n",
    "    x_t = vdp.reset() # reset the state \n",
    "    u_t = vdp.sample_action()\n",
    "    for t in range(horizon):\n",
    "        x_tpo = vdp.step(u_t)\n",
    "        u_tpo = u_t\n",
    "        \n",
    "        A += np.outer(z(x_tpo, u_tpo), z(x_t, u_t))\n",
    "        G += np.outer(z(x_t, u_t), z(x_t, u_t))\n",
    "        \n",
    "        # reset for next loop\n",
    "        x_t = x_tpo\n",
    "        u_t = u_tpo\n",
    "A /= M\n",
    "G /= M\n",
    "koopman_operator = np.dot(A, np.linalg.pinv(G))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we calculate the continuous time Koopman operator."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:37.889137Z",
     "start_time": "2019-04-17T19:01:37.863820Z"
    }
   },
   "outputs": [],
   "source": [
    "cont_koopman_operator = logm(koopman_operator)/vdp.dt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For optimal control with the Koopman operator, we split $\\mathfrak{K}$ into the free dynamics $A$ and dynamic actuation matrix $B$. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:38.992743Z",
     "start_time": "2019-04-17T19:01:38.988317Z"
    }
   },
   "outputs": [],
   "source": [
    "A = cont_koopman_operator[:num_x_obs,:num_x_obs]\n",
    "B = cont_koopman_operator[:num_x_obs, num_x_obs:]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-16T16:04:20.527780Z",
     "start_time": "2019-04-16T16:04:20.521669Z"
    }
   },
   "source": [
    "Since the Koopman operator representation of the dynamical system is linear, we can directly apply LQR control."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:39.868307Z",
     "start_time": "2019-04-17T19:01:39.863311Z"
    }
   },
   "outputs": [],
   "source": [
    "Q = np.diag([1.0, 1.0, 0., 0.])\n",
    "R = np.diag([1.0]) * 1e-2\n",
    "P = scipy.linalg.solve_continuous_are(A, B, Q, R)\n",
    "Klqr = np.linalg.inv(R).dot(B.T.dot(P))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The controller can then be tested on the true actuated dynamical system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:40.792304Z",
     "start_time": "2019-04-17T19:01:40.743470Z"
    }
   },
   "outputs": [],
   "source": [
    "state = vdp.reset() # reset the state\n",
    "u_def = vdp.sample_action() # sample the action to get a dummy input\n",
    "target_z = z(0.*state, u_def)[:num_x_obs] # transform the target state into the lifted functions\n",
    "horizon = 500 # simulation time\n",
    "trajectory = np.zeros((horizon, vdp.num_states))\n",
    "for t in range(horizon):\n",
    "    u = -np.dot(Klqr, z(state, u_def)[:num_x_obs] - target_z)\n",
    "    state = vdp.step(u)\n",
    "    trajectory[t,:] = state"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we visualize the resulting trajectory from applying a model-based controller using the Koopman operator representation of the dynamical system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-04-17T19:01:46.496016Z",
     "start_time": "2019-04-17T19:01:46.381919Z"
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "plt.plot(trajectory)\n",
    "plt.xlabel('t')\n",
    "plt.ylabel('x_1, x_2')\n",
    "plt.show()"
   ]
  }
 ],
 "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"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": true,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}