mpc_python_learn/notebooks/2.3-MPC-simplified.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simplified MPC\n",
    "\n",
    "Checking the implementation from [Reuben Ferrante](https://github.com/arex18/rocket-lander) \n",
    "\n",
    "Here the system dynamics matrices are evaluated only once given the current state, input -> So no more need to keep track of **x_bar** and **u_bar**.\n",
    "\n",
    "This should give less precise results but the computation time should be gratly reduced and the overall code is slimmer, worth checking out!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cvxpy as opt\n",
    "import time\n",
    "from scipy.integrate import odeint\n",
    "from scipy.interpolate import interp1d\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "N = 4  # number of state variables\n",
    "M = 2  # number of control variables\n",
    "T = 10  # Prediction Horizon\n",
    "DT = 0.2  # discretization step\n",
    "\n",
    "L = 0.3  # vehicle wheelbase\n",
    "MAX_SPEED = 1.5  # m/s\n",
    "MAX_ACC = 1.0  # m/ss\n",
    "MAX_D_ACC = 1.0  # m/sss\n",
    "MAX_STEER = np.radians(30)  # rad\n",
    "MAX_D_STEER = np.radians(30)  # rad/s\n",
    "\n",
    "\n",
    "def get_linear_model_matrices(x_bar, u_bar):\n",
    "    \"\"\"\n",
    "    Computes the LTI approximated state space model x' = Ax + Bu + C\n",
    "    \"\"\"\n",
    "\n",
    "    x = x_bar[0]\n",
    "    y = x_bar[1]\n",
    "    v = x_bar[2]\n",
    "    theta = x_bar[3]\n",
    "\n",
    "    a = u_bar[0]\n",
    "    delta = u_bar[1]\n",
    "\n",
    "    ct = np.cos(theta)\n",
    "    st = np.sin(theta)\n",
    "    cd = np.cos(delta)\n",
    "    td = np.tan(delta)\n",
    "\n",
    "    A = np.zeros((N, N))\n",
    "    A[0, 2] = ct\n",
    "    A[0, 3] = -v * st\n",
    "    A[1, 2] = st\n",
    "    A[1, 3] = v * ct\n",
    "    A[3, 2] = v * td / L\n",
    "    A_lin = np.eye(N) + DT * A\n",
    "\n",
    "    B = np.zeros((N, M))\n",
    "    B[2, 0] = 1\n",
    "    B[3, 1] = v / (L * cd**2)\n",
    "    B_lin = DT * B\n",
    "\n",
    "    f_xu = np.array([v * ct, v * st, a, v * td / L]).reshape(N, 1)\n",
    "    C_lin = DT * (\n",
    "        f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
    "    )  # .flatten()\n",
    "\n",
    "    # return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)\n",
    "    return A_lin, B_lin, C_lin\n",
    "\n",
    "\n",
    "class MPC:\n",
    "    def __init__(self, N, M, Q, R):\n",
    "        \"\"\" \"\"\"\n",
    "        self.state_len = N\n",
    "        self.action_len = M\n",
    "        self.state_cost = Q\n",
    "        self.action_cost = R\n",
    "\n",
    "    def optimize_linearized_model(\n",
    "        self,\n",
    "        A,\n",
    "        B,\n",
    "        C,\n",
    "        initial_state,\n",
    "        target,\n",
    "        time_horizon=10,\n",
    "        Q=None,\n",
    "        R=None,\n",
    "        verbose=False,\n",
    "    ):\n",
    "        \"\"\"\n",
    "        Optimisation problem defined for the linearised model,\n",
    "        :param A:\n",
    "        :param B:\n",
    "        :param C:\n",
    "        :param initial_state:\n",
    "        :param Q:\n",
    "        :param R:\n",
    "        :param target:\n",
    "        :param time_horizon:\n",
    "        :param verbose:\n",
    "        :return:\n",
    "        \"\"\"\n",
    "\n",
    "        assert len(initial_state) == self.state_len\n",
    "\n",
    "        if Q == None or R == None:\n",
    "            Q = self.state_cost\n",
    "            R = self.action_cost\n",
    "\n",
    "        # Create variables\n",
    "        x = opt.Variable((self.state_len, time_horizon + 1), name=\"states\")\n",
    "        u = opt.Variable((self.action_len, time_horizon), name=\"actions\")\n",
    "\n",
    "        # Loop through the entire time_horizon and append costs\n",
    "        cost_function = []\n",
    "\n",
    "        for t in range(time_horizon):\n",
    "\n",
    "            _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) + opt.quad_form(\n",
    "                u[:, t], R\n",
    "            )\n",
    "\n",
    "            _constraints = [\n",
    "                x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n",
    "                u[0, t] >= -MAX_ACC,\n",
    "                u[0, t] <= MAX_ACC,\n",
    "                u[1, t] >= -MAX_STEER,\n",
    "                u[1, t] <= MAX_STEER,\n",
    "            ]\n",
    "            # opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]\n",
    "\n",
    "            # Actuation rate of change\n",
    "            if t < (time_horizon - 1):\n",
    "                _cost += opt.quad_form(u[:, t + 1] - u[:, t], R * 1)\n",
    "                _constraints += [opt.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC]\n",
    "                _constraints += [opt.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER]\n",
    "\n",
    "            if t == 0:\n",
    "                # _constraints += [opt.norm(target[:, time_horizon] - x[:, time_horizon], 1) <= 0.01,\n",
    "                #                x[:, 0] == initial_state]\n",
    "                _constraints += [x[:, 0] == initial_state]\n",
    "\n",
    "            cost_function.append(\n",
    "                opt.Problem(opt.Minimize(_cost), constraints=_constraints)\n",
    "            )\n",
    "\n",
    "        # Add final cost\n",
    "        problem = sum(cost_function)\n",
    "\n",
    "        # Minimize Problem\n",
    "        problem.solve(verbose=verbose, solver=opt.OSQP)\n",
    "        return x, u"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "the ODE is used to update the simulation given the mpc results\n",
    "I use this insted of using the LTI twice\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "def kinematics_model(x, t, u):\n",
    "    \"\"\"\n",
    "    Returns the set of ODE of the vehicle model.\n",
    "    \"\"\"\n",
    "\n",
    "    L = 0.3  # vehicle wheelbase\n",
    "    dxdt = x[2] * np.cos(x[3])\n",
    "    dydt = x[2] * np.sin(x[3])\n",
    "    dvdt = u[0]\n",
    "    dthetadt = x[2] * np.tan(u[1]) / L\n",
    "\n",
    "    dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
    "\n",
    "    return dqdt\n",
    "\n",
    "\n",
    "def predict(x0, u):\n",
    "    \"\"\" \"\"\"\n",
    "\n",
    "    x_ = np.zeros((N, T + 1))\n",
    "\n",
    "    x_[:, 0] = x0\n",
    "\n",
    "    # solve ODE\n",
    "    for t in range(1, T + 1):\n",
    "\n",
    "        tspan = [0, DT]\n",
    "        x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
    "\n",
    "        x0 = x_next[1]\n",
    "        x_[:, t] = x_next[1]\n",
    "\n",
    "    return x_\n",
    "\n",
    "\n",
    "\"\"\"\n",
    "MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
    "    \"\"\"\n",
    "    Computes a reference path given a set of waypoints\n",
    "    \"\"\"\n",
    "\n",
    "    final_xp = []\n",
    "    final_yp = []\n",
    "    delta = step  # [m]\n",
    "\n",
    "    for idx in range(len(start_xp) - 1):\n",
    "        section_len = np.sum(\n",
    "            np.sqrt(\n",
    "                np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
    "                + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
    "            )\n",
    "        )\n",
    "\n",
    "        interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
    "\n",
    "        fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
    "        fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
    "\n",
    "        # watch out to duplicate points!\n",
    "        final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
    "        final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
    "\n",
    "    dx = np.append(0, np.diff(final_xp))\n",
    "    dy = np.append(0, np.diff(final_yp))\n",
    "    theta = np.arctan2(dy, dx)\n",
    "\n",
    "    return np.vstack((final_xp, final_yp, theta))\n",
    "\n",
    "\n",
    "def get_nn_idx(state, path):\n",
    "    \"\"\"\n",
    "    Computes the index of the waypoint closest to vehicle\n",
    "    \"\"\"\n",
    "\n",
    "    dx = state[0] - path[0, :]\n",
    "    dy = state[1] - path[1, :]\n",
    "    dist = np.hypot(dx, dy)\n",
    "    nn_idx = np.argmin(dist)\n",
    "\n",
    "    try:\n",
    "        v = [\n",
    "            path[0, nn_idx + 1] - path[0, nn_idx],\n",
    "            path[1, nn_idx + 1] - path[1, nn_idx],\n",
    "        ]\n",
    "        v /= np.linalg.norm(v)\n",
    "\n",
    "        d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
    "\n",
    "        if np.dot(d, v) > 0:\n",
    "            target_idx = nn_idx\n",
    "        else:\n",
    "            target_idx = nn_idx + 1\n",
    "\n",
    "    except IndexError as e:\n",
    "        target_idx = nn_idx\n",
    "\n",
    "    return target_idx\n",
    "\n",
    "\n",
    "def normalize_angle(angle):\n",
    "    \"\"\"\n",
    "    Normalize an angle to [-pi, pi]\n",
    "    \"\"\"\n",
    "    while angle > np.pi:\n",
    "        angle -= 2.0 * np.pi\n",
    "\n",
    "    while angle < -np.pi:\n",
    "        angle += 2.0 * np.pi\n",
    "\n",
    "    return angle\n",
    "\n",
    "\n",
    "def get_ref_trajectory(state, path, target_v):\n",
    "    \"\"\"\n",
    "    For each step in the time horizon\n",
    "    modified reference in robot frame\n",
    "    \"\"\"\n",
    "    xref = np.zeros((N, T + 1))\n",
    "    dref = np.zeros((1, T + 1))\n",
    "\n",
    "    # sp = np.ones((1,T +1))*target_v #speed profile\n",
    "\n",
    "    ncourse = path.shape[1]\n",
    "\n",
    "    ind = get_nn_idx(state, path)\n",
    "    dx = path[0, ind] - state[0]\n",
    "    dy = path[1, ind] - state[1]\n",
    "\n",
    "    xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])  # X\n",
    "    xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])  # Y\n",
    "    xref[2, 0] = target_v  # V\n",
    "    xref[3, 0] = normalize_angle(path[2, ind] - state[3])  # Theta\n",
    "    dref[0, 0] = 0.0  # steer operational point should be 0\n",
    "\n",
    "    dl = 0.05  # Waypoints spacing [m]\n",
    "    travel = 0.0\n",
    "\n",
    "    for i in range(1, T + 1):\n",
    "        travel += abs(target_v) * DT  # current V or target V?\n",
    "        dind = int(round(travel / dl))\n",
    "\n",
    "        if (ind + dind) < ncourse:\n",
    "            dx = path[0, ind + dind] - state[0]\n",
    "            dy = path[1, ind + dind] - state[1]\n",
    "\n",
    "            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
    "            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
    "            xref[2, i] = target_v  # sp[ind + dind]\n",
    "            xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])\n",
    "            dref[0, i] = 0.0\n",
    "        else:\n",
    "            dx = path[0, ncourse - 1] - state[0]\n",
    "            dy = path[1, ncourse - 1] - state[1]\n",
    "\n",
    "            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
    "            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
    "            xref[2, i] = 0.0  # stop? #sp[ncourse - 1]\n",
    "            xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])\n",
    "            dref[0, i] = 0.0\n",
    "\n",
    "    return xref, dref"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.0908s Max: 0.1129s Min: 0.0846s\n"
     ]
    }
   ],
   "source": [
    "track = compute_path_from_wp(\n",
    "    [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
    ")\n",
    "\n",
    "# track = compute_path_from_wp([0,10,10,0],\n",
    "#                              [0,0,1,1],0.05)\n",
    "\n",
    "sim_duration = 200  # time steps\n",
    "opt_time = []\n",
    "x_sim = np.zeros((N, sim_duration))\n",
    "u_sim = np.zeros((M, sim_duration - 1))\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0  # x\n",
    "x0[1] = -0.25  # y\n",
    "x0[2] = 0.0  # v\n",
    "x0[3] = np.radians(-0)  # yaw\n",
    "x_sim[:, 0] = x0  # simulation_starting conditions\n",
    "\n",
    "# starting guess\n",
    "action = np.zeros(M)\n",
    "action[0] = MAX_ACC / 2  # a\n",
    "action[1] = 0.0  # delta\n",
    "u_sim[:, 0] = action\n",
    "\n",
    "# Cost Matrices\n",
    "Q = np.diag([20, 20, 10, 20])  # state error cost\n",
    "Qf = np.diag([30, 30, 30, 30])  # state final error cost\n",
    "R = np.diag([10, 10])  # input cost\n",
    "R_ = np.diag([10, 10])  # input rate of change cost\n",
    "\n",
    "mpc = MPC(N, M, Q, R)\n",
    "REF_VEL = 1.0\n",
    "\n",
    "for sim_time in range(sim_duration - 1):\n",
    "\n",
    "    iter_start = time.time()\n",
    "\n",
    "    # dynamics starting state w.r.t. robot are always null except vel\n",
    "    start_state = np.array([0, 0, x_sim[2, sim_time], 0])\n",
    "\n",
    "    # OPTIONAL: Add time delay to starting State- y\n",
    "\n",
    "    current_action = np.array([u_sim[0, sim_time], u_sim[1, sim_time]])\n",
    "\n",
    "    # State Matrices\n",
    "    A, B, C = get_linear_model_matrices(start_state, current_action)\n",
    "\n",
    "    # Get Reference_traj -> inputs are in worldframe\n",
    "    target, _ = get_ref_trajectory(x_sim[:, sim_time], track, REF_VEL)\n",
    "\n",
    "    x_mpc, u_mpc = mpc.optimize_linearized_model(\n",
    "        A, B, C, start_state, target, time_horizon=T, verbose=False\n",
    "    )\n",
    "\n",
    "    # retrieved optimized U and assign to u_bar to linearize in next step\n",
    "    u_bar = np.vstack(\n",
    "        (np.array(u_mpc.value[0, :]).flatten(), (np.array(u_mpc.value[1, :]).flatten()))\n",
    "    )\n",
    "\n",
    "    u_sim[:, sim_time] = u_bar[:, 0]\n",
    "\n",
    "    # Measure elpased time to get results from cvxpy\n",
    "    opt_time.append(time.time() - iter_start)\n",
    "\n",
    "    # move simulation to t+1\n",
    "    tspan = [0, DT]\n",
    "    x_sim[:, sim_time + 1] = odeint(\n",
    "        kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
    "    )[1]\n",
    "\n",
    "print(\n",
    "    \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
    "        np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
    "    )\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RESUTS\n",
    "\n",
    "SCS -> Optimization Time: Avrg: 0.2139s Max: 0.3517s Min: 0.1913s\n",
    "\n",
    "OSQP -> Optimization Time: Avrg: 0.1035s Max: 0.1311s Min: 0.0959s\n",
    "\n",
    "ECOS -> Avrg: 0.2024s Max: 0.2313s Min: 0.1904s\n",
    "\n",
    "**Qualitative result** aka \"it drives?\" seems the same..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 1080x720 with 5 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "grid = plt.GridSpec(4, 5)\n",
    "\n",
    "plt.figure(figsize=(15, 10))\n",
    "\n",
    "plt.subplot(grid[0:4, 0:4])\n",
    "plt.plot(track[0, :], track[1, :], \"b+\")\n",
    "plt.plot(x_sim[0, :], x_sim[1, :])\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(grid[0, 4])\n",
    "plt.plot(u_sim[0, :])\n",
    "plt.ylabel(\"a(t) [m/ss]\")\n",
    "\n",
    "plt.subplot(grid[1, 4])\n",
    "plt.plot(x_sim[2, :])\n",
    "plt.ylabel(\"v(t) [m/s]\")\n",
    "\n",
    "plt.subplot(grid[2, 4])\n",
    "plt.plot(np.degrees(u_sim[1, :]))\n",
    "plt.ylabel(\"delta(t) [rad]\")\n",
    "\n",
    "plt.subplot(grid[3, 4])\n",
    "plt.plot(x_sim[3, :])\n",
    "plt.ylabel(\"theta(t) [rad]\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python [conda env:.conda-jupyter] *",
   "language": "python",
   "name": "conda-env-.conda-jupyter-py"
  },
  "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}