{ "cells": [ { "cell_type": "markdown", "id": "9d41c333", "metadata": {}, "source": [ "# RHC Example: Double integrator with bounded input\n", "\n", "Richard M. Murray, 3 Feb 2022\n", "\n", "To illustrate the implementation of a receding horizon controller, we\n", "consider a linear system corresponding to a double integrator with\n", "bounded input:\n", "\n", "$$\n", " \\dot x = \\begin{bmatrix} 0 & 1 \\\\ 0 & 0 \\end{bmatrix} x + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} \\text{clip}(u)\n", " \\qquad\\text{where}\\qquad\n", " \\text{clip}(u) = \\begin{cases}\n", " -1 & u < -1, \\\\\n", " u & -1 \\leq u \\leq 1, \\\\\n", " 1 & u > 1.\n", " \\end{cases}\n", "$$\n", "\n", "We implement a model predictive controller by choosing\n", "\n", "$$\n", " Q_x = \\begin{bmatrix} 1 & 0 \\\\ 0 & 0 \\end{bmatrix}, \\qquad\n", " Q_u = \\begin{bmatrix} 1 \\end{bmatrix}, \\qquad\n", " P_1 = \\begin{bmatrix} 0.1 & 0 \\\\ 0 & 0.1 \\end{bmatrix}.\n", "$$" ] }, { "cell_type": "code", "execution_count": null, "id": "4fe0af7f", "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import scipy as sp\n", "import matplotlib.pyplot as plt\n", "import control as ct\n", "import control.optimal as opt\n", "import control.flatsys as fs\n", "import time" ] }, { "cell_type": "markdown", "id": "4c695f81", "metadata": {}, "source": [ "## System definition" ] }, { "cell_type": "code", "execution_count": null, "id": "5c01f571", "metadata": {}, "outputs": [], "source": [ "def doubleint_update(t, x, u, params):\n", " # Get the parameters\n", " lb = params.get('lb', -1)\n", " ub = params.get('ub', 1)\n", " assert lb < ub\n", "\n", " # bound the input\n", " u_clip = np.clip(u, lb, ub)\n", "\n", " return np.array([x[1], u_clip[0]])\n", "\n", "proc = ct.NonlinearIOSystem(\n", " doubleint_update, None, name=\"double integrator\",\n", " inputs = ['u'], outputs=['x[0]', 'x[1]'], states=2)" ] }, { "cell_type": "markdown", "id": "6c2f0d00", "metadata": {}, "source": [ "## Receding horizon controller\n", "\n", "Initial and final conditions:" ] }, { "cell_type": "code", "execution_count": null, "id": "62f3c9ce", "metadata": {}, "outputs": [], "source": [ "X0 = np.array([2, 1])" ] }, { "cell_type": "markdown", "id": "c3eec2a8", "metadata": {}, "source": [ "Cost functions to use:" ] }, { "cell_type": "code", "execution_count": null, "id": "a501efef", "metadata": {}, "outputs": [], "source": [ "Qx = np.diag([1, 0]) # state cost\n", "Qu = np.diag([1]) # input cost\n", "traj_cost=opt.quadratic_cost(proc, Qx, Qu)\n", "\n", "P1 = np.diag([0.1, 0.1]) # terminal cost\n", "term_cost = opt.quadratic_cost(proc, P1, None)" ] }, { "cell_type": "markdown", "id": "c5470629", "metadata": {}, "source": [ "Constraints:" ] }, { "cell_type": "code", "execution_count": null, "id": "cb4c511a", "metadata": {}, "outputs": [], "source": [ "traj_constraints = opt.input_range_constraint(proc, -1, 1)" ] }, { "cell_type": "markdown", "id": "a5568374", "metadata": {}, "source": [ "Horizon for evaluating finite-time, optimal control" ] }, { "cell_type": "code", "execution_count": null, "id": "9edec673", "metadata": {}, "outputs": [], "source": [ "T = 5\n", "timepts = np.linspace(0, T, 6, endpoint=True)\n", "print(timepts)" ] }, { "cell_type": "markdown", "id": "cb8fcecc", "metadata": {}, "source": [ "Define the optimal control problem that we want to solve (without actually solving it)" ] }, { "cell_type": "code", "execution_count": null, "id": "e9f31be6", "metadata": {}, "outputs": [], "source": [ "# Set up the optimal control problem\n", "ocp = opt.OptimalControlProblem(\n", " proc, timepts, traj_cost,\n", " terminal_cost=term_cost,\n", " trajectory_constraints=traj_constraints,\n", " # terminal_constraints=term_constraints,\n", ")" ] }, { "cell_type": "code", "execution_count": null, "id": "887295eb", "metadata": {}, "outputs": [], "source": [ "# Solve test problem to make sure optimal control problem works\n", "start_time = time.process_time()\n", "res = ocp.compute_trajectory(X0, initial_guess=0, return_states=True)\n", "stop_time = time.process_time()\n", "print(f'* Cold start: {stop_time-start_time:.3} sec, '\n", " f'u = {np.round(res.inputs, decimals=2)}')\n", "\n", "# Resolve using previous solution as initial guess to compare timing\n", "start_time = time.process_time()\n", "u = res.inputs\n", "u_shift = np.hstack([u[:, 1:], u[:, -1:]])\n", "res = ocp.compute_trajectory(\n", " X0, initial_guess=u_shift, return_states=True, print_summary=False)\n", "stop_time = time.process_time()\n", "print(f'* Warm start: {stop_time-start_time:.3} sec, '\n", " f'u_shift = {np.round(u_shift, decimals=2)}')\n", "\n", "plt.plot(res.time, res.states[0], 'k-', label='$x_1$')\n", "plt.plot(res.time, res.inputs[0], 'b-', label='u')\n", "plt.xlabel('Time [s]')\n", "plt.ylabel('$x_1$, $u$')\n", "plt.legend()\n" ] }, { "cell_type": "code", "execution_count": null, "id": "bdf70063", "metadata": {}, "outputs": [], "source": [ "?np.round" ] }, { "cell_type": "code", "execution_count": null, "id": "eb2e8126", "metadata": {}, "outputs": [], "source": [ "# Create a figure to use for plotting\n", "def run_rhc_and_plot(proc, ocp, X0, Tf, print_summary=False, ax=None): \n", " # Start at the initial point\n", " x = X0\n", " \n", " # Initialize the axes\n", " if ax is None:\n", " ax = plt.axes()\n", " \n", " # Generate the individual traces for the receding horizon control\n", " for t in np.arange(0, Tf - T + 1):\n", " # Compute the optimal trajectory over the horizon\n", " res = ocp.compute_trajectory(\n", " x, return_states=True,\n", " print_summary=print_summary\n", " )\n", "\n", " # Simulate the system for the update time, with higher res for plotting\n", " tvec = np.linspace(0, res.time[1], 20)\n", " inputs = res.inputs[:, 0] + np.outer(\n", " res.inputs[:, 1] - res.inputs[:, 0], tvec)\n", " soln = ct.input_output_response(proc, tvec, inputs, x)\n", "\n", " # Plot the results over the full horizon\n", " h3, = ax.plot(res.time + t, res.states[0], 'k--')\n", " ax.plot(res.time + t, res.inputs[0], 'b--')\n", "\n", " # Plot the results for this time segment\n", " h1, = ax.plot(soln.time + t, soln.states[0], 'k-')\n", " h2, = ax.plot(soln.time + t, soln.inputs[0], 'b-')\n", "\n", " # Update the state to use for the next time point\n", " x = soln.states[:, -1]\n", " \n", " # Adjust the limits for consistency\n", " ax.set_ylim([-4, 3.5])\n", "\n", " # Add reference line for input lower bound\n", " ax.plot([0, 7], [-1, -1], 'k--', linewidth=0.666)\n", "\n", " # Label the results\n", " ax.set_xlabel(\"Time $t$ [sec]\")\n", " ax.set_ylabel(\"State $x_1$, input $u$\")\n", " ax.legend(\n", " [h1, h2, h3], ['$x_1$', '$u$', 'prediction'],\n", " loc='lower right', labelspacing=0)\n", " plt.tight_layout()\n", " \n", " return x\n", "\n", "xf = run_rhc_and_plot(proc, ocp, X0, 10, print_summary=True)\n", "print(f\"{xf=}\")" ] }, { "cell_type": "markdown", "id": "6005bfb3", "metadata": {}, "source": [ "## RHC with LQR terminal cost\n", "\n", "To see how things change, try changing the terminal cost to the LQR-based terminal cost." ] }, { "cell_type": "code", "execution_count": null, "id": "ea2de1f3", "metadata": {}, "outputs": [], "source": [ "linsys = proc.linearize(0, 0)\n", "\n", "# Get the LQR solution\n", "K, P_lqr, E = ct.lqr(proc.linearize(0, 0), Qx, Qu)\n", "print(f\"P_lqr = \\n{P_lqr}\")\n", "\n", "ocp_lqr = opt.OptimalControlProblem(\n", " proc, timepts, traj_cost,\n", " terminal_cost=opt.quadratic_cost(proc, P_lqr, None),\n", " trajectory_constraints=traj_constraints,\n", " # terminal_constraints=term_constraints,\n", ")\n", "xf = run_rhc_and_plot(proc, ocp_lqr, X0, 10)\n", "print(f\"\\n{xf=}\")" ] }, { "cell_type": "code", "execution_count": null, "id": "80dd3f5e", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "language_info": { "name": "python" } }, "nbformat": 4, "nbformat_minor": 5 }