{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import scipy.optimize\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Problem 3: Reformulating an $\\mathcal{l}_1$-norm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "N = 5\n",
    "b = np.random.randn(N)\n",
    "print('b = {}\\n'.format(b))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "c = np.concatenate([np.zeros(N), np.ones(N)])\n",
    "I = np.eye(N)\n",
    "A_ub = np.block([[I, -I],\n",
    "                 [-I, -I]])\n",
    "b_ub = np.concatenate([b, -b])\n",
    "\n",
    "sol = scipy.optimize.linprog(c, A_ub, b_ub, bounds = (None, None), options = {'disp': True})\n",
    "x = sol.x[:N]\n",
    "print(sol)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Problem 2:  Minimum fuel optimal control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "dt = .1\n",
    "N = 100\n",
    "A = np.array([[1., dt],\n",
    "              [0., 1.]])\n",
    "b = np.array([0., dt])\n",
    "x0 = np.array([0., 0.])\n",
    "x_des = np.array([-6.5, 0.])\n",
    "\n",
    "def step(x, u):\n",
    "    return A@x + b*u"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = x0\n",
    "x_traj = [x0]\n",
    "for t in range(N):\n",
    "#     x = step(x, u=1)\n",
    "    x = step(x, u=np.cos(dt*t))\n",
    "    x_traj.append(x)\n",
    "x_traj = np.stack(x_traj)\n",
    "# print(x_traj)\n",
    "\n",
    "fig = plt.figure(figsize=(8,8))\n",
    "\n",
    "ax1 = fig.add_subplot(211)\n",
    "ax1.plot(dt*np.arange(N+1), x_traj[:,0], label = 'pos')\n",
    "ax1.plot(dt*np.arange(N+1), x_traj[:,1], label = 'vel')\n",
    "ax1.grid()\n",
    "ax1.legend()\n",
    "plt.xlabel('time')\n",
    "\n",
    "\n",
    "ax2 = fig.add_subplot(212)\n",
    "ax2.plot(x_traj[:,0], x_traj[:,1])\n",
    "ax2.grid()\n",
    "plt.xlabel('pos')\n",
    "plt.ylabel('vel')\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "c = np.concatenate([np.zeros(N), np.ones(N)])\n",
    "\n",
    "A_eq = []\n",
    "tmp = b\n",
    "for t in range(N):\n",
    "    A_eq.append(tmp)\n",
    "    tmp = A@tmp\n",
    "    \n",
    "A_eq = np.stack(A_eq[::-1]).T\n",
    "A_eq = np.concatenate([A_eq, np.zeros((2,N))], axis=1)\n",
    "b_eq = x_des\n",
    "I = np.eye(N)\n",
    "A_ub = np.block([[I, -I],\n",
    "                 [-I, -I],\n",
    "                 [2*I, -I],\n",
    "                 [-2*I, -I]])\n",
    "b_ub = np.concatenate([np.zeros(2*N), np.ones(2*N)])\n",
    "# print(A_ub.shape, b_ub.shape)\n",
    "# print('c = \\n{}, \\n\\nA_eq = \\n{},\\n\\nb_eq =\\n{},\\n\\nA_ub = \\n{},\\n\\nb_ub =\\n{} '.format(c, A_eq, b_eq, A_ub, b_ub))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = scipy.optimize.linprog(c, A_ub, b_ub, A_eq, b_eq, bounds = (None, None), options = {'disp': True})\n",
    "u_opt = sol.x[:N]\n",
    "print(u_opt)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = x0\n",
    "x_traj = [x0]\n",
    "for t in range(N):\n",
    "    x = step(x, u=u_opt[t])\n",
    "    x_traj.append(x)\n",
    "x_traj = np.stack(x_traj)\n",
    "\n",
    "fig = plt.figure(figsize=(8,8))\n",
    "ax1 = fig.add_subplot(211)\n",
    "ax1.plot(dt*np.arange(N+1), x_traj[:,0], label = 'pos')\n",
    "ax1.plot(dt*np.arange(N+1), x_traj[:,1], label = 'vel')\n",
    "ax1.plot(dt*np.arange(N), u_opt, label = 'con')\n",
    "ax1.grid()\n",
    "ax1.legend()\n",
    "plt.xlabel('time')\n",
    "\n",
    "\n",
    "ax2 = fig.add_subplot(212)\n",
    "ax2.plot(x_traj[:,0], x_traj[:,1])\n",
    "ax2.grid()\n",
    "plt.xlabel('pos')\n",
    "plt.ylabel('vel')\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}