add min time double integrator with GCS example

book/optimization.html

@@ -621,6 +621,8 @@ <h1>Semidefinite programming relaxation of the unit circle</h1>
 
         <todo>port content from slides (e.g. https://slides.com/russtedrake/2023-columbia/edit starting at slide 28) and link to mi_convex notebook</todo>
 
+        <script>document.write(notebook_link('optimization', "mi_convex"))</script>
+
       </example>
 
       <p>Here are some back-references to applications of GCS throughout these notes:

book/trajopt/double_integrator.ipynb

@@ -18,12 +18,20 @@
    "outputs": [],
    "source": [
     "import numpy as np\n",
+    "import pydot\n",
+    "from IPython.display import SVG, display\n",
     "from matplotlib import pyplot as plt\n",
     "from pydrake.all import (\n",
+    "    Binding,\n",
     "    DirectCollocation,\n",
     "    DirectTranscription,\n",
+    "    GraphOfConvexSets,\n",
+    "    GraphOfConvexSetsOptions,\n",
+    "    HPolyhedron,\n",
+    "    LinearConstraint,\n",
     "    LinearSystem,\n",
     "    MathematicalProgram,\n",
+    "    Point,\n",
     "    Solve,\n",
     "    eq,\n",
     ")"
@@ -229,7 +237,13 @@
    "id": "aeb3fb81",
    "metadata": {},
    "source": [
-    "Note that there is a sample in the middle of the solution with u = 0.  Do you understand why?  (Hint: trying changing `N`)"
+    "Note that there is a sample in the middle of the solution with u = 0.  Do you understand why?  (Hint: trying changing `N`)\n",
+    "\n",
+    "# Minimum-time solutions via convex optimization with Graphs of Convex Sets\n",
+    "\n",
+    "Rather than doing a line search to find the minimum time (as we did in the first example above), we can use the powerful machinery of [Graphs of Convex Sets](http://underactuated.mit.edu/optimization.html#gcs) (GCS) to write an optimization problem with the horizon length as (effectly) a decision variable. We accomplish this by forming a serial chain of convex sets, with edge constraints enforcing the (linear) dynamics. But each vertex also has an additional edge that could allow it to transition directly to the target set, if the state is exactly equal to the target.\n",
+    "\n",
+    "NOTE: If `options.convex_relaxation=False`, then this program requires a mixed-integer programming solver (like Gurobi).  If `optionst.convex_relaxation=True`, then the resulting optimization is a linear program, and can be solved using open-source solvers."
    ]
   },
   {
@@ -238,6 +252,68 @@
    "id": "b0a5364a",
    "metadata": {},
    "outputs": [],
+   "source": [
+    "def min_time_gcs():\n",
+    "    # Discrete-time approximation of the double integrator.\n",
+    "    dt = 0.05\n",
+    "    A = np.eye(2) + dt * np.mat(\"0 1; 0 0\")\n",
+    "    B = dt * np.mat(\"0; 1\")\n",
+    "\n",
+    "    bbox = HPolyhedron.MakeBox([-4, -4], [4, 4])\n",
+    "    gcs = GraphOfConvexSets()\n",
+    "    source = gcs.AddVertex(Point([-2, 0]), \"x0\")\n",
+    "    target = gcs.AddVertex(Point([0, 0]), \"xf\")\n",
+    "    prev = source\n",
+    "    # B @ -1 <= x_{next} - A @ x_{prev} <= B @ 1\n",
+    "    dynamics_constraint = LinearConstraint(\n",
+    "        np.concatenate([np.eye(2), -A], axis=1), -B, B\n",
+    "    )\n",
+    "\n",
+    "    for n in range(int(3.0 / dt)):\n",
+    "        next = gcs.AddVertex(bbox, f\"n={n}\")\n",
+    "        e = gcs.AddEdge(prev, next)\n",
+    "        e.AddConstraint(\n",
+    "            Binding[LinearConstraint](\n",
+    "                dynamics_constraint, np.concatenate([next.x(), prev.x()])\n",
+    "            )\n",
+    "        )\n",
+    "        e.AddCost(dt)\n",
+    "        e = gcs.AddEdge(next, target)\n",
+    "        e.AddConstraint(next.x()[0] == target.x()[0])\n",
+    "        e.AddConstraint(next.x()[1] == target.x()[1])\n",
+    "        prev = next\n",
+    "\n",
+    "    options = GraphOfConvexSetsOptions()\n",
+    "    options.convex_relaxation = True\n",
+    "    options.max_rounded_paths = 100\n",
+    "    result = gcs.SolveShortestPath(source, target, options)\n",
+    "    assert result.is_success(), \"Optimization failed\"\n",
+    "\n",
+    "    print(f\"minimum time = {result.get_optimal_cost()}\")\n",
+    "\n",
+    "    # display(SVG(pydot.graph_from_dot_data(gcs.GetGraphvizString(result))[0].create_svg()))\n",
+    "\n",
+    "    # True solution: min time(q=-2, qdot=0) = 2√2 = 2.828 seconds.\n",
+    "    path = gcs.GetSolutionPath(source, target, result)\n",
+    "    print(f\"path length = {(len(path)-1)}\")\n",
+    "    x_sol = np.array([result.GetSolution(e.xu()) for e in path])\n",
+    "    np.append(x_sol, [0, 0])\n",
+    "\n",
+    "    fig, ax = plt.subplots()\n",
+    "    ax.plot(x_sol[:, 0], x_sol[:, 1], \"-\")\n",
+    "    ax.set_xlabel(\"q\")\n",
+    "    ax.set_ylabel(\"qdot\")\n",
+    "\n",
+    "\n",
+    "min_time_gcs()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "a1e39e54",
+   "metadata": {},
+   "outputs": [],
    "source": []
   }
  ],
@@ -257,7 +333,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.11.2"
+   "version": "3.10.12"
   }
  },
  "nbformat": 4,