optimization.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This notebook provides examples to go along with the [textbook](https://underactuated.csail.mit.edu/optimization.html).  I recommend having both windows open, side-by-side!\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from IPython.display import display\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "from pydrake.all import MathematicalProgram, Solve"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "# SDP relaxation of non-convex quadratic constraints\n",
    "\n",
    "Consider the problem:\n",
    "\n",
    "\\begin{align*}\n",
    "\\min_x \\quad & \\| x - a \\|^2 \\\\\n",
    "\\text{subject to} \\quad & \\| x - b \\| \\ge 1\n",
    "\\end{align*}\n",
    "\n",
    "We can write this as\n",
    "\n",
    "\\begin{align*}\n",
    "\\min_{x,y} \\quad & y - 2ax + a^2 \\\\\n",
    "\\text{subject to} \\quad & y - 2bx + b^2 \\ge 1 \\\\\n",
    "& y \\ge x^2\n",
    "\\end{align*}\n",
    "\n",
    "where we write $y \\ge x^2$ as the semidefinite constraint\n",
    "$\\begin{bmatrix} y & x \\\\ x & 1 \\end{bmatrix}.$  I've plotted the feasible\n",
    "region and the objective with an arrow.  As you know, for linear\n",
    "objectives, the optimal solution will lie on the boundary only if the cost is\n",
    "directly orthogonal to the objective; otherwise it will lie at a vertex.  So in\n",
    "this case, the solution will only lie on the interior if $a = b;$ for every\n",
    "other value, this relaxation will give the optimal solution.  (Try changing $a$ to see)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "def sdf_nonconvex_quadratic():\n",
    "    fig, ax = plt.subplots()\n",
    "    a = 0.8\n",
    "    b = 0.5\n",
    "    x = np.linspace(-2, 2)\n",
    "    plt.fill(x, np.maximum(x * x, 1 + 2 * b * x - b * b), color=\"lightgray\")\n",
    "    ax.plot(x, x * x, \"k\")\n",
    "    ax.plot(x, 1 + 2 * b * x - b * b, \"k\")\n",
    "    qx = 0\n",
    "    qy = 3\n",
    "    scale = 4\n",
    "    ax.quiver(qx, qy, 2 * a, -1, scale=10, zorder=2, color=\"b\")\n",
    "\n",
    "    ax.set_xlim(-2, 2)\n",
    "    ax.set_ylim(0, 4)\n",
    "    ax.set_xlabel(\"x\")\n",
    "    ax.set_ylabel(\"y\")\n",
    "\n",
    "    prog = MathematicalProgram()\n",
    "    x = prog.NewContinuousVariables(1, \"x\")[0]\n",
    "    y = prog.NewContinuousVariables(1, \"y\")[0]\n",
    "\n",
    "    prog.AddPositiveSemidefiniteConstraint(np.array([[y, x], [x, 1]]))\n",
    "    prog.AddLinearConstraint(y - 2 * b * x + b * b >= 1)\n",
    "    prog.AddLinearCost(y - 2 * a * x + a * a)\n",
    "\n",
    "    result = Solve(prog)\n",
    "    ax.plot(result.GetSolution(x), result.GetSolution(y), \"b*\", markersize=15)\n",
    "    display(plt.show())\n",
    "\n",
    "\n",
    "sdf_nonconvex_quadratic()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Global optimization with Sums-of-Squares (the \"Six Hump Camel\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def sos_six_hump_camel():\n",
    "    prog = MathematicalProgram()\n",
    "    v = prog.NewIndeterminates(2, \"x\")\n",
    "    x = v[0]\n",
    "    y = v[1]\n",
    "\n",
    "    # This is the famous \"six-hump camel back function\".  It has six local\n",
    "    # minima, two of them being global minima.\n",
    "    p = (\n",
    "        4 * x**2\n",
    "        + x * y\n",
    "        - 4 * y**2\n",
    "        - 2.1 * x**4\n",
    "        + 4 * y**4\n",
    "        + x**6 / 3\n",
    "    )\n",
    "\n",
    "    # Find the minimum value by adding a sums of squares constraint, via\n",
    "    #   for all x, p(x) >= pmin\n",
    "    # which we write as\n",
    "    #   p(x) - pmin is sos.\n",
    "    pmin = prog.NewContinuousVariables(1, \"pmin\")[0]\n",
    "    prog.AddSosConstraint(p - pmin)\n",
    "\n",
    "    # Maximize pmin.\n",
    "    prog.AddCost(-pmin)\n",
    "\n",
    "    result = Solve(prog)\n",
    "    assert result.is_success()\n",
    "    print(\"Minimum value (lower bound): \" + str(result.GetSolution(pmin)))\n",
    "\n",
    "    # Now, let's plot it.\n",
    "    fig = plt.figure(figsize=(10, 5))\n",
    "    ax0 = fig.add_subplot(121, projection=\"3d\")\n",
    "    ax1 = fig.add_subplot(122)\n",
    "    xs = np.linspace(-2.2, 2.2, 51)\n",
    "    ys = np.linspace(-1.2, 1.2, 51)\n",
    "    [X, Y] = np.meshgrid(xs, ys)\n",
    "    P = (\n",
    "        4 * X**2\n",
    "        + X * Y\n",
    "        - 4 * Y**2\n",
    "        - 2.1 * X**4\n",
    "        + 4 * Y**4\n",
    "        + X**6 / 3\n",
    "    )\n",
    "    ax0.plot_surface(X, Y, P)\n",
    "    ax1.contour(X, Y, P, 100)\n",
    "\n",
    "    print(\"Minimum sampled value: \" + str(np.min(P)))\n",
    "    display(plt.show())\n",
    "\n",
    "\n",
    "sos_six_hump_camel()"
   ]
  },
  {
   "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.11.2"
  },
  "vscode": {
   "interpreter": {
    "hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}