From 2c8bf8665febe63df8b5d0a552afd64b6f09ba06 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Wed, 27 Nov 2019 13:15:13 +0000 Subject: [PATCH] Initial Commit --- MPC_cvxpy.ipynb | 956 ++++++++++++++++++++++++++++++++++++++++++++++++ MPC_scipy.ipynb | 731 ++++++++++++++++++++++++++++++++++++ 2 files changed, 1687 insertions(+) create mode 100644 MPC_cvxpy.ipynb create mode 100644 MPC_scipy.ipynb diff --git a/MPC_cvxpy.ipynb b/MPC_cvxpy.ipynb new file mode 100644 index 0000000..e929ca3 --- /dev/null +++ b/MPC_cvxpy.ipynb @@ -0,0 +1,956 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "from scipy.integrate import odeint\n", + "from scipy.interpolate import interp1d\n", + "import cvxpy as cp\n", + "\n", + "import matplotlib.pyplot as plt\n", + "plt.style.use(\"ggplot\")\n", + "\n", + "import time" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### kinematics model equations\n", + "\n", + "The variables of the model are:\n", + "\n", + "* $x$ coordinate of the robot\n", + "* $y$ coordinate of the robot\n", + "* $\\theta$ heading of the robot\n", + "* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n", + "* $cte$ lateral error = distance of the robot from the path \n", + "\n", + "The inputs of the model are:\n", + "\n", + "* $v$ linear velocity of the robot\n", + "* $w$ angular velocity of the robot\n", + "\n", + "These are the differential equations f(x,u) of the model:\n", + "\n", + "* $\\dot{x} = v\\cos{\\theta}$ \n", + "* $\\dot{y} = v\\sin{\\theta}$\n", + "* $\\dot{\\theta} = w$\n", + "* $\\dot{\\psi} = -w$\n", + "* $\\dot{cte} = v\\sin{\\psi}$\n", + "\n", + "The **Continuous** State Space Model is\n", + "\n", + "${\\dot{x}} = Ax + Bu $\n", + "\n", + "with:\n", + "\n", + "$\n", + "A = \\quad\n", + "\\begin{bmatrix}\n", + "\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} & \\frac{\\partial f(x,u)}{\\partial \\psi} & \\frac{\\partial f(x,u)}{\\partial cte} \\\\\n", + "\\end{bmatrix}\n", + "\\quad\n", + "=\n", + "\\quad\n", + "\\begin{bmatrix}\n", + "0 & 0 & -vsin(\\theta) & 0 & 0 \\\\\n", + "0 & 0 & vcos(\\theta) & 0 & 0 \\\\\n", + "0 & 0 & 0 & 0 & 0 \\\\\n", + "0 & 0 & 0 & 0 & 0 \\\\\n", + "0 & 0 & 0 & 0 & vcos(\\psi) \n", + "\\end{bmatrix}\n", + "\\quad\n", + "$\n", + "\n", + "$\n", + "B = \\quad\n", + "\\begin{bmatrix}\n", + "\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial w} \\\\\n", + "\\end{bmatrix}\n", + "\\quad\n", + "=\n", + "\\quad\n", + "\\begin{bmatrix}\n", + "\\cos{\\theta_t} & 0 \\\\\n", + "\\sin{\\theta_t} & 0 \\\\\n", + "0 & 1 \\\\\n", + "0 & -1 \\\\\n", + "\\sin{(-\\psi_t)} & 0 \n", + "\\end{bmatrix}\n", + "\\quad\n", + "$\n", + "\n", + "discretize with forward Euler Integration\n", + "\n", + "* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}*dt$\n", + "* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}*dt$\n", + "* ${\\theta_{t+1}} = \\theta_{t} + w_t*dt$\n", + "* ${\\psi_{t+1}} = \\psi_{t} - w_t*dt$\n", + "* ${cte_{t+1}} = cte_{t} + v_t\\sin{-\\psi}*dt$\n", + "\n", + "The **Discrete** State Space Model is\n", + "\n", + "${x_{t+1}} = Ax_t + Bu_t $\n", + "\n", + "with:\n", + "\n", + "$\n", + "A = \\quad\n", + "\\begin{bmatrix}\n", + "1 & 0 & -v\\sin{\\theta}dt & 0 & 0 \\\\\n", + "0 & 1 & v\\cos{\\theta}dt & 0 & 0 \\\\\n", + "0 & 0 & 1 & 0 & 0 \\\\\n", + "0 & 0 & 0 & 1 & 0 \\\\\n", + "0 & 0 & 0 & 0 & 1 +vcos{-\\psi}dt\n", + "\\end{bmatrix}\n", + "\\quad\n", + "$\n", + "\n", + "$\n", + "B = \\quad\n", + "\\begin{bmatrix}\n", + "\\cos{\\theta_t}dt & 0 \\\\\n", + "\\sin{\\theta_t}dt & 0 \\\\\n", + "0 & dt \\\\\n", + "0 & -dt \\\\\n", + "\\sin{-\\psi_t}dt & 0 \n", + "\\end{bmatrix}\n", + "\\quad\n", + "$\n", + "\n", + "This State Space Model is not linear (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n", + "\n", + "$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n", + "\n", + "So:\n", + "\n", + "$ x_{k+1} = x_k + (f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u}) )dt $\n", + "\n", + "$ x_{k+1} = (I+dtA)x_k + dtBu_k +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "NB: psi and cte are expressed w.r.t. the reference point frame.\n", + "in the reference frame of the veicle the equtions would be:\n", + "* psi_dot = w\n", + "* cte_dot = sin(psi)" + ] + }, + { + "cell_type": "code", + "execution_count": 48, + "metadata": {}, + "outputs": [], + "source": [ + "# Control problem statement.\n", + "\n", + "N = 5 #number of state variables\n", + "M = 2 #number of control variables\n", + "T = 20 #Prediction Horizon\n", + "dt = 0.25 #discretization step\n", + "\n", + "x = cp.Variable((N, T+1))\n", + "u = cp.Variable((M, T))\n", + "\n", + "def get_model(x,u):\n", + " \n", + " x_t = x[0]\n", + " y_t = x[1]\n", + " theta_t = x[2]\n", + " psi_t = x[3]\n", + " cte_t = x[4]\n", + " \n", + " v_t = u[0]\n", + " u_t = u[1]\n", + " \n", + " A = np.eye(N)\n", + " A[0,2]=-v_t*np.sin(theta_t)*dt\n", + " A[1,2]=v_t*np.cos(theta_t)*dt\n", + " A[4,4]=1+v_t*np.cos(psi_t)*dt\n", + " \n", + " B = np.zeros((N,M))\n", + " B[0,0]=np.cos(theta_t)*dt\n", + " B[1,0]=np.sin(theta_t)*dt\n", + " B[2,1]=dt\n", + " B[3,1]=-dt\n", + " B[4,0]=np.sin(-psi_t)*dt\n", + " \n", + " return A,B" + ] + }, + { + "cell_type": "code", + "execution_count": 71, + "metadata": {}, + "outputs": [], + "source": [ + "def get_linear_model(x_bar,u_bar):\n", + " \n", + " x = x_bar[0]\n", + " y = x_bar[1]\n", + " theta = x_bar[2]\n", + " psi = x_bar[3]\n", + " cte = x_bar[4]\n", + " \n", + " v = u_bar[0]\n", + " w = u_bar[1]\n", + " \n", + " A = np.zeros((N,N))\n", + " A[0,2]=-v*np.sin(theta)\n", + " A[1,2]=v*np.cos(theta)\n", + " A[4,4]=v*np.cos(-psi)\n", + " A_lin=np.eye(N)+dt*A\n", + " \n", + " B = np.zeros((N,M))\n", + " B[0,0]=np.cos(theta)\n", + " B[1,0]=np.sin(theta)\n", + " B[2,1]=1\n", + " B[3,1]=-1\n", + " B[4,0]=np.sin(-psi)\n", + " B_lin=dt*B\n", + " \n", + " f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)\n", + " C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n", + " \n", + " return A_lin,B_lin,C_lin" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "(array([[1., 0., 0., 0., 0.],\n", + " [0., 1., 0., 0., 0.],\n", + " [0., 0., 1., 0., 0.],\n", + " [0., 0., 0., 1., 0.],\n", + " [0., 0., 0., 0., 1.]]), array([[ 0.25, 0. ],\n", + " [ 0. , 0. ],\n", + " [ 0. , 0.25],\n", + " [ 0. , -0.25],\n", + " [-0. , 0. ]]))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "get_model(np.zeros(N))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Motion Prediction: using scipy intergration" + ] + }, + { + "cell_type": "code", + "execution_count": 50, + "metadata": {}, + "outputs": [], + "source": [ + "# Define process model\n", + "def kinematics_model(x,t,u):\n", + "\n", + " # calculate derivative\n", + " dxdt = u[0]*np.cos(x[2])\n", + " dydt = u[0]*np.sin(x[2])\n", + " dthetadt = u[1]\n", + " dpsidt = -u[1]\n", + " dctedt = u[0]*np.sin(-x[3])\n", + "\n", + " dqdt = [dxdt,\n", + " dydt,\n", + " dthetadt,\n", + " dpsidt,\n", + " dctedt]\n", + "\n", + " return dqdt\n", + "\n", + "def predict(x0,u):\n", + " \"\"\"\n", + " x0(5)\n", + " u(2,T)\n", + " \n", + " x_bar(5,T+1)\n", + " \"\"\"\n", + " \n", + " x_bar = np.zeros((N,T+1))\n", + " \n", + " x_bar[:,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,\n", + " x0,\n", + " tspan,\n", + " args=(u[:,t-1],))\n", + "\n", + " x0 = x_next[1]\n", + " x_bar[:,t]=x_next[1]\n", + " \n", + " return x_bar" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Validate the model, here the status w.r.t a straight line with constant heading 0" + ] + }, + { + "cell_type": "code", + "execution_count": 51, + "metadata": {}, + "outputs": [], + "source": [ + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:] = 1 #m/s\n", + "u_bar[1,:] = np.radians(-10) #rad/s\n", + "\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = 1\n", + "x0[2] = np.radians(0)\n", + "x0[3] = 0\n", + "x0[4] = 1\n", + "\n", + "x_bar=predict(x0,u_bar)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Check the model prediction" + ] + }, + { + "cell_type": "code", + "execution_count": 57, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(x_bar[0,:],x_bar[1,:])\n", + "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", + "plt.axis('equal')\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(np.degrees(x_bar[2,:]))\n", + "plt.ylabel('theta(t) [deg]')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 3)\n", + "plt.plot(np.degrees(x_bar[3,:]))\n", + "plt.ylabel('psi(t) [deg]')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 4)\n", + "plt.plot(x_bar[4,:])\n", + "plt.ylabel('cte(t)')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "the results seems valid:\n", + "* the cte is correct\n", + "* theta error is correct" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Motion Prediction: using the state space model" + ] + }, + { + "cell_type": "code", + "execution_count": 72, + "metadata": {}, + "outputs": [], + "source": [ + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:] = 1 #m/s\n", + "u_bar[1,:] = np.radians(-10) #rad/s\n", + "\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = 1\n", + "x0[2] = np.radians(0)\n", + "x0[3] = 0\n", + "x0[4] = 1\n", + "\n", + "x_bar=np.zeros((N,T+1))\n", + "x_bar[:,0]=x0\n", + "\n", + "for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(5,1)\n", + " ut=u_bar[:,t-1].reshape(2,1)\n", + " \n", + " A,B,C=get_linear_model(xt,ut)\n", + " \n", + " xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n", + " \n", + " x_bar[:,t]= np.squeeze(xt_plus_one)" + ] + }, + { + "cell_type": "code", + "execution_count": 73, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(x_bar[0,:],x_bar[1,:])\n", + "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", + "plt.axis('equal')\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(x_bar[2,:])\n", + "plt.ylabel('theta(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 3)\n", + "plt.plot(x_bar[3,:])\n", + "plt.ylabel('psi(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 4)\n", + "plt.plot(x_bar[4,:])\n", + "plt.ylabel('cte(t)')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The results are the same as expected" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "this way of predictiting psi and cte does not work for non-constant heading trajectories, appropriate functions have to be developed." + ] + }, + { + "cell_type": "code", + "execution_count": 36, + "metadata": {}, + "outputs": [], + "source": [ + "def calc_err(state,path):\n", + " \"\"\"\n", + " Finds psi and cte w.r.t. the closest waypoint.\n", + "\n", + " :param state: array_like, state of the vehicle [x_pos, y_pos, theta]\n", + " :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]\n", + " :returns: (float,float)\n", + " \"\"\"\n", + "\n", + " dx = state[0]-path[0,:]\n", + " dy = state[1]-path[1,:]\n", + " dist = np.sqrt(dx**2 + dy**2)\n", + " nn_idx = np.argmin(dist)\n", + "\n", + " try:\n", + " v = [path[0,nn_idx+1] - path[0,nn_idx],\n", + " path[1,nn_idx+1] - path[1,nn_idx]] \n", + " v /= np.linalg.norm(v)\n", + "\n", + " d = [path[0,nn_idx] - state[0],\n", + " 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", + "# front_axle_vect = [np.cos(state[2] - np.pi / 2),\n", + "# np.sin(state[2] - np.pi / 2)]\n", + " path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),\n", + " np.sin(path[2,target_idx] + np.pi / 2)]\n", + " \n", + " #heading error w.r.t path frame\n", + " psi = path[2,target_idx] - state[2]\n", + " \n", + " # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor\n", + " #cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)\n", + " cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect)\n", + "\n", + " return target_idx,psi,cte\n", + "\n", + "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", + " '''\n", + " Interpolation range is computed to assure one point every fixed distance step [m].\n", + " \n", + " :param start_xp: array_like, list of starting x coordinates\n", + " :param start_yp: array_like, list of starting y coordinates\n", + " :param step: float, interpolation distance [m] between consecutive waypoints\n", + " :returns: array_like, of shape (3,N)\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(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n", + "\n", + " interp_range = np.linspace(0,1,section_len/delta)\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", + " final_xp=np.append(final_xp,fx(interp_range))\n", + " final_yp=np.append(final_yp,fy(interp_range))\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))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "test it" + ] + }, + { + "cell_type": "code", + "execution_count": 75, + "metadata": {}, + "outputs": [], + "source": [ + "track = compute_path_from_wp([0,5],[0,0])\n", + "\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:] = 1 #m/s\n", + "u_bar[1,:] = np.radians(-10) #rad/s\n", + "\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = 1\n", + "x0[2] = np.radians(0)\n", + "_,psi,cte = calc_err(x0,track)\n", + "x0[3]=psi\n", + "x0[4]=cte\n", + "\n", + "x_bar=np.zeros((N,T+1))\n", + "x_bar[:,0]=x0\n", + "\n", + "for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(5,1)\n", + " ut=u_bar[:,t-1].reshape(2,1)\n", + " \n", + " A,B,C=get_linear_model(xt,ut)\n", + " \n", + " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", + " \n", + " _,psi,cte = calc_err(xt_plus_one,track)\n", + " xt_plus_one[3]=psi\n", + " xt_plus_one[4]=cte\n", + " \n", + " x_bar[:,t]= xt_plus_one" + ] + }, + { + "cell_type": "code", + "execution_count": 76, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(x_bar[0,:],x_bar[1,:])\n", + "plt.plot(track[0,:],track[1,:],\"b-\")\n", + "plt.axis('equal')\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(x_bar[2,:])\n", + "plt.ylabel('theta(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 3)\n", + "plt.plot(x_bar[3,:])\n", + "plt.ylabel('psi(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 4)\n", + "plt.plot(x_bar[4,:])\n", + "plt.ylabel('cte(t)')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": 79, + "metadata": {}, + "outputs": [], + "source": [ + "track = compute_path_from_wp([0,2,4,5,10],[0,0,3,1,1])\n", + "\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:] = 1 #m/s\n", + "u_bar[1,:] = np.radians(10) #rad/s\n", + "\n", + "x0 = np.zeros(N)\n", + "x0[0] = 2\n", + "x0[1] = 2\n", + "x0[2] = np.radians(0)\n", + "_,psi,cte = calc_err(x0,track)\n", + "x0[3]=psi\n", + "x0[4]=cte\n", + "\n", + "x_bar=np.zeros((N,T+1))\n", + "x_bar[:,0]=x0\n", + "\n", + "for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(5,1)\n", + " ut=u_bar[:,t-1].reshape(2,1)\n", + " \n", + " A,B,C=get_linear_model(xt,ut)\n", + " \n", + " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", + " \n", + " _,psi,cte = calc_err(xt_plus_one,track)\n", + " xt_plus_one[3]=psi\n", + " xt_plus_one[4]=cte\n", + " \n", + " x_bar[:,t]= xt_plus_one" + ] + }, + { + "cell_type": "code", + "execution_count": 81, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(x_bar[0,:],x_bar[1,:])\n", + "plt.plot(track[0,:],track[1,:],\"b-\")\n", + "plt.axis('equal')\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(x_bar[2,:])\n", + "plt.ylabel('theta(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 3)\n", + "plt.plot(x_bar[3,:])\n", + "plt.ylabel('psi(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "plt.subplot(2, 2, 4)\n", + "plt.plot(x_bar[4,:])\n", + "plt.ylabel('cte(t)')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "MPC Problem formulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": 109, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "optimal\n", + "time elapsed: 0.2521398067474365\n" + ] + } + ], + "source": [ + "MAX_SPEED = 1.2\n", + "MIN_SPEED = 0.8\n", + "MAX_STEER_SPEED = 0.57\n", + "\n", + "#starting guess\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:]=1\n", + "u_bar[1,:]=0.01\n", + "\n", + "track = compute_path_from_wp([0,10],[2,2])\n", + "\n", + "# Starting Condition\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = 1\n", + "x0[2] = np.radians(-30)\n", + "_,psi,cte = calc_err(x0,track)\n", + "x0[3]=psi\n", + "x0[4]=cte\n", + "\n", + "start = time.time()\n", + "\n", + "# Prediction\n", + "x_bar=np.zeros((N,T+1))\n", + "x_bar[:,0]=x0\n", + "\n", + "for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(5,1)\n", + " ut=u_bar[:,t-1].reshape(2,1)\n", + " \n", + " A,B,C=get_linear_model(xt,ut)\n", + " \n", + " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", + " \n", + " _,psi,cte = calc_err(xt_plus_one,track)\n", + " xt_plus_one[3]=psi\n", + " xt_plus_one[4]=cte\n", + " \n", + " x_bar[:,t]= xt_plus_one\n", + "\n", + "\n", + "#CVXPY MPC problem statement\n", + "cost = 0\n", + "constr = []\n", + "\n", + "for t in range(T):\n", + " \n", + " # Cost function\n", + " #cost += 10*cp.sum_squares( x[3, t]) # psi\n", + " #cost += 50*cp.sum_squares( x[4, t]) # cte\n", + " \n", + " # Tracking\n", + " idx,_,_ = calc_err(x_bar[:,t],track)\n", + " delta_x =track[:,idx]-x[0:3,t]\n", + " cost+= cp.quad_form(delta_x,10*np.eye(3))\n", + " #cost += cp.quad_form( x[:, t],10*np.eye(N))\n", + " \n", + " # Actuation effort\n", + " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", + " \n", + " # Actuation rate of change\n", + " if t < (T - 1):\n", + " cost += cp.quad_form(u[:, t + 1] - u[:, t], 1*np.eye(M))\n", + " \n", + " #constrains\n", + " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", + " constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()]\n", + " \n", + "# sums problem objectives and concatenates constraints.\n", + "constr += [x[:,0] == x0]\n", + "constr += [u[0, :] <= MAX_SPEED]\n", + "constr += [u[0, :] >= MIN_SPEED]\n", + "constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", + "\n", + "\n", + "prob = cp.Problem(cp.Minimize(cost), constr)\n", + "solution = prob.solve(solver=cp.ECOS, verbose=False)\n", + "\n", + "end = time.time()\n", + "\n", + "print(prob.status)\n", + "print(\"time elapsed: {}\".format(end-start))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Print Results:" + ] + }, + { + "cell_type": "code", + "execution_count": 110, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "x_mpc=np.array(x.value[0, :]).flatten()\n", + "y_mpc=np.array(x.value[1, :]).flatten()\n", + "theta_mpc=np.array(x.value[1, :]).flatten()\n", + "psi_mpc=np.array(x.value[1, :]).flatten()\n", + "cte_mpc=np.array(x.value[1, :]).flatten()\n", + "v_mpc=np.array(u.value[0, :]).flatten()\n", + "w_mpc=np.array(u.value[1, :]).flatten()\n", + "\n", + "#simulate q for optimized u_hat\n", + "x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n", + "\n", + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(track[0,:],track[1,:],\"b*\")\n", + "plt.plot(x_traj[0,:],x_traj[1,:])\n", + "plt.axis(\"equal\")\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "#plot v(t)\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(v_mpc)\n", + "plt.ylabel('v(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "#plot w(t)\n", + "plt.subplot(2, 2, 3)\n", + "plt.plot(w_mpc)\n", + "plt.ylabel('w(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "#plot theta(t)\n", + "#plt.subplot(2, 2, 4)\n", + "#plt.plot(cte_mpc)\n", + "#plt.ylabel('cte(t)')\n", + "#plt.xlabel('time')\n", + "#plt.legend(loc='best')\n", + "\n", + "plt.tight_layout()\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 +} diff --git a/MPC_scipy.ipynb b/MPC_scipy.ipynb new file mode 100644 index 0000000..a089768 --- /dev/null +++ b/MPC_scipy.ipynb @@ -0,0 +1,731 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "plt.style.use(\"ggplot\")\n", + "import time\n", + "from scipy.interpolate import interp1d\n", + "from scipy.integrate import odeint\n", + "from scipy.optimize import minimize" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "differential drive kinematics model equations" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# Define process model\n", + "def kinematics_model(q,t,u):\n", + " # arguments\n", + " # y = outputs\n", + " # t = time\n", + " # u = input value\n", + " # K = process gain\n", + " # tau = process time constant\n", + " x = q[0]\n", + " y = q[1]\n", + " theta = q[2]\n", + "\n", + " v = u[0]\n", + " w = u[1]\n", + "\n", + " # calculate derivative\n", + " dxdt = v*np.cos(theta)\n", + " dydt = v*np.sin(theta)\n", + " dthetadt = w\n", + "\n", + " dqdt = [dxdt,dydt,dthetadt]\n", + "\n", + " return dqdt" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "simulate" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# time points\n", + "t = np.arange(0, 40, 0.1)\n", + "\n", + "#fake inputs\n", + "# u = [v(t),\n", + "# w(t)]\n", + "u = np.zeros((2,len(t)))\n", + "u[0,100:]=0.4\n", + "u[1,200:]=0.15\n", + "u[1,300:]=-0.15" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# initial conditions\n", + "q0 = [0,0,0]\n", + "\n", + "# store solution\n", + "x = np.empty_like(t)\n", + "y = np.empty_like(t)\n", + "theta = np.empty_like(t)\n", + "\n", + "# record initial conditions\n", + "x[0] = q0[0]\n", + "y[0] = q0[1]\n", + "theta[0] = q0[2]\n", + "\n", + "# solve ODE\n", + "for i in range(1,len(t)):\n", + " # span for next time step\n", + " tspan = [t[i-1],t[i]]\n", + " # solve for next step\n", + " q_t = odeint(kinematics_model,q0,tspan,args=(u[:,i],))\n", + " # store solution for plotting\n", + " x[i] = q_t[1][0]\n", + " y[i] = q_t[1][1]\n", + " theta[i] = q_t[1][2]\n", + " # next initial condition\n", + " q0 = q_t[1]" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "\n", + "# plot results\n", + "# plt.plot(t,u,'g:',label='u(t)')\n", + "# plt.plot(t,x,'b-',label='x(t)')\n", + "# plt.plot(t,y,'r--',label='y(t)')\n", + "\n", + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(x,y)\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "#plot x(t)\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(t,x)\n", + "plt.ylabel('x(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "\n", + "#plot y(t)\n", + "plt.subplot(2, 2, 3)\n", + "plt.plot(t,y)\n", + "plt.ylabel('y(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "#plot theta(t)\n", + "plt.subplot(2, 2, 4)\n", + "plt.plot(t,theta)\n", + "plt.ylabel('theta(t)')\n", + "#plt.xlabel('time')\n", + "#plt.legend(loc='best')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "preliminaries:\n", + "* path - > waypoints\n", + "* error computation -> cte" + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "metadata": {}, + "outputs": [], + "source": [ + "def calc_target_index(state,path):\n", + " \"\"\"\n", + " Finds the index of the closest waypoint.\n", + "\n", + " :param state: array_like, state of the vehicle [x_pos, y_pos, theta]\n", + " :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...)]\n", + " :returns: (int,float), nearest_index and cross track error\n", + " \"\"\"\n", + "\n", + " if np.shape(state)[0] is not 3 or np.shape(path)[0] is not 2:\n", + " raise ValueError(\"Input has wrong shape!\")\n", + "\n", + " # Search nearest point index by finding the point closest to the vehicle\n", + " # dx = [state[0] - icx for icx in path[0,:]]\n", + " # dy = [state[1] - icy for icy in path[1,:]]\n", + " # dist = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]\n", + " dx = state[0]-path[0,:]\n", + " dy = state[1]-path[1,:]\n", + " dist = np.sqrt(dx**2 + dy**2)\n", + " #nn_idx = dist.index(min(dist))\n", + " nn_idx = np.argmin(dist)\n", + "\n", + " try:\n", + "\n", + " # versor v from nearest wp -> next wp\n", + " v = [path[0,nn_idx+1] - path[0,nn_idx],\n", + " path[1,nn_idx+1] - path[1,nn_idx]] \n", + " v /= np.linalg.norm(v)\n", + "\n", + " # vector d from car position -> nearest wp\n", + " d = [path[0,nn_idx] - state[0],\n", + " path[1,nn_idx] - state[1]]\n", + "\n", + " # Get the scalar projection of d on v to compare direction\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", + " front_axle_vect = [np.cos(state[2] - np.pi / 2),\n", + " np.sin(state[2] - np.pi / 2)]\n", + "\n", + " # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor\n", + " error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vect)\n", + "\n", + " return target_idx, error_front_axle" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [], + "source": [ + "def compute_path_from_wp(start_xp, start_yp, step = 0.25):\n", + " '''\n", + " Interpolation range is computed to assure one point every fixed distance step [m].\n", + " \n", + " :param start_xp: array_like, list of starting x coordinates\n", + " :param start_yp: array_like, list of starting y coordinates\n", + " :param step: float, interpolation distance [m] between consecutive waypoints\n", + " :returns: array_like, of shape (2,N)\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(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n", + "\n", + " interp_range = np.linspace(0,1,section_len/delta)\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", + " final_xp=np.append(final_xp,fx(interp_range))\n", + " final_yp=np.append(final_yp,fy(interp_range))\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))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "test path" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(2, 91)\n" + ] + }, + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "start_x=[0,5,7.5,8,10,15]\n", + "start_y=[0,0,5,10,10,12]\n", + "path = compute_path_from_wp(start_x,start_y)\n", + "\n", + "print(np.shape(path))\n", + "\n", + "plt.plot(path[0,:],path[1,:])\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "test cte" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "(1, -1.642756358806414)" + ] + }, + "execution_count": 26, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "calc_target_index([0, 2, 0.75],path)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "mpc" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": {}, + "outputs": [], + "source": [ + "# Define Objective function\n", + "def objective(u_hat,*args):\n", + " \"\"\"\n", + " Computes objective function\n", + " \n", + " :param u_hat: array_like, input [v,w\n", + " v,w\n", + " ...]\n", + " \"\"\"\n", + " \n", + " #undo input flattening\n", + " u_hat = u_hat.reshape(2, -1).T\n", + " se = np.zeros(PRED_HZN) #squared_errors\n", + " \n", + " # Prediction\n", + " for k in range(PRED_HZN):\n", + " \n", + " # Initialize state for prediction\n", + " if k==0:\n", + " q_hat0 = args[0]\n", + " \n", + " # Clamp control horizon\n", + " elif k>CTRL_HZN:\n", + " u_hat[k,:] = u_hat[CTRL_HZN,:]\n", + "\n", + " ts_hat = [delta_t_hat*(k),delta_t_hat*(k+1)]\n", + " \n", + " #DEBUG\n", + "# print(\"k : {}\".format(k))\n", + "# print(\"q_hat0 : {}\".format(q_hat0))\n", + "# print(\"ts : {}\".format(ts_hat))\n", + "# print(\"u_hat : {}\".format(u_hat[k,:]))\n", + " \n", + " q_hat = odeint(kinematics_model,q_hat0,ts_hat,args=(u_hat[k,:],))\n", + " \n", + "# print(q_hat)\n", + " \n", + " q_hat0 = q_hat[-1,:]\n", + "\n", + " # Squared Error calculation\n", + " _,cte = calc_target_index(q_hat[-1,:],path)\n", + "# print(cte)\n", + " if k >0:\n", + " delta_u_hat = np.sum(u_hat[k,:]-u_hat[k-1,:])\n", + " else:\n", + " delta_u_hat = 0\n", + " #delta_u_hat=np.sum(np.subtract([u_hat[2*k],u_hat[2*k+1]],[u_hat[2*(k-1)],u_hat[2*(k-1)+1]]))\n", + " se[k] = 20 * (cte)**2 + 15 * (delta_u_hat)**2\n", + "\n", + " # Sum of Squared Error calculation\n", + " obj = np.sum(se[1:])\n", + " return obj" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "TODO: add heading error" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "test optimization" + ] + }, + { + "cell_type": "code", + "execution_count": 32, + "metadata": {}, + "outputs": [], + "source": [ + "#PARAMS\n", + "v=1.5\n", + "vb=0.5\n", + "wb=0.5\n", + "\n", + "# Define horizons\n", + "PRED_HZN = 20 # Prediction Horizon\n", + "CTRL_HZN = 10 # Control Horizon\n", + "\n", + "delta_t_hat = 0.25 #time step" + ] + }, + { + "cell_type": "code", + "execution_count": 33, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "/home/marcello/.local/lib/python3.5/site-packages/ipykernel_launcher.py:28: RuntimeWarning: invalid value encountered in true_divide\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + " fun: 36.02837407165175\n", + " jac: array([-2.19157696e+00, -3.22256088e-02, 1.18650579e+00, -4.04749918e+00,\n", + " 1.49658680e-01, 6.93027973e-01, -8.36541653e-01, -2.46588659e+00,\n", + " 1.18037081e+00, 1.26046467e+00, -4.76961613e-01, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -2.14655476e+01, -2.45364900e+01, -2.71573195e+01, -3.44248009e+01,\n", + " -3.15746927e+01, -3.04034195e+01, -3.05026488e+01, -2.93093610e+01,\n", + " -2.17690802e+01, -1.74863310e+01, -8.52382469e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00])\n", + " message: 'Optimization terminated successfully.'\n", + " nfev: 1034\n", + " nit: 22\n", + " njev: 22\n", + " status: 0\n", + " success: True\n", + " x: array([ 1.36094105, 1.44124498, 1.49189215, 1.4944542 , 1.48896953,\n", + " 1.49211915, 1.43846883, 1.36823453, 1.34829146, 1.37191554,\n", + " 0.99609573, 0.99609573, 0.99609573, 0.99609573, 0.99609573,\n", + " 0.99609573, 0.99609573, 0.99609573, 0.99609573, 0.99609573,\n", + " -0.29866483, -0.28173159, -0.24925358, -0.24879537, -0.15900868,\n", + " -0.13969979, -0.13969797, -0.13969797, -0.13969797, -0.23856165,\n", + " 0.02904087, 0.02904087, 0.02904087, 0.02904087, 0.02904087,\n", + " 0.02904087, 0.02904087, 0.02904087, 0.02904087, 0.02904087])\n", + "time elapsed: 3.381047248840332\n" + ] + } + ], + "source": [ + "q=np.array([[0,0.5],[0,0.4],[0,0.2]])\n", + "\n", + "u_hat0 = np.zeros((PRED_HZN,2))\n", + "u_hat0[:,0]=v\n", + "u_hat0[:,1]=0.1\n", + "u_hat0=u_hat0.flatten(\"F\")\n", + "\n", + "bnds=((v-2*vb,v+vb),)\n", + "for i in range (PRED_HZN-1):\n", + " bnds=bnds+((v-2*vb,v+vb),)\n", + " \n", + "bnds=bnds+((-wb,wb),)\n", + "for i in range (PRED_HZN-1):\n", + " bnds=bnds+((-wb,wb),)\n", + "\n", + "# print(u_hat0)\n", + "# print(u_hat0.reshape(2, -1).T)\n", + "\n", + "start = time.time()\n", + "\n", + "solution = minimize(objective,u_hat0,args=(q[:,-1],),method='SLSQP',bounds=bnds,options={'maxiter':50})\n", + "\n", + "end = time.time()\n", + "\n", + "print(solution)\n", + "print(\"time elapsed: {}\".format(end-start))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "check what the optimizer returns" + ] + }, + { + "cell_type": "code", + "execution_count": 34, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "u=solution.x.reshape(2,-1).T\n", + "x=[]\n", + "y=[]\n", + "theta=[]\n", + "q0=[0.5,0.4,0.2]\n", + "\n", + "#simulate q for optimized u_hat\n", + "def step(q0,u,delta):\n", + " \"\"\"\n", + " steps the simulated vehicle\n", + " \"\"\"\n", + " \n", + " # span for next time step\n", + " tspan = [delta*0,delta*1]\n", + " # solve for next step\n", + " q_t = odeint(kinematics_model,q0,tspan,args=(u,))\n", + " \n", + " return q_t[1]\n", + "\n", + "for i in range(1,np.shape(u)[0]):\n", + " \n", + " # step simulation of t\n", + " q_t = step(q0,u[i,:],delta_t_hat)\n", + " \n", + " # store solution for plotting\n", + " x.append(q_t[0])\n", + " y.append(q_t[1])\n", + " theta.append(q_t[2])\n", + " \n", + " # next initial condition\n", + " q0 = q_t\n", + " \n", + "plt.plot(x,y)\n", + "plt.plot(path[0,:],path[1,:])\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "compute MPC on the whole path" + ] + }, + { + "cell_type": "code", + "execution_count": 185, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "/home/marcello/.local/lib/python3.5/site-packages/ipykernel_launcher.py:28: RuntimeWarning: invalid value encountered in true_divide\n" + ] + } + ], + "source": [ + "# Initialize state\n", + "# q=[x1, x2, ..., xn\n", + "# y1, y2, ..., yn\n", + "# theta1, theta2, ..., thetan]\n", + "\n", + "q=np.array([0,0.5,0.15]).reshape(3,-1)\n", + "\n", + "# Initialize input for prediction horizon\n", + "# u=[v1,v2,...,vp\n", + "# w1,w2,...,wp]\n", + "\n", + "# Initial guess \n", + "# u_hat0=[v1,v2,..vp,w1,w2,...,wp]\n", + "# uhat0 -> u_hat=optimize(obj,u_hat0)\n", + "\n", + "u_hat0 = np.zeros((PRED_HZN,2))\n", + "u_hat0[:,0]=v\n", + "u_hat0[:,1]=0.01\n", + "u_hat0=u_hat0.flatten(\"F\")\n", + "\n", + "# Optimization Bounds\n", + "# bnds=((v1_min,v1_max),...,(vp_min,vp_max),(w1_min,w1_max),...,(wp_min,wp_max))\n", + "\n", + "bnds=((v-2*vb,v+vb),)\n", + "for i in range (1,PRED_HZN):\n", + " bnds=bnds+((v-2*vb,v+vb),)\n", + " \n", + "bnds=bnds+((-wb,wb),)\n", + "for i in range (1,PRED_HZN):\n", + " bnds=bnds+((-wb,wb),)\n", + "\n", + "#while np.sum(np.abs(q[-1,0:2]-path[-1,0:2]))>0.1:\n", + "for i in range(110): \n", + " \n", + " start = time.time()\n", + " \n", + " #MPC LOOP\n", + " u_hat = minimize(objective,\n", + " u_hat0,\n", + " args=(q[:,-1],),\n", + " method='SLSQP',\n", + " bounds=bnds,\n", + " options={'maxiter':100}\n", + " ).x\n", + " \n", + " end = time.time()\n", + " #print(\"iter:1 time elapsed: {}\".format(end-start))\n", + " \n", + "# print(\"u_hat0 {}\".format(np.shape(u_hat0)))\n", + "# print(\"u_hat {}\".format(np.shape(u_hat)))\n", + "# print(\"bounds {}\".format(np.shape(bnds)))\n", + " \n", + " # \"move\" vehicle\n", + " q_next = odeint(kinematics_model,\n", + " q[:,-1], # q(t)\n", + " [0,delta_t_hat], # \n", + " args=([u_hat[0],u_hat[PRED_HZN]],) # v,w\n", + " )\n", + " \n", + " q = np.hstack((q,q_next[-1,:].reshape(3,-1)))\n", + " \n", + " # next initial condition\n", + " u_hat0=u_hat\n", + "\n", + " " + ] + }, + { + "cell_type": "code", + "execution_count": 186, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAABDAAAALICAYAAACJhQBYAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADl0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uIDMuMC4zLCBodHRwOi8vbWF0cGxvdGxpYi5vcmcvnQurowAAIABJREFUeJzs3XmQ3Odh5vfv291zD4AB0AOQAMULFA+QBAECJEFiKF66RUmUJbWsw5IsOVzbZa2zdmVtx5VyKlWb2q244t3sbtVGcWx5E8e1HTlryZJtiYckYsBTJMELIAmAuM9uAHN1z9E9/eaPASmSAkiAmJlfH99P1VRPHzP9EPWyu/HgPUKMEUmSJEmSpHqWSjqAJEmSJEnSu7HAkCRJkiRJdc8CQ5IkSZIk1T0LDEmSJEmSVPcsMCRJkiRJUt2zwJAkSZIkSXXPAkOSJEmSJNU9CwxJkiRJklT3LDAkSZIkSVLdyyQd4D2ISQeQJEmSJEmzKrzbAxqxwODQoUNJRzhn2WyWYrGYdAzVIceGTsdxoTNxbOhMHBs6HceFzsSxodNJalysWLHirB7nEhJJkiRJklT3LDAkSZIkSVLds8CQJEmSJEl1zwJDkiRJkiTVPQsMSZIkSZJU9ywwJEmSJElS3bPAkCRJkiRJdc8CQ5IkSZIk1T0LDEmSJEmSVPcsMCRJkiRJUt2zwJAkSZIkSXXPAkOSJEmSJNU9CwxJkiRJklT3LDAkSZIkSVLds8CQJEmSJEl1zwJDkiRJkiTVPQsMSZIkSZJU9ywwJEmSJElS3bPAkCRJkiRJdc8CQ5IkSZIk1T0LDEmSJEmSVPcsMCRJkiRJUt2zwJAkSZIkSXXPAkOSJEmSJNU9CwxJkiRJklT3LDAkSZIkSVLds8CQJEmSJKkJxBiTjjCnLDAkSZIkSWpQsVYjPv8U03/6x8QHv590nDmVSTqAJEmSJEk6N7EyRXz8p8QHvgeH90PfUujqTjrWnLLAkCRJkiSpQcTREeLP/oH48A9hdBjedxnhm79H2DBAyDT3X/Gb+79OkiRJkqQmEI8eIj74PeKjD8HUFFy/gdSHPg1XryGEkHS8eWGBIUmSJElSHYoxws7t1H78X+G5JyGdJmy8i/ChTxNWXJx0vHlngSFJkiRJUh2J09PEZx4jPvB3sPtV6FlA+PjnCXd9grBocdLxEmOBIUmSJElSHYgTZeLggzOniRw/BssuJHzpNwm33U3o6Ew6XuIsMCRJkiRJSlA8USA+/APiIz+G8RJccQ2pL/wG3HATIZVOOl7dsMCQJEmSJCkBcc8O4gPfI/58ECKE9bcRPvgpwqqrk45WlywwJEmSJEmaJ7E2Dc89Re2Bv4Md26Czi3DPJwl330vILk86Xl2zwJAkSZIkaY7FyQniow/N7G9x7DAsXUbIfZMw8CFCV3fS8RqCBYYkSZIkSXMknjxO/MkPiD/7EZTH4LIrCff/GuHGWwlp97c4FxYYkiRJkiTNsrhvF/GB7xOfegRqEdZtJPWhT8OqqwkhJB2vIVlgSJIkSZI0C2JtGp7/ObUHvw+vvAAdXYQ7Pz6zx0X/BUnHa3gWGJIkSZIknYc4MU7c8hDxoe9D4QgsyRI+93XC7R8mdPcmHa9pWGBIkiRJkvQexOPHiA//gLj5ARgvweVXET7zVfe3mCMWGJIkSZIknaUYI+x6mdqD34NnHocAYf2mmWUiq65OOl5Ts8CQJEmSJOldxGqV+PQW4kN/D7tfhe4ewofvI9z9CcKS/qTjtQQLDEmSJEmSziCWRomP/Jj4kx/CySIsW0H40j8j3Ho3obMr6XgtxQJDkiRJkqS3iYcPEB/+e+KjD8PUJFy9htSXfwuuX09IpZKO15IsMCRJkiRJAmKtBi89S+2h78NLz0ImQ7jlDsIHP0W46LKk47U8CwxJkiRJUkuLE+PERx8iPvxDOHoQFi0hfPrLhA98hLCwL+l4OsUCQ5IkSZLUkmLhCPHhHxK3PADjZbjsSsJv/D5h/W2ETFvS8fQ2FhiSJEmSpJYRY4RXXqD20N/Dc09CKkW48TaPQW0AFhiSJEmSpKYXpyaJT/yM+PAP4MAe6F1A+NjnCHd+nLB4adLxdBYsMCRJkiRJTSueKBJ/9o/ER34EYyOw8hLCV39nZnPO9o6k4+kcWGBIkiRJkppKjBF2vETt4R/As49DjHDDzaTu+SRcdT0hhKQj6j2wwJAkSZIkNYU4OUl88mczp4kc2A3dvYQPfppw58cI/RckHU/nyQJDkiRJktTQYuHIzDKRzQ9AeQwuunRmmcjNdxA6XCbSLCwwJEmSJEkNJ8YI25+bWSby/FMQAqzbSOrue+H917pMpAlZYEiSJEmSGkacKBMf+ynxJz+Ew/thwSLCxz5PuOOjhCXZpONpDllgSJIkSZLqXjxykPjTfyA++hCMl+GSKwi//t8SbhogtLUnHU/zwAJDkiRJklSX4vQ0PPcktZ/+A2x/DtIZwoZNhLvvhcuudJlIi7HAkCRJkiTVlTh8krj5x8RHfgQni7AkS7jvK4SBDxEWLU46nhJigSFJkiRJSlyMEXa8RPzpPxKfeRSmp2H1OlJfuh+uv4mQTicdUQmzwJAkSZIkJSZOlImP/5T403+Eg3uhu4dw170zm3JesDLpeKojFhiSJEmSpHkXD+6b2ZTzsZ/A5DhcvIrwtW8RbvoAoaMj6XiqQxYYkiRJkqR5EasV4rOPz8y2ePVFyLTNnCJy58fdlFPvygJDkiRJkjSn4rFDxEd+RHz0YRgdhqXLCJ/9GmHThwgLFiYdTw3CAkOSJEmSNOtitQJbn6D2yI9mjkBNpeCGm0l94COweh0hlUo6ohqMBYYkSZIkadbEwpGZ2RZbHpyZbbGkn/DpLxMGPkjoW5p0PDUwCwxJkiRJ0nmJ1So89yS1R/4Jtm2FkIIbbiL1gY/CtWsJKY9A1fmzwJAkSZIkvSexcIS4+cczsy1GhmBJlvCpLxEGPkRY7GwLzS4LDEmSJEnSWYuVCjz3BLXBB2ZmWxBgzYaZvS2uu9HZFpozFhiSJEmSpHcVD+wmDj5IfOKnMDY6M9vi3i/MzLZY0p90PLUACwxJkiRJ0mnF8hjxyUeIgw/C3p2QyRDWbiQMfAiuWeNsC80rCwxJkiRJ0htirQavvEDc8iDxmcegMgUXXUr41f+GcMsdhN6FSUdUi7LAkCRJkiQRTxSIjz5E3PIQFI9CVw9h0wdnZltcfDkhhKQjqsXNS4GRy+X+ArgXOJbP5687ddv/AnwSmAJ2Ab+ez+eH5iOPJEmSJAliZYq49UnillMbcsYI19xAuO8rhHUbCe0dSUeU3jBfMzC+A/wH4D+/6bYHgD/K5/PVXC73b4A/Av5gnvJIkiRJUkuKtRqTLz1L7UffI/58C4yXZjbk/MQXCLfdTei/IOmI0mnNS4GRz+cfyeVyl77tth+/6erjwOfmI4skSZIktaIDr+1n8OevMjjawRd3/gMbR3cSbryVsPEuuPp6N+RU3auXPTC+AfyXM92Zy+XuB+4HyOfzZLPZ+co1azKZTEPm1txzbOh0HBc6E8eGzsSxodNxXOjAwWP8+CdP8/DhKXa3LyXEC1mdOc7S+77AsntuI3R2JR1RdaTeXzMSLzByudwfA1Xgr8/0mHw+/23g26euxmKxOB/RZlU2m6URc2vuOTZ0Oo4LnYljQ2fi2NDpOC5aU2G4zJYntzN4aJIdmSXAAq6sHeEbPUfYtHE12QtW/2JsjJWSjqs6ktRrxooVK87qcYkWGLlc7uvMbO55Tz6fj0lmkSRJkqRGdaI0xaPP7GRw7zDbw2Kgh8unRvlqx142bXg/F1xxZ9IRpfOWWIGRy+U+CvxL4I58Pl9OKockSZIkNaLh8QqPPb+HwV0neKm2gFpIcXF5ii+1v8rAmotZsfZ297VQU5mvY1T/BrgTyOZyuQPAnzBz6kgH8EAulwN4PJ/P/+Z85JEkSZKkRjQ2Nc3jL+xlcEeB56ozpcWK8hSfYzsDVy3j4ptvdl8LNa35OoXki6e5+f+cj+eWJEmSpEZWrkzz5PaDDL58hGeneqiGNMvHq9w3vY2BVUu4bOMGUr0Lk44pzbnEN/GUJEmSJL3VZLXGUzuOsPmlgzwz3sVUKsPSiRofq2zn9ksX8v5b1pNavCnpmNK8ssCQJEmSpDowNV3j6dcKDL64n5+PdTCRaqNvCj5Y3sbA+3q5+sM3ku7fmHRMKTEWGJIkSZKUkMp0ZOueIoMv7OfJ0QzlVDsLKoEPjG5nYEUn1961lsyKm5KOKdUFCwxJkiRJmkfTtcjz+04w+PxeHh/OMJZqp6cCG0dfYWB5mjW3rSFz6ecJISQdVaorFhiSJEmSNMema5FtB08yuHUPjw6lGEl10llNc/PoDgb6U6zbuJq2yz9naSG9AwsMSZIkSZoDMUZePjzM5md38+gJOJnqon06w4aRXQwsjay/bTUdqz5DSKWSjio1BAsMSZIkSZolMUZ2Hh1l8NldbClGCqlu2moZ1o28xkDfNDfdfDVdV37a0kJ6DywwJEmSJOk8xBjZUywx+MxOBo9NcyTVQ7rWzg2ju/nioiluufH99FxzLyGVTjqq1NAsMCRJkiTpPdh3oszg0zsZPDLFwVQvqdjO9aO7+eyCSTauXcWC1R8jpC0tpNligSFJkiRJZ+nQ0DiDz+xky6EJ9oQFhJhh9dgh7u3ex61rLqHv+o8QMv41S5oL/p8lSZIkSe/g2OgEW57ZxeCBEjtZCLRx1dhhvtGxj4HrL2bJDfcQ2tqSjik1PQsMSZIkSXqb42NTPLp1F5v3jvIKC4E0q8ZG+VrbfjZdu5Jl6+4gtHckHVNqKRYYkiRJkgScHK/w2NbXGNw9xLbaQmIIXFIq86X0fgauvoAVGzYROjqTjim1LAsMSZIkSS1rZKLK4y/sYXDncV6YXkAtpFhZnuTzYRsDVy7j4ptuJnR1Jx1TEhYYkiRJklpMuTLN4y/uZ/DVozxX6aUa0iwfr/KZ2nYGrljKpTevJ9W7IOmYkt7GAkOSJElS05uo1nhy+0EGtx/mmcluKqkM2YnIJ6rbGbisjytuWU9q0aakY0p6BxYYkiRJkprSZLXG0zuOMPjSQX4+3slkqo3Fk/DhyZcZuGQBV21cT3rJxqRjSjpLFhiSJEmSmkZlOvLsawUGX9zHE2MdTKTaWDgVuHP8VQYu6mL1h9aRWXZz0jElvQcWGJIkSZIa2nQt8tze42x+fi9PjLRRSrXTU0mxqfQqAxd2sObOG8is3JB0TEnnyQJDkiRJUsOZrkVeOjjE5q17eHwoxUiqg65qmptHdzCwPMPaTdfRdsmvEEJIOqqkWWKBIUmSJKkh1GLk5SOjDG7dzaPFyMlUJx3TaTaM7GRgaWD9wGraL7e0kJqVBYYkSZKkuhVjZMexEpu37mZLocrx0EVbLcX64Z0MLJ5mw01X03nVfYRUKumokuaYBYYkSZKkuhJj5LUT42zZupvBw5McDd1kaoG1w/v4yoJJbln3frqv/RQhnU46qqR5ZIEhSZIkqS7sG5pgcOseBg+WOUg3qQhrhg/xua4St669jN7rP05oa0s6pqSEWGBIkiRJSsyhkUkGn9/L5n1j7IvdhFjj2uEj3NsxzK3XXkzfjR8idHQkHVNSHbDAkCRJkjSvjo5NMfjCfgb3DPFarQeAq4eP8c3MCW67ZiVLN9xJ6OpOOKWkemOBIUmSJGnOHS9XGHzpIIO7jvPq9ExpccXICb4WXmbgqgvo//RthN6FCaeUVM8sMCRJkiTNiaGJKlu2H2ZwxzG2T3UTQ+DSsWG+PL2dgSuyXHjvLYRFi5OOKalBWGBIkiRJmjWjk9M89uoRNr98lBcnO6mFFBeVxvjC1HYGLl/MRR+7mbDk7qRjSmpAFhiSJEmSzktpaprHdxQYfPkQz413Mh1SXFgu8ysT2xi4ZCGXfHA9qeV3JB1TUoOzwJAkSZJ0zsYrNZ58rcjgSwd4ptxBNaTpn5jgk6VtDFzUw6o7byS1ciDpmJKaiAWGJEmSpLMyWa3x8z0nGHxxPz8fa2MqZFgyWeGjo9sZWNnJVbevI7zvVkIISUeV1IQsMCRJkiSdUWW6xjP7hhh8YR9PjqSZCG0smqpy98h2Bi5oZ/Wta0hd9iVLC0lzzgJDkiRJ0ltUa5HnDgwx+Pw+Hh9KUQ5t9FamGRh+mduXpblu4DrSq36VkEolHVVSC7HAkCRJksR0LfLioREGn9vDYycDo6Gd7mqNW4ZeZlMW1t5yLZmrPk9IpZOOKqlFWWBIkiRJLaoWI9sOj7LluT08ejwyFDronIabhl5lU980N264mvarP0dIW1pISp4FhiRJktRCYoy8emyMwa272VKocTx00j4duHF4JwOLKty07ko6rrmPkPGvCpLqi69KkiRJUpOLMfJasczgs68xeKzKsdBFppZi3fBrfHXBFDffsIqu6z5JyLQlHVWSzsgCQ5IkSWpSu46N8IOfvMjmw5McDj2kaynWjBzgCz3j3LLmMnqv/wShzdJCUmOwwJAkSZKayIGT4ww+u4vBQxPsD72kYoprR45yX+cYt665lIU3fITQ1p50TEk6ZxYYkiRJUoM7MjLBlmd2MXiwzGssIMQU14wW+a2e/dxy9YX0rb2H0N6RdExJOi8WGJIkSVIDKoxN8OjTu9h8oMQOFgJp3j86xDfa97Jp9UqWrruL/pUrKRaLSUeVpFlhgSFJkiQ1iBOlKR59ZieD+0bZziIgzeVjo/xa2z4GrlnB8htvJ3R0Jh1TkuaEBYYkSZJUx4bHKzz27A4Gd4/wUlxILaS4uDTOF9MHGbj6Alauv43Q2ZV0TEmacxYYkiRJUp0ZnazyxNZdDO46wXO1RdRCihXlCp9NvcLAlcu45KabCZ3dSceUpHllgSFJkiTVgfLUNE8+/xqDO4o8W11ANZVh2UTkvvgKA+/PctlNN5Lq7kk6piQlxgJDkiRJSshktcZTL+xh8JUjPF1ZyFQqw9LJwMemdzKwajFX3nwjqd7bko4pSXXBAkOSJEmaR5XpGk9v28fgtsM8NdXDRKqdvqk27qnsZOCyRVxzy42kF25MOqYk1R0LDEmSJGmOVWuRrS8fYPNLB3hyvJtyuoMFlXY+MLWbgYt7ufbWdWQW3ZR0TEmqaxYYkiRJ0hyYrkVe2HWEzc/v5fFSJ2PpTnoqHWyc3MvARV2s2biOtqXrk44pSQ3DAkOSJEmaJbUY2ba3yOatu3lspI3hdBed1S5uGd/LphUdrLt1Le39NyYdU5IakgWGJEmSdB5ijLxyaIjBZ3ax5WSKE+lu2qc72VDaw8DyNtbfch2dK29IOqYkNTwLDEmSJOkcxRjZdWyUzc/sYksxUkh101brYN3YHgaycNNtq+m69JOEEJKOKklNwwJDkiRJOgsxRvYcL7Pl2V0MHqlwONVDutbODaO7+WJflVtuuoqeq+61tJCkOWKBIUmSJL2DA0MTDD67i80HJzgQekjFDNeN7uczvZPcum4VC1Z/jJBOJx1TkpqeBYYkSZL0NodHJxl8bg+De0fZQy8hBlaPHOMTXWPcuuZS+tZ8mNDWlnRMSWopFhiSJEkSUChVGHxxP4OvnWRnrQeAq0aO843MTjZdexFL199F6OhMOKUktS4LDEmSJLWsE+NVtmw/xOCOAi9XZ0qLVaMn+SqvsOmq5Sy/bxOhpzfhlJIksMCQJElSixmeqPLoq8cYfOUoL012EkPgkrERvlTZxsAVWVZ84hZC35KkY0qS3sYCQ5IkSU1vbHKax147zuD2QzxfbqcWUqwsjfD5iRcYuHQRF3/4FkL/XUnHlCS9AwsMSZIkNaVyZZon9g4x+NJBto5mqIYUy8dLfGZ0KwPv6+HSD2wg9b47ko4pSTpLFhiSJElqGhPVGk8dGGHwxYM8PQSVkCY7UeITQy8zsKKTK+5cR1h1GyGEpKNKks6RBYYkSZIa2tR0jacPjjG47RBPHZ9mkjSLJ0t8+OQ2BpYGrtq4htTVXyWk00lHlSSdBwsMSZIkNZzKdOS5IyU2v3yEJ45MMk6ahVNj3Hn8JTYtrHDt+tWk13yJ0NaedFRJ0iyxwJAkSVJDmK5FXjha5pFXj/HEwTJjMU1PpcxtxZfY1DnKmrVXkfnC5wjdPUlHlSTNAQsMSZIk1a3pWmRboczgjuM8un+UkVqaruoENxdfYlOqyNrrL6f9M/cSFi1OOqokaY5ZYEiSJKmu1GLkleI4g7tOsmXPECen03RMT7Hh+DY2VQ6y/pr30fGxuwnLLkw6qiRpHllgSJIkKXExRnaemGBw9xCDr52kWEnRVqtw4/GXGSjvZsP7L6Drc7fDRZd6gogktSgLDEmSJCUixsieoUk27xlmy64THJkMZGrTrD3xCl8eeZWbL19CzycGYNVnCalU0nElSQmzwJAkSdK82j88yeY9I2zZdZwD45CKNdac3MFnT2xj40U9LLh7E1zzKULGj6qSpF/wXUGSJElz7vDoFJv3jrBl53H2lCIhRq4deo1PHH+Bjf1p+jbeSljze4SOjqSjSpLqlAWGJEmS5kShVGFw7wibdxTZNRYBuGp4D9889hy3LqqydMNNhBt/h9DTm3BSSVIjsMCQJEnSrDlerrBl7yiDOwq8MjpTWlwxso+vFV7gtgWTLFu3lvDl+z32VJJ0ziwwJEmSdF6GJqo8um+EwVeOsW0kEglcOnaILxeeZ9OCSS5cdwNh7TcJC/uSjipJamAWGJIkSTpno5PTPLZ/hMGXj/LCcKRG4KLSUb5QnCktLlq7hrD2G4QFC5OOKklqEhYYkiSpZcUYYXoa0mlCCEnHqXvlyjSP7yww+Moxto5lmA4pLiwX+ZXiC2xaMMkla68jtfZrhF5LC0nS7LPAkCRJTSdWpqBwBI4dIh49DMWjUBollsagPAbl0qnLMajVZn4onYFMZuYynYZM26nL12/LvPX6m24P3T2QXQ7Z5YRTlyzsa4pSZHxiiqe27mRwzzDPVBdQCRmyEyU+eWIbmxZOseq6K0mt/TU34pQkzTkLDEmS1NDi6AjseJH46kvEQ/vg2GE4UYAYf/Gg7l7oXQg9vdDTS+i/YOb77l5oa5+ZhTFdmbmsVme+pl//miZW33TfdBWqFZgYP/V9lVgahZGhmTyvP2d7Oyx9vdRYdurygl8UHd098/5ndbYmjhzm6WdfZfBIhZ+nlzOVbmPJZIoPT73CwPIMV99yFeGyrxPS6aSjSpJaiAWGJElqKHHkJPGVl+DVF4mvvgiH9s3c0d4OKy4hrLoGbrsblq0gLF8Jyy6cl9kBcXISjh+F4lFi8W2XO7fBeJn45h/o7v2lWRtvzN7ILiO0tc95ZmCmfNm/m6n9e9h6cIzBUhdP9l7ORGY5C1Ml7g5HuP3iRVxz42rSPbfMSyZJkk7HAkOSJNW9WBojPj1IfOwnsHP7zI0dXXDF1YRb7iBceR1cegUh05ZYxtDRASsuhhUX8/aFIzHGmeUqby82ikfh0F7i809BtfLWgmPRkpkiI7sc+i94a8GxeCkhdXazH2KMMDUJY6NQGoFjh4n79xAP7KZ6YC/P08eW/ht4ov86ym1d9C6cZKBngtuvXsR1V11JJp2arT8iSZLOiwWGJEmqS7FahZeeJT72MPG5J2eWbVz4PsJ9XyGsXgsXr2qYJQwhBOhZMPN1yRW/XHDUajBy8i3lxsz3x4g7tsGTmyHWflFwpNMzvyvTduorw/HOLqZDmLkOUBqdKS3GRmb+7E6ZJrBtyRUMXnIbj1//aUZDO92pyC0rurj9iiw3XNhDJtX4e3dIkpqPBYYkSaorsTRKfOB7xEd+BKPD0LuQcMdHCbfeNVNaNMHGmG8XUinoWwp9SwlXrP6l+2O1OrOvx5sLjtIoVCozMzeqFVKpAOXyzG0AS5cRLlkFvQupdS/k5bYsWyqLeHSsk6EKdGYCN63s5fZLFrJuRQ/tzrSQJNW5eSkwcrncXwD3Asfy+fx1p25bAvwX4FJgD5DL5/Mn5yOPJEmqP28UFw/9/cwGmWs3khr4IFx7IyHT2v/mEjIZWHbhzH4eZ3jM4myWYrH4xvUYIzuOT7B57whb9o1y/GSV9nRg/Ypebr9kARtW9tKRsbSQJDWO+fo08B3gPwD/+U23/SHwUD6f/9e5XO4PT13/g3nKI0mS6sTbi4uwfhPhk79KWHlJ0tEaToyR3Scn3ygtjo5VyKRg3YW9fHXtAm6+qJfutsZYdiNJ0tvNS4GRz+cfyeVyl77t5k8Dd576/q+An2KBIUlSXZio1hieqM7pc8RabWZ/iwe+B5MThOsGCPfcS7hg5cwDxqbm9PmbSWmqxn/dsZcfbz/KodEpUgFuuKCHL1y3lFvet4DedksLSVLjS3I+5vJ8Pn/41PdHgOUJZpEkSW/yBz/ay56hyXl4psth3b/4xdUnJoHX5uF5m08qwHXLurnvmiXc+r5eFna29rIbSVLzqYt3tnw+H3O5XDzT/blc7n7g/lOPJZvNzlu22ZLJZBoyt+aeY0On47jQmczH2KjWIvuHX+YDq5Zw++VLZ/eX12pMPvs4k48/Am0Zuj7wEdquvn52n6MFtaVT3HzpEhZ1ONNCb+X7ic7EsaHTqfdxkWSBcTSXy12Yz+cP53K5C4FjZ3pgPp//NvDtU1fjmzeoahTZt22sJb3OsaHTcVzoTOZjbBRKFaYjXLe0nZuXzd5fiOPBfdT+8t/C3p1w462kvnQ/YdHiWfv9rW5RR9rXDf0S3090Jo4NnU5S42LFihVn9bgkC4zvA18D/vWpy+8lmEWSJJ1SLM0cw9nfM3sfE+JzT1L7P/4U2jtI/bN/Ces3NeVxqJIkae7M1zGqf8PMhp3ZXC53APgTZoqLfC6X+yawF8jNRxZJkvTOCuWZzTv7e9rO+3fFGGdOGPnuX8LFq0j9zh8T+mZ5WYokSWoJ83UKyRfPcNc98/H8kiTp7BVOzcDIdp9fgRGrVeLf/O/ER340s2TkG79H6OiYjYiSJKkF1cUmnpIkqX4UShUWtKfoaku9598Ry2PU/tO/ge3PET72OcJ9XyGk3vvvkyRJssCQJElvUSxXyJ7H8pE4Xqb2Z38C+3cTvv67pDY54VKSJJ0/CwxJkvT0iSlpAAAgAElEQVQWhVKVZb3vrcCIkxPU/v3/BPtfI/Vbf0S44eZZTidJklqVczklSdJbFMoV+rvP/d84YmWK2n/8V7DzZcI3f9/yQpIkzSoLDEmS9IZyZZrSVO2cl5DEauUXe158/VukbhqYo4SSJKlVWWBIkqQ3FEunjlA9hxNIYq1G/It/C88/Rfjyb5K6zT0vJEnS7LPAkCRJb3jjCNWes19CEn+YJz61mfArXyN158fnKJkkSWp1FhiSJOkNx04VGMvOcglJ3Po48fv/D2HjXYSP/spcRpMkSS3OAkOSJL2hWK6SDtDX+e4zMOKhfdT+/M/gkisIv/bbhBDmIaEkSWpVFhiSJOkNhVKFpd1tpFPvXEbE8hi1//g/Q3s7qd/+7wntHfOUUJIktSoLDEmS9IZCqUL/u+x/EWOk9uf/Kxw/Ruq3/oiwJDtP6SRJUiuzwJAkSW8olivvegJJ3PxjeOHnhM9/g/D+1fOUTJIktToLDEmSBMB0LVIsV8m+wwae8fgxYv4v4Oo1hLs8cUSSJM0fCwxJkgTAyYkqtcgZl5DEWo3ad/43AFJf+xYh5ccISZI0f/zkIUmSgJn9L4AzLiGJj/wTvPw8IffrhOzy+YwmSZJkgSFJkmYUSlUA+k+zhCQWjhC/+x1YvZZw+0fmOZkkSZIFhiRJOqV4agZG9jRLSOLf/hXESOqr3yKEdz5iVZIkaS5YYEiSJAAK5Qo97Sm629JvuT3uepn49BbCRz5DWNqfUDpJktTqLDAkSRIws4Qk+7b9L2KM1L77HVjYR/jwZ5IJJkmShAWGJEk6pViusOzty0e2PgE7txE+9SVCZ1cywSRJkrDAkCRJpxRKlbfMwIjVKrW//Su44CLCwIcSTCZJkmSBIUmSgHJlmrGp2ltOIIlP/AyOHiT12a8R0ul3+GlJkqS5Z4EhSZIolt96hGqMkfjg92HlJXDDzUlGkyRJAiwwJEkSvzhCtf/1PTBefREO7Cbc80mPTZUkSXXBAkOSJFEovXUGRu3B70PvQsItdyQZS5Ik6Q0WGJIkiUKpQirA4s4M8dhheO5Jwh0fJbR3JB1NkiQJsMCQJElAoVwh250hnQrEh38AqTThzo8lHUuSJOkNFhiSJIniqSNU4/Q08YmfEW68ldC3NOlYkiRJb7DAkCRJFMrVmf0vXnkBxkYIGwaSjiRJkvQWFhiSJLW46VrkeLlCf08b8ekt0NEF192YdCxJkqS3sMCQJKnFDU1UqdZgaWeK+MxjhDUb3LxTkiTVHQsMSZJaXLF86gjVkwdPLR/ZlHAiSZKkX2aBIUlSizs2VgEgu+tZ6OiE69YnnEiSJOmXWWBIktTiCuVTBcaOZ+HK61w+IkmS6pIFhiRJLa5YqtCdCXQfeo3w/tVJx5EkSTotCwxJklpcoVylPz0zC8MCQ5Ik1SsLDEmSWlyhVKF/agQybXDJ+5OOI0mSdFoWGJIktbhiqUJ2+DBcfiWhrS3pOJIkSadlgSFJUgsbr9QYnaqRLe4jrLom6TiSJElnZIEhSVILK546gaR/4iSsvCThNJIkSWdmgSFJUgsrlE4doTpxknDhRQmnkSRJOjMLDEmSWlixXAWgf3IIlq9MOI0kSdKZWWBIktTCCqUKqVhjcU8HoaMz6TiSJElnZIEhSVILK5QqLJkukb7A2ReSJKm+WWBIktTCCqUK/eMnCcsuSDqKJEnSO7LAkCSphRVKFbLl49C3NOkokiRJ78gCQ5KkFjVdixwvV2eOUO1bknQcSZKkd2SBIUlSixqaqFKNkJ0cIiyywJAkSfXNAkOSpBb1+hGq2Ykh6FuccBpJkqR3ZoEhSVKLKpQqAPRPDsEi98CQJEn1zQJDkqQW9XqBkZ0ahp7ehNNIkiS9MwsMSZJaVKFcpTtW6elsJ6T8SCBJkuqbn1YkSWpRxVKFbK0EvQuTjiJJkvSuLDAkSWpRhVKFbGUUehckHUWSJOldWWBIktSiCuUq/RND0OMMDEmSVP8sMCRJakET1Rqjk9MsLRcJzsCQJEkNwAJDkqQWVHz9CNWRI7BwccJpJEmS3p0FhiRJLahQrgLQP34CFvUlnEaSJOndWWBIktSCCqdmYGQnhgiLnIEhSZLqnwWGJEktqFCqkCKyZGrEJSSSJKkhWGBIktSCiuUKS9LTZGLNJSSSJKkhWGBIktSCCqUq2TA5c8UZGJIkqQFYYEiS1IIKpQr9U6PQu5DQ2ZV0HEmSpHdlgSFJUoupxUixXCU7fhz6L0g6jiRJ0lmxwJAkqcUMT0xTrUWyQ4cJFhiSJKlBWGBIktRiXj9Ctf/EAWdgSJKkhmGBIUlSiymUZwqM7MQJ6L8w4TSSJElnxwJDkqQWUyxVAchODruERJIkNQwLDEmSWkyhVKEr1OipjsMyZ2BIkqTGYIEhSVKLOVaqkK2VCd09sGhx0nEkSZLOigWGJEktpliu0D85BCsuJoSQdBxJkqSzYoEhSVKLKZSqZEeOEFZcnHQUSZKks2aBIUlSC5ms1hiZnCY7VgALDEmS1EAsMCRJaiGvH6HaPznkDAxJktRQLDAkSWohbxyhOjHkDAxJktRQLDAkSWohhdKpGRipCizsSziNJEnS2bPAkCSphRTKFUKMLMku8gQSSZLUUCwwJElqIYWxCounRmi76NKko0iSJJ0TCwxJklpIYbhE/8RJeN9lSUeRJEk6JxYYkiS1kOLoFEsnhwnOwJAkSQ3GAkOSpBZRi5FiJbBs8qQnkEiSpIZjgSFJUosYnpimQopsJhI6OpOOI0mSdE4sMCRJahGvH6GaXdSVcBJJkqRzl0k6QC6X+xfAbwAReAH49Xw+P5FsKkmSmk9hqATAsmWLE04iSZJ07hKdgZHL5VYC/xzYkM/nrwPSwK8mmUmSpGZVOFIEoH/lBQknkSRJOnf1sIQkA3TlcrkM0A0cSjiPJElNqXB8hM7pSXovdgNPSZLUeEKMMdEAuVzud4F/BYwDP87n818+zWPuB+4HyOfz66empuY35CzIZDJUq9WkY6gOOTZ0Oo4Lncn5jI3/7t9/n/1jVf7mDz9DCGGWkylpvm7odBwXOhPHhk4nqXHR3t4O8K4fThItMHK53GLgb4EvAEPA/wt8N5/P/9/v8GPx0KHGm6SRzWYpFotJx1AdcmzodBwXOpPzGRu/95eDLKxN8D9+84OznEr1wNcNnY7jQmfi2NDpJDUuVqxYAWdRYCS9hOSDwO58Pl/I5/MV4P8Dbks4kyRJTSfGSCF00d+Z9Fu/JEnSe5P0KST7gI25XK6bmSUk9wA/TzaSJEnNZ/LYUUbaesgubLxlmJIkSZDwDIx8Pv8E8F3gGWaOUE0B304ykyRJzai4Zz8A/f0eoSpJkhpT0jMwyOfzfwL8SdI5JElqZoXDx4AFZFcuTzqKJEnSe+JCWEmSWkChOAzA8sU9CSeRJEl6bywwJElqAYWxKUKMLOlqSzqKJEnSe2KBIUlSk4uVKYrVDItTFdrS73pCmSRJUl2ywJAkqdkdO0yxcxHZ9qSDSJIkvXcWGJIkNbsjByl09NG/oCPpJJIkSe+ZBYYkSU2udvgAxc4++hcvSDqKJEnSe2aBIUlSkxs+VqCSamPZoq6ko0iSJL1nFhiSJDW5wolRALI9mYSTSJIkvXcWGJIkNbEYI8WxSQD6uz1CVZIkNS4LDEmSmtnoMIUws3Skv8cCQ5IkNS4LDEmSmtmRgxQ7+uhMRXrbfduXJEmNy08ykiQ1sXj0IIXOPrJdaUIISceRJEl6zywwJElqZkcOUuxcQv+CzqSTSJIknRcLDEmSmlg8epBi1xL6e93/QpIkNTYLDEmSmtjUsSMMZbo9gUSSJDU8CwxJkppUrNUojswcoZr1BBJJktTgLDAkSWpWI0MU2xYA0N+TSTiMJEnS+bHAkCSpWZ0oUOjsA3AJiSRJangWGJIkNasTBYodfQRgabczMCRJUmOzwJAkqUnFEwWKnX30daZoS/uWL0mSGpufZiRJalYnihS6l9Lf0550EkmSpPNmgSFJUpOKxwsUu5bQ7wkkkiSpCVhgSJLUpOKJAsW2BRYYkiSpKVhgSJLUpEZGSkyFDFk38JQkSU3AAkOSpCYUJycpVtMAzsCQJElNwQJDkqRmdLJAobMPsMCQJEnNwQJDkqRmdKJAseNUgeESEkmS1AQsMCRJakLxRJFC52LaU7CgI510HEmSpPNmgSFJUjMaOk6xo4/+njZCCEmnkSRJOm8WGJIkNaOTJyh2L3X/C0mS1DQsMCRJakJx6DiFzj6yFhiSJKlJWGBIktSEKkMnGcr0sMwCQ5IkNQkLDEmSmtDxUgXwCFVJktQ8LDAkSWoysVqlMD1zdGrWI1QlSVKTsMCQJKnZDJ+k2NEHOANDkiQ1DwsMSZKazakNPMEZGJIkqXlYYEiS1GyGjlPs6GNxO7SlfauXJEnNwU81kiQ1mXjyOMXOPrLdLh+RJEnNwwJDkqRmM3ScQudi+hd0JJ1EkiRp1lhgSJLUZOLJEzMFhht4SpKkJmKBIUlSkxkZKTGVarPAkCRJTcUCQ5KkJlOYigBkLTAkSVITscCQJKnJFKtpAPrdxFOSJDURCwxJkppIjJFinNm8c1lPJuE0kiRJs8cCQ5KkZjJeptC+kHZqLOhIJ51GkiRp1lhgSJLUTMZGKHb00d82TQgh6TSSJEmzxgJDkqRmMjZCobOPbIflhSRJai4WGJIkNZOxEYodi8l2u/+FJElqLhYYkiQ1kanREU52LGTZgs6ko0iSJM0qCwxJkprI8eFxALJ93QknkSRJml0WGJIkNZHi2BQA/YssMCRJUnOxwJAkqYkcm6gBsKy3PeEkkiRJs8sCQ5KkJlKYmjl9ZKmbeEqSpCZjgSFJUhMpTmfomx6nPe1bvCRJai5+upEkqYkU6STLRNIxJEmSZp0FhiRJTaSQ6aE/NZV0DEmSpFlngSFJUpOIMVJsW0g2VUk6iiRJ0qyzwJAkqUmMTtWYTLfTn5lOOookSdKss8CQJKlJFEZm9r7ItsWEk0iSJM0+CwxJkppEYbgMQLYjJJxEkiRp9p11gZHL5f4sl8utncswkiTpvSuMTgLQ32WBIUmSmk/mHB6bBn6Uy+UKwP8F/HU+nz8wN7EkSdK5KpamaKtVWNTZlnQUSZKkWXfWMzDy+fw/B1YAfwisBbbncrkHc7ncV3O5XO9cBZQkSWenUKqSnRgi1dGZdBRJkqRZd057YOTz+el8Pv+DfD7/RWAj0A98BziSy+X+PJfLrZyDjJIk6SwUJ6bpnxwCCwxJktSEzmUJCblcbiHweeArwBrgb4HfBvYBvw/846nbJUnSPCtMwtqJIWhflXQUSZKkWXfWBUYul/su8BHgEeA/AX+Xz+cn33T/7wHDs55QkiS9q8p05GQFspND0NGRdBxJkqRZdy4zMB4Hfiefzx853Z35fL6Wy+WWz04sSZJ0Lk6MV4gE+idOQrsFhiRJaj5nXWDk8/k/PYvHlM8vjiRJei8KpSpwagZGu3tgSJKk5nNOm3hKkqT6VChVAOifcBNPSZLUnCwwJElqAoXyTIGxdHLIJSSSJKkpWWBIktQEiqUqi6jQEachc06HjEmSJDUECwxJkppAoVQhGyahrY0QQtJxJEmSZp0FhiRJTaBQrtAfxyHTlnQUSZKkOWGBIUlSg4sxUihV6a+NQ1t70nEkSZLmhAWGJEkNrjRVY6JaIztdcgaGJElqWhYYkiQ1uNdPIOmvjjkDQ5IkNS0LDEmSGtyx0qkjVCsjzsCQJElNywJDkqQGVyxVAeifHIE2CwxJktScLDAkSWpwhVKFtlRgYWXEJSSSJKlpWWBIktTgCuUK2Z4MqUrFGRiSJKlpWWBIktTgCqUq/T1tUKm4B4YkSWpaFhiSJDW4YqlCf3cbVKcILiGRJElNygJDkqQGVq1FToxX6e/JwMQ4dHYlHUmSJGlOZJIOkMvl+oA/B64DIvCNfD7/WLKpJElqDMfLFSKQ7W6D8XHo7E46kiRJ0pyohxkY/w74p3w+fzVwA7A94TySJDWMN45Q7U7D5Dh0OQNDkiQ1p0RnYORyuUXAB4CvA+Tz+SlgKslMkiQ1kkK5AkA2U5u5wRkYkiSpSSW9hOQyoAD8ZS6XuwF4GvjdfD5fevODcrnc/cD9APl8nmw2O+9Bz1cmk2nI3Jp7jg2djuNCZ/L2sVF6bRyAK5f0MAr09i+j27HTknzd0Ok4LnQmjg2dTr2Pi6QLjAxwI/CtfD7/RC6X+3fAHwL/w5sflM/nvw18+9TVWCwW5zflLMhmszRibs09x4ZOx3GhM3n72NhbGGZRR5rSsUMAjFWnKTt2WpKvGzodx4XOxLGh00lqXKxYseKsHpf0HhgHgAP5fP6JU9e/y0yhIUmSzkKxXCHb0wbjZQCCS0gkSVKTSrTAyOfzR4D9uVzuqlM33QNsSzCSJEkN5Vip8osjVMFNPCVJUtNKegkJwLeAv87lcu3Aa8CvJ5xHkqSGEGOkUKqy9oIemJiZgeEmnpIkqVklXmDk8/mtwIakc0iS1GhKUzUmqjX6e9qIx08VGF0WGJIkqTklvQeGJEl6j944QrUn88YeGM7AkCRJzcoCQ5KkBlUozRQY/d1tMDoM6bQzMCRJUtOywJAkqUEVSlUA+nvaYGQIFiwipHxrlyRJzclPOZIkNahiuUImFVjUmSaODMHCvqQjSZIkzRkLDEmSGlShVCHbnSEVwswMDAsMSZLUxCwwJElqUIVSdWb5CMDIEGHh4mQDSZIkzSELDEmSGlShXKG/J0OM0RkYkiSp6VlgSJLUgKq1yMnxKtnuNiiPwXTVAkOSJDU1CwxJkhrQiXKVWnzTCSRggSFJkpqaBYYkSQ2oUK4Aby0wggWGJElqYhYYkiQ1oELpVIHRnSEOn5y50U08JUlSE7PAkCSpARVLVQCyLiGRJEktwgJDkqQGVChXWNCR/v/bu+8oucoETfPPzYg0UqZMKo28AyGBHCAJW0DhTQGFv0VRQOGn6nT31kx170xX9+5sn57tPT07O9Ndp2emawDh7cVD4QvvkcEIkIQRQiDkUilkUlK6+PaPSECITBCQyhuR8fzOuSfcjdKL9NWNyDe/+12qsmXwWTNkslBdk3YsSZKk3cYCQ5KkIrSupZ3G6iwAYd0qaBhOVObHuiRJ6r/8piNJUhFqaum6hCrA2lXQMDLdQJIkSbuZBYYkSUVo3dZ26qvLCSHA2lVEw0elHUmSJGm3ssCQJKnItLR1srU9lz+FZOMGaGuFRmdgSJKk/s0CQ5KkIvPlJVTLYe2nAESeQiJJkvo5CwxJkopM09YvL6Ea1q7KP+kpJJIkqZ+zwJAkqch8MQOjujy/gGcmA8MaUk4lSZK0e1lgSJJUZNa1tJMtg6FVmXyBUTecKJNJO5YkSdJuZYEhSVKRWdd1CdWyKCKsW+UCnpIkqSRYYEiSVGS+cgnVNV5CVZIklQYLDEmSisy6lnYaBmZhQxO0bnMBT0mSVBIsMCRJKiIduUDzto78Ap4fvgtANGFyuqEkSZL6gAWGJElFpGlLK7mQvwJJ+PBdyGZhzIS0Y0mSJO12FhiSJBWRNZtbgR0KjLF7EJWXp5xKkiRp97PAkCSpiHxeYNRVlsHy94kmevqIJEkqDRYYkiQVkS9mYGxeBW2tYIEhSZJKhAWGJElFZM3mVgZVZqhc8T6AMzAkSVLJsMCQJKmIrNncmr+E6ofvwsAaaByZdiRJkqQ+YYEhSVIRWbO59csFPCfuRRRFaUeSJEnqExYYkiQVkTWbW6mvjGDlCk8fkSRJJcUCQ5KkItHS1smWtk4atjVDyBHtMSXtSJIkSX3GAkOSpCLRtLUDgPpPlkBlFUyZkXIiSZKkvmOBIUlSkVjX0g5A3bvziWYeQFRRmXIiSZKkvmOBIUlSkfi8wGhoXkk0+0cpp5EkSepbFhiSJBWJdS3tZMkxlDaYPjvtOJIkSX3KAkOSpCKxrqWdutaNZGbMJqr09BFJklRaLDAkSSoSn65upm7bBqJDjko7iiRJUp+zwJAkqQgsXLmF91vLOaBjNcyYk3YcSZKkPmeBIUlSgWvvDFz90gpGbGviZ0dMJSrz41uSJJUevwFJklTgHnp3Aytby7hk1dMMPvKEtONIkiSlwgJDkqQCtmFbB7e+sZZZ6xdzwEHTiMrL044kSZKUCgsMSZIK2A2vr6O9I8cln/yJ6HBnX0iSpNJlgSFJUoFa2rSNJ5dt5JSPn2X0wQcRDRiYdiRJkqTUWGBIklSAciFw1fw11IZWzln5DNGxp6YdSZIkKVUWGJIkFaAnl23kvfXbufDd+xh40OFEg2vTjiRJkpQqCwxJkgpMS1snN7y+jilhI0esWUh0/OlpR5IkSUqdBYYkSQXmtkVNbNreyWULr6PsqJOJho9KO5IkSVLqLDAkSSogH29s5cGlGzi2ZSl7hs1EP/152pEkSZIKggWGJEkFInQt3FkV5Tjv9duIzriAaGBN2rEkSZIKggWGJEkF4pVPtvDG6q2cu+IJhowcQXTYsWlHkiRJKhgWGJIkFYDWjhxzF6xlXLSVEz94grKfX05Ulkk7liRJUsGwwJAkqQDcu7iZtS3tXPrmLWQPOoJo0tS0I0mSJBUUCwxJklK2rqWdO99ez6GtHzNjywqis36ZdiRJkqSCY4EhSVLKrl24FnKdXLjwxvzCnUPr0o4kSZJUcCwwJElK0aI1LbywYjNnrniaxj3GEx11ctqRJEmSCpIFhiRJKenMBa6at4bGjs2ctvolyi76DVGZH82SJEnd8VuSJEkpeeS9z/hoYxsXLbmbqp9fRjSsPu1IkiRJBcsCQ5KkFGza3sHNr69h5ob3OHhCLdGBR6QdSZIkqaBZYEiSlIKbXlvDtvYcl65+mrLzf00URWlHkiRJKmgWGJIk9bEPmrfz2LJN/OSTFxh/7nlE1YPSjiRJklTwsmkHkCSplIQQuPLZZQxua+HcsRBNn512JEmSpKLgDAxJkvrQM++uY0lLGec3vUTN2RekHUeSJKloOANDkqQ+srW9k+vmfcqkzes45szjiSqr0o4kSZJUNJyBIUlSH7njybfYEFVxecNmMnvunXYcSZKkomKBIUlSH1i5upn712Y5atNippx2atpxJEmSio4FhiRJfWDuo4soz7Vz4fH7EpWXpx1HkiSp6FhgSJK0m7361CssyA7nZzXNDNtzj7TjSJIkFSULDEmSdqO25mauWdbB6LYNnHzqEWnHkSRJKloWGJIk7SYhBO676wlWVdVx2QEjqajw4l+SJEnflwWGJEm7SdNzT3Nn5WQOrNjMrOkT0o4jSZJU1CwwJEnaDULzOm54bR2dZVkuOWFG2nEkSZKKngWGJEm9LITA27fcyrP1Mzl9j4GMHFyVdiRJkqSiZ4EhSVIv63j6Ea7OTqMu08HZB45PO44kSVK/YIEhSVIvCmtX8fhzi/hw0GguPngsVVk/aiVJknqD36okSeolIZdj4/X/yi3jjmXasCyHjR+cdiRJkqR+wwJDkqReEp5/jNty42kpH8AVB48hiqK0I0mSJPUbFhiSJPWCsGkDHz70MI+OPoQT96plQq0Ld0qSJPUmCwxJknpB7vZrmDvuBKrLyzhv34a040iSJPU7FhiSJP1A4e3XeOHDDbw9ZA9+sf9wBlVm0o4kSZLU71hgSJL0A4S2VrbechXXTT6NiUMrOH7S0LQjSZIk9UvZtAMAxHGcAeYDK5MkOSXtPJIk7arwYMLdA6eyvnwQf3XACDJlLtwpSZK0OxTKDIzfAIvTDiFJ0ncRVq5g1TNPc9/4ozhiwmCmNg5MO5IkSVK/lXqBEcfxGOBk4Oq0s0iStKtCLkfupv/JdZNOJZPNctH+LtwpSZK0OxXCKST/DPx7YFBPO8RxfAVwBUCSJNTX1/dRtN6TzWaLMrd2P8eGuuO4KHxbH7+f59a38+qYvfnVQeOYMm5kn/y5jg31xLGh7jgu1BPHhrpT6OMi1QIjjuNTgLVJkiyI4/jInvZLkuRK4Mquh6Gpqakv4vWq+vp6ijG3dj/HhrrjuChsYdNntF7/P5m7358zoqacY8dV9tm/l2NDPXFsqDuOC/XEsaHupDUuRo0atUv7pX0KyY+An8ZxvBy4DTg6juOb0o0kSdI3C3dey8N1+7OyfCiXzR5OeSbtj1NJkqT+L9UZGEmS/A74HUDXDIy/SpLk/DQzSZL0TcLSRTQvmMfth/4ts0dVM2d0ddqRJEmSSoK/MpIkaReFjnZyN/0rN+99Bu1RhktnDyeKvGyqJElSXyiERTwBSJLkaeDplGNIktSj8Og9vNcS8eSwGZy5zzBGD65IO5IkSVLJcAaGJEm7IKxbTeeDd3DVfudTOyDLOdPr0o4kSZJUUiwwJEn6FiEEcrf8L54aPov3M7VctH8DA8szaceSJEkqKRYYkiR9m4Uv0bL4bW6afCpT6gfw4wmD004kSZJUciwwJEn6BmH7VnK3XUUy40w25bJcMceFOyVJktJQMIt4SpJUiML9t/JJe5aHavfjuD2HMKmuKu1IkiRJJckZGJIk9SB8spzcEw8wd85FVJWXcf6+DWlHkiRJKlkWGJIkdSO/cOcfeHXULN6I6jhvZj1Dqpy4KEmSlBYLDEmSuhFeeZrWD97l2ilnMm5IBSftVZt2JEmSpJJmgSFJ0k7C1hbCHddy38wzWduZ5fI5w8mUuXCnJElSmpwLK0nSTsL9t7CuLeLuYXM4dMwgZo6oTjuSJElSyXMGhiRJOwgff0h48kGuP/hSiCIu3r8x7UiSJEnCAkOSpC+EXI7cLX9g0YhpvEgjZ02ro7GmPO1YkiRJwgJDkqQvhJeeovODpcydfi6N1VnO2GdY2pEkSZLUxQJDkiQgtGwh3HUdj848jRXt5Vw8q5HKrB+TkiRJhcJFPCVJIr9w58bWHLc2HMLM+oEcMnZQ2pEkSZK0A3+1JEkqeWHlR+IKKE8AACAASURBVISnH+LWQy9hWydcPns4UeRlUyVJkgqJBYYkqaSFEMjdfjUfDNuDxxnNyVNqGTe0Mu1YkiRJ2okFhiSptL32MmHxG8zd/wIGV2Y4d0Z92okkSZLUDQsMSVLJCu1t5O64hmf3Po4l7QO4YL8GaioyaceSJElSNywwJEklKzx2L9s2fMYNY49l0rAqjtlzSNqRJEmS1AMLDElSSQrNTYSH7uDOAy9gQ3vEFQcMp8yFOyVJkgqWl1GVJJWkcNf1fFpRywNVkzl64mCm1A9IO5IkSZK+gTMwJEklJ7z/DuHVZ7j2oEsoz5Rx4X6NaUeSJEnSt7DAkCSVlJDrJHfrVcwfdyALOody7sw6agc4IVGSJKnQWWBIkkpKeOEJ2j9ezrVTTmf04ApOnjws7UiSJEnaBRYYkqSSEbZvJdxzIw/sdxar2rNcNruR8owLd0qSJBUD58xKkkpGeOhOmlsDd9bO4aBRNcwaVZN2JEmSJO0iZ2BIkkpCaFpDePw+bjjoYjqJuGSWC3dKkiQVEwsMSVJJCHffwOIh43k2M5rT9xnGiEEVaUeSJEnSd2CBIUnq98IHS+iY9zxz9z2fugFZzp5el3YkSZIkfUcWGJKkfi3kcuRuv5o/7XEUy3IDuWhWI1VZP/4kSZKKjd/gJEn9Wpj3HJs//phbJhzHtMYBHD5+UNqRJEmS9D1YYEiS+q3Q1kq4+3pum3EOLSHD5XOGE0VeNlWSJKkYWWBIkvqt8Ph9LG/N8uiQaZwwaSgTa6vSjiRJkqTvyQJDktQvhc+ayT18J3P3v4Dqigzn7duQdiRJkiT9ABYYkqR+Kdx3My8O3Ye3sw38Yt8GBldm0o4kSZKkH8ACQ5LU74QVy9j20jNcv8+ZTKyt5PhJQ9OOJEmSpB/IAkOS1K+EEMjdfjX37Hk8TVRy+ZzhZMpcuFOSJKnYWWBIkvqXhS+yesWn3DvqMI4YP5hpjQPTTiRJkqReYIEhSeo3QlsruTuu5brpMZlshotmuXCnJElSf2GBIUnqN8Jj9/B6bgiv1uzBOdPqqRtYnnYkSZIk9RILDElSvxCa19H+yN3MnX4uI2rKOW2f2rQjSZIkqRdZYEiS+oVw1/U8NPxgVmYGcdns4ZRn/IiTJEnqT/x2J0kqeuG9d2h+fSG373ECs0dVM2d0ddqRJEmS1MssMCRJRS3kcuRuu4qbp5xOe5ThktmNRJGXTZUkSepvLDAkSUUtvPgE721o48m6mZy69zDGDK5MO5IkSZJ2AwsMSVLRCltb6Lz7Bq6efi61VRniGXVpR5IkSdJuYoEhSSpa4cGEp6on815lA7/cv5GB5Zm0I0mSJGk3scCQJBWlsHolW55+jJum/JQp9QP48cTBaUeSJEnSbmSBIUkqOiEEcrddSTLxODZFlVwxZzhlLtwpSZLUr1lgSJKKzxuv8MmHK3lo5CEcN2kIk+qq0k4kSZKk3cwCQ5JUVEJbK523Xc3caTGV5RnO37ch7UiSJEnqAxYYkqSiEh65m1ejBt6oHsd5M+sZUpVNO5IkSZL6gAWGJKlohHWr2f7YfVw79RzGDangpMm1aUeSJElSH7HAkCQVjVwyl/tHH87asoFcPmc42TIX7pQkSSoVFhiSpKIQFi1g3eKl3DXuKA4dN4iZI6rTjiRJkqQ+ZIEhSSp4ob2d3G1Xcf20syGT4eL9G9OOJEmSpD5mgSFJKnjhT/fxVlsVLw6ezJlTh9FYU552JEmSJPUxCwxJUkELzU10/PEO5s74OY3VWc6cWpd2JEmSJKXAAkOSVNDCndfy6PA5fJQZwiWzhlOZ9aNLkiSpFPktUJJUsMKSN9n4+gJu3fMkZo4YyMFja9KOJEmSpJRYYEiSClLo7CR321XcsvfpbIuyXD57OFHkZVMlSZJKlQWGJKkghWcfYdnGdh6v25eTp9Qybmhl2pEkSZKUIgsMSVLBCVs2kbv3Zq6eeR6DK7OcO6M+7UiSJElKmQWGJKnghPtv4dlBk1lS0cgF+zVQU5FJO5IkSZJSZoEhSSoo4ZPlbH3uSW7Y5wwmDavimD2HpB1JkiRJBcACQ5JUMEII5G67irv2PIENVHLFAcMpc+FOSZIkYYEhSSokr73EqhWfcv/IQzl6j8FMqR+QdiJJkiQVCAsMSVJBCG2t5JJruGb6zyjPZrhgv8a0I0mSJKmAWGBIkgpCeOxe5odhLKiewLkz6xg2IJt2JEmSJBUQCwxJUupCcxNtj9zDtdNiRg+u4OTJw9KOJEmSpAJjgSFJSl2463r+OOJgVmVquGx2I+UZF+6UJEnSV1lgSJJSFd5/h/Wvv8YdE4/jwDE1zBpVk3YkSZIkFSALDElSakIuR+62q7lx79PpLMtwySwX7pQkSVL3LDAkSakJrzzD4g3tPDNsOqfvU8fIQRVpR5IkSVKBssCQJKUitG6n4+4buXraz6gbkOXs6XVpR5IkSVIBs8CQJKUiPHYvTwzYkw8r67loViNVWT+SJEmS1DO/LUqS+lzYsJ7Njz/IzXudwrTGARw+flDakSRJklTgLDAkSX0u3HMjt449mpaySi6fM5wo8rKpkiRJ+mYWGJKkPhU+ep/lb77Do6MO5oS9hjKxtirtSJIkSSoCFhiSpD4TQqAzmcvcvc+kujzDefs2pB1JkiRJRcICQ5LUd157mZc2lPHWoAn8Yr8GBldm0k4kSZKkIpFN8w+P43gscAMwHAjAlUmS/D7NTJKk3SO0t7Ptrhu5bvLFTBhawfGThqYdSZIkSUUk7RkYHcBfJkkyFTgY+LM4jqemnEmStBuEp/7I3QOn0lQ+iCsOGEGmzIU7JUmStOtSLTCSJFmVJMnCrvubgcXA6DQzSZJ6X9i8idWPP8a944/iiPGDmdY4MO1IkiRJKjKpnkKyoziOJwD7A69089oVwBUASZJQX1/ft+F6QTabLcrc2v0cG+pOfxsXm+6+nuvGHEM2m+W3x06hvqYy7UhFq7+NDfUex4a647hQTxwb6k6hj4sohJB2BuI4rgGeAf4hSZK7v2X38Omnn/ZBqt5VX19PU1NT2jFUgBwb6k5/Ghdh9UoW/tM/8fczL+OCfRs4e3pd2pGKWn8aG+pdjg11x3Ghnjg21J20xsWoUaMAvvX84rTXwCCO43LgLuDmXSgvJElFpu2em7hm0k8ZMTDDT/epTTuOJEmSilSqBUYcxxEwF1icJMl/SzOLJKn3hQ+W8PDaMj4Z2MilB4ygIpN6by5JkqQilfYaGD8CLgAWxXH8etdzf5MkyUMpZpIk9YIQAs333MbtE09n1vAqDhhdk3YkSZIkFbFUC4wkSZ5nF85zkSQVoTfncTN70Jap5NIDRxJFHu4lSZL0/TmXV5LU60JnJ0sffIQnRh7IKXvXMmawVx2RJEnSD2OBIUnqdZ0vPMHVtQdTm8nxs5kNaceRJElSP2CBIUnqVaG1ladeWMR7g8dx4YGjGFieSTuSJEmS+gELDElSr9rypz9y04gjmFIdOHLikLTjSJIkqZ+wwJAk9ZqweRO3L97IxvIaLj98AmUu3ClJkqReYoEhSeo1Kx58gIeGH8Sxo7LsVTcg7TiSJEnqRywwJEm9Ird2Fdc0DaIyynHBoRPTjiNJkqR+xgJDktQrXvnjE7xeO5nzpg1lSFU27TiSJEnqZywwJEk/2PYVy7kmtwdjo62ctO+YtONIkiSpH7LAkCT9YPc+vpC1A4Zx2SFjyZa5cKckSZJ6nwWGJOkHWfveB9xVPolDMhvYb2JD2nEkSZLUT1lgSJJ+kGufXwZRxMXH7pN2FEmSJPVjFhiSpO/tzdeW8GLFWM4cuJ7h9UPTjiNJkqR+zGXiJUnfS2cucPWbn9HQCWecPiftOJIkSernnIEhSfpeHnlxMR9lh3Jx/RaqqmvSjiNJkqR+zgJDkvSdbdrewS0ftjNz84cccvxhaceRJElSCbDAkCR9Zzc9s5StUZZLJ5RRVlWVdhxJkiSVAAsMSdJ38sH6bTy2royTml5j/DFHpR1HkiRJJcJFPCVJuyyEwFXPLmNQ+zZ+vm8jUXlF2pEkSZJUIpyBIUnaZc8s38TirRnOX/s8NYc7+0KSJEl9xxkYkqRdsq09x/WvrmTPTSs55kfTibLlaUeSJElSCXEGhiRpl9zxVhPNHWVctv55Moc4+0KSJEl9yxkYkqRv9emmNu5bvJ6jVi9g76MOJ8pk0o4kSZKkEuMMDEnSt5q7YA3lne38YsM8ooOOTDuOJEmSSpAFhiTpG81fuYX5n7ZwzoePUXf8SURZJ+9JkiSp71lgSJJ61N6ZY+6CNYxu38jJLUuIDjkm7UiSJEkqURYYkqQePbBkA59ubufixXdQccLpROVeeUSSJEnpsMCQJHVr/dZ2bn9rPQds/5hZnU1Ehx+fdiRJkiSVMAsMSVK3bnhtHR2dnVz8+s1EJ5xBVF6RdiRJkiSVMAsMSdLXLF67laeXb+K0Le8woryT6IgT044kSZKkEmeBIUn6is5c4Mr5a6irgLMW3EJ0/OlElZVpx5IkSVKJs8CQJH3Fnz7YyLINrVzY/DJVA6qIjjwp7UiSJEmSBYYk6UtbWju58Y11TB0ccdj8u4mO/SlR1cC0Y0mSJElk0w4gSSoctyxqoqWtk0vXP0E0oJro6FPSjiRJkiQBzsCQJHVZvmE7D7+7gRNGZpk4/1GiY04hGliddixJkiQJsMCQJAEhBK5asJbq8jLOff8hqKwiOubUtGNJkiRJX7DAkCTx4orNvLVmK+ftUcGgeU8SHXkSUc3gtGNJkiRJX7DAkKQS19qR45qFa5lYW8lxbz0AmQzRcaenHUuSJEn6CgsMSSpxd72znqatHVy2VyVlLz1FdPhxRENq044lSZIkfYUFhiSVsDVb2rj77WaOGD+YqfMeAALRCWelHUuSJEn6GgsMSSph1yxcS1kEF06qIDz/ONHBRxHVNaQdS5IkSfoaCwxJKlGvr2rh5Y+3cM70OuqefwA6OohOOjvtWJIkSVK3LDAkqQR15AJXzV/DiJpyfjq2nPD0I0QHHEY0fFTa0SRJkqRuWWBIUgl6cOkGPtnUxqWzGyl/+kFo3Ub0k3PSjiVJkiT1yAJDkkrMZ9s6uG1RE7NGVjNnWBnhyQdgv4OJRo9PO5okSZLUIwsMSSoxN76xjrbOHJfOaYRnHoGtLZSd7OwLSZIkFTYLDEkqIe82beNPH2zk1CnDGF0J4fF7Ydr+RBP2SjuaJEmS9I0sMCSpRORC4Mr5a6ityhDPqCM8/RBs3kjZT+K0o0mSJEnfygJDkkrEU8s28t767Vy4fyMD2rYRHrojP/ti8rS0o0mSJEnfygJDkkpAS1snN7y+jin1Azhy4mDCw3fAthbKzroo7WiSJEnSLrHAkKQScPuiJjZu7+TyOY1EzU2EJ/5IdPCRRGMnph1NkiRJ2iUWGJLUz328sZU/Lt3AsXsOYa+6AYT7bgIgOu38lJNJkiRJu84CQ5L6sRACVy9YS1W2jPP3ayB8/CHh5aeJjjmVqK4h7XiSJEnSLrPAkKR+7NVPtvD6qhZ+PrOeoVVZcnddBwOqiU46O+1okiRJ0ndigSFJ/VRrR465C9cybkgFJ02uJbzzOrz9GtHJMVF1TdrxJEmSpO/EAkOS+qn7FjezZks7l80ZTqazg9xtV0FdI9FRJ6cdTZIkSfrOLDAkqR9a19LOHW+v55Cxg9h3RDXhgVtg1ceUnf9rovLytONJkiRJ35kFhiT1Q9e9thaAS2Y1EpYtJTxyD9HhxxNNn51yMkmSJOn7scCQpH5m0ZoWnv9oM2dNraOhMpC79vdQO4zonEvSjiZJkiR9bxYYktSPdOYCV81fS2N1ljOmDiPcdzOs/oSyX/4F0YCBaceTJEmSvjcLjCIVQkg7gqQC9Mh7n/HRZ61cPKuRiuVLCY/dS3TEiURT9087miRJkvSDZNMOIAhtrbBlE2zZDC2bCVs2Q8uXj9mymbBlU9f9rtutLVBWBuUVUF4O2QqoqMg/zpZ/+XxFJdFOj7/6egXR0DpoHAkNI/0NrVTENm3v4JY31zFzxEAObqwg959+D8MaiM65KO1okiRJ0g9mgdEHwpI32bLifXJrV+fLiK5SgpauoqKtrec3Vw6AmkFQPQhqBhM1jMjfr66BXA7a26C9/Yvb0N765eO21nwh0t6ev9/R/tX9P8+34583aAg0jiRqGJkvNRpH5v/MxpFQPYgoinbb35OkH+bmN5vY2p7j8tnD4d4bYO2nlP32PxFVWUxKkiSp+Flg9IGwdBEtD90BA2u+LCPqGojG7bFDOTGIqGYwVA/+8rnqQbvtcochBOjogLbt0NwEa1cR1q6Cdfnb8O4iePmp/L6fv2lgdX6WRtdsDRq77jeOhMFDLTekFC1r3s6j733GKVNqGbPkRcITDxAddTLRPvumHU2SJEnqFRYYfSA6Oabh4r9gfXNz2lG+EEVR/hSS8vJ8WTJ2IjvXD6GtFZrW5MuNdau/KDnC8vdgwQuQy31ZblRWQddMjfzsjRFdt6Ogto6ozOVWpN0lhMCV89cwqDLDz2qaCb//F5gygyj2qiOSJEnqPyww+kCULS/KH+CjikoYNQ5Gjft6udHRAc1rd5i5sTp/++nHhDfnQUfHl+XGgGqYPI1oygyiKdNhzMSi/PuQCtWzyzexeN02/mzaQAZe+fcwrIGyX/91fv0bSZIkqZ+wwND3EmWz+dkVjaO+Xm7kOmFDM6z9NF9qfPQ+YelbhDdezZcaA2t2KDRmwOjxFhrS97StPcd1r61jUm0FR937X6Cjg7K/+D+JqgelHU2SJEnqVRYY6nVRWQbqGvLrfOxw/n1obiK8+xYsXURYuojw+iv5QqN6UFehMTM/Q2PUOAsNaRfd8VYTzds6+N9XPkbZqo8p+83fEY0Yk3YsSZIkqddZYKjPRMPqiQ4+Eg4+EoCwfl1XofFmfobGay/nC42awTB5OtHeM4gmz4BRY10gVOrGp5vauG9JM0dufZ8pi54g+uX/RjR1v7RjSZIkSbuFBYZSE9U1EB1yFBxyFABh/VrC0kWwpGuGxsIX84XGoCFEk6fD3l2nnIwYY6EhAXNfXUl5Rxvnv5kQXf5XlB1weNqRJEmSpN3GAkMFI6prJDr0GDj0GADCutX5GRpdhQYLXsgXGoOH5ouMzxcFHT7aQkMlZ97ST5m/ppULP3qC+sv/gmjmAWlHkiRJknYrCwwVrKhhBFHDCPjRsYQQoGkNYcmb8O5bhCWLYN5z+UKjcSTRnMOIDjgMRk+wzFC/17ZmFXNfWM7oXCennnUc0T7T044kSZIk7XYWGCoKURTB54XG4cfnC411qwiL3yQsfInwyF2Eh+7In17SVWZEo8alHVvqdWHlR9x/80OsGn0U/3GfMir2mZx2JEmSJKlPWGCoKEVRlL+Ea+Mo+PGJhM0b80XGvOcIDyaEP96WvzzrnMPy24jRaUeWfrDci0+yPrmJO2b9hgPrypg9y/JCkiRJpcMCQ/1CNGgI0Y9PzJcZGzcQFrxAmPc84b6bCffdDGMnEh1weL7MaBiRdlzpOwmtrYRb/0B44QluPPDf0FFeySU/mpB2LEmSJKlPWWCo34mG1BIdfQocfQphw3rCgufzZcbdNxDuvgEm7PXlzIy6hrTjSt8ofLqC3P/6f2HVxyw56RKe2bYnZ+9Tx8hBFWlHkyRJkvqUBYb6tai2jujY0+DY0/KXaZ3fVWbceS3hzmthz727yowfEQ2tSzuu9IXQ2Unu6YcJd1wDlVWE3/wdc1cMpY5OzpnuWJUkSVLpscBQyYjqGolOOBNOOJOwdtWXZcbtVxOSubDXVKI5hxPNPpRo8NC046qEhaVv0XzntYTl78E++1J2yb/lsXUZPmhezV/+aBRV2bK0I0qSJEl9zgJDJSlqHEn0k3PgJ+cQVn+SLzLmPUe45Q+EW6+EfWbmZ2bMOpSouibtuCoRoWkN4c7rCAtegIYRlP3qP8CsQ2lpy3HjG8uY2jCAw8cPSjumJEmSlAoLDJW8aMQYolPPhVPPJaz8iPDqc4R5zxJu+O+Em/8A0/bPX5Z1v4OIqgamHVf9UNiyifCn+wmP3QsRRKedR/3PL2f95s0A3LKoiZa2Tq44YHj+CjySJElSCbLAkHYQjR5PdMZ4wum/gBUf5MuM+c8R3pxHKK+AGXMoO+AwmHEAUWVl2nFV5MLKFYQnHyC8/BS0tREd+GOisy4kGtaQH1+bN7N8w3YefncDJ0waysTaqrQjS5IkSamxwJC6EUURjJ9ENH4S4axfwrIl+dNM5j9PbuGLkC3Pr5kxdT+iqfvDmAlEZa5LoG8XcjlYtIDcE/fD4jegvILo4COJjj6FaMyEr+4bAlctWEt1eRnn7esVcyRJklTaLDCkbxGVlcGkqUSTphJ+diksfYvw5nzCO68R7rqecNf1MGgI0T77wdT98qVGrVeJ0JdCLgcfvkt449X8+hZrV8HQOqIzLiA6/ASiQYO7fd+LKzbz1pqt/OqA4QyuzPRxakmSJKmwpF5gxHF8IvB7IANcnSTJP6YcSepRVJaBffYl2mdfAMJn6wnvvAHvvEZY/Aa8+gwBYPhoonF75GdmjN0Dxk6EIbWuX1BCQlsrLHmT8PorhDfnwcYNUFYGk6cTnfaL/AKx2Z4PwdvbO7lm4Vom1lZy/CSviiNJkiSlWmDEcZwB/gdwHPAJMC+O4/uTJHknzVzSroqG1hEdejQcejQhBFi5nPD264T3FxM+fBfmPZcvNAAGDYGxE4nGTISRY4hq66G2jtzAAWn+J6gXhM5OWL2S8PEHsGIZYcUyWP4etG6HqgFE02fDvgcSzZizy1e1uWn+JzRt7eC3h44iU2bxJUmSJKU9A+NA4P0kSZYBxHF8G3Aa0K8KjPkrt7D07U1s274t7Sja7QZC/aH5DaC9HbZsgs2bYPNGwpZN8OEm+GA5sLzrPY9CNguVVVBZRVQ1AMorIJPNP5/N5tfc2PFxpmuLovxGBBFffQwQcpALXbc5CKHrtqf7Ozz3+ftCyG903Qa67rPDc4Fdtis/i3f9cdCVg/Dlbdhhi8qgLPrqbRTlZzp0+9zn+3b33A777vy4sxPaWqG9bYfbNmjbnv833bIJOnP57JkyqN6P6JAjoX4E1NbnnwNY0gK07NJ//6Pvb+SI8YOZNtwr30iSJEmQfoExGvh4h8efAAftvFMcx1cAVwAkSUJ9fX3fpOsln36wlQffWZV2DKWmHKiDAXUwAGhgh9Ig5Gdu7FgUhABtX/wEv4OOrq0vFdLCpN/WfOxKiRKAzq7t+yoDqoABEA2BmpEwOANlZfn1UnZczHUTsGnL9/pTRg+t4t8dM4X6QV7tRl+VzWaL7nNQfcOxoe44LtQTx4a6U+jjIu0CY5ckSXIlcGXXw9DU1JRmnO/sp3sO5JKDDqHYcqtv1NfXdzs2QmcntG6Dbdtg+1bYvg22bYXtWwnbt+Uff2XGxE4zKKBrpkbmq7M2up6Lsl/e/2J2x+f3MxnIlHfdZnaasdDd/R1mfXyj7zBToyzTtZXt0hVeQq4zP1Ois+PL247PH3d89bWObp7r7Mj/nXd2Qm7H2w4or8wvtFkzJH8q0KDBMLBmt1555vNx0dS6ebf9GSpOPR0zJMeGuuO4UE8cG+pOWuNi1KhRu7Rf2gXGSmDsDo/HdD0nlbwok4GBNflt59dSyFPoos8Lj/KK7/+/0Yt5JEmSJPWutAuMecBecRxPJF9cnAucl24kSZIkSZJUaFI9wT1Jkg7gz4FHgcX5p5K308wkSZIkSZIKT9ozMEiS5CHgobRzSJIkSZKkwlVIlxiQJEmSJEnqlgWGJEmSJEkqeBYYkiRJkiSp4FlgSJIkSZKkgmeBIUmSJEmSCp4FhiRJkiRJKngWGJIkSZIkqeBZYEiSJEmSpIJngSFJkiRJkgqeBYYkSZIkSSp4FhiSJEmSJKngWWBIkiRJkqSCZ4EhSZIkSZIKngWGJEmSJEkqeBYYkiRJkiSp4FlgSJIkSZKkgmeBIUmSJEmSCp4FhiRJkiRJKngWGJIkSZIkqeBZYEiSJEmSpIJngSFJkiRJkgqeBYYkSZIkSSp4FhiSJEmSJKngWWBIkiRJkqSCZ4EhSZIkSZIKXhRCSDvDd1V0gSVJkiRJ0jeKvm2HYpyBERXjFsfxgrQzuBXm5thw625zXLj1tDk23HraHBtu3W2OC7eeNseGW3dbyuPiWxVjgSFJkiRJkkqMBYYkSZIkSSp4Fhh958q0A6hgOTbUHceFeuLYUE8cG+qO40I9cWyoOwU9LopxEU9JkiRJklRinIEhSZIkSZIKngWGJEmSJEkqeNm0A/Q3cRyfCPweyABXJ0nyjzu9XgncAMwG1gM/S5JkeV/nVN+K43gs+X/34UAArkyS5Pc77XMkcB/wYddTdydJ8vd9mVN9L47j5cBmoBPoSJJkzk6vR+SPKT8BtgIXJUmysK9zqm/FcTwFuH2Hp/YA/mOSJP+8wz5H4jGjJMRxfA1wCrA2SZLpXc8NIz9GJgDLgThJkg3dvPeXwP/R9fD/TpLk+r7IrN2vh3HxX4BTgTbgA+DiJEk+6+a9y/mGzx4Vtx7Gxt8BlwPrunb7myRJHurmvd/4s4yKVw/j4nZgStcuQ4HPkiTZr5v3LqdAjhkWGL0ojuMM8D+A44BPgHlxHN+fJMk7O+x2KbAhSZJJcRyfC/xn4Gd9n1Z9rAP4yyRJFsZxPAhYEMfx4zuNDYDnkiQ5JYV8StdRSZI09fDaScBeXdtBwL923aofS5JkKbAffPHZshK4p5tdPWaUhuuA/06+CP/cXwNPJEnyj3Ec/3XX4/+w45u6So7/C5hDvjxf0PW95GtFh4rSdXx9XDwO/C5Jko44jv8z8Dt2Ghc7+KbPHhW36/j62AD4pyRJ/r+e3rSLP8uoeF3HTuMiSZIvfg6N4/i/Ahu/4f0FcczwFJLedSDwfpIkHH70AgAABVxJREFUy5IkaQNuA07baZ/TgM9/+3EncEzXb1jVjyVJsurz35onSbIZWAyMTjeVisRpwA1JkoQkSV4GhsZxPDLtUOpTxwAfJEnyUdpBlI4kSZ4Fmnd6esfvE9cDp3fz1hOAx5Mkae4qLR4HTtxtQdWnuhsXSZI8liRJR9fDl4ExfR5MqevhmLErduVnGRWpbxoXXT+PxsCtfRrqe7DA6F2jgY93ePwJX/8h9Yt9uj5gNgJ1fZJOBSGO4wnA/sAr3bx8SBzHb8Rx/HAcx9P6NplSEoDH4jheEMfxFd28vivHFfVv59LzFwqPGaVreJIkq7ruryZ/iuLOPH6UtkuAh3t47ds+e9Q//Xkcx2/GcXxNHMe13bzuMaN0HQ6sSZLkvR5eL5hjhgWG1IfiOK4B7gL+bZIkm3Z6eSEwPkmSfYF/Ae7t63xKxWFJkswif6rIn8VxfETagVQ44jiuAH4K3NHNyx4zBECSJIH8l0sJgDiO/5b86as397CLnz2l51+BPcmfnrgK+K/pxlGB+TnfPPuiYI4ZFhi9ayUwdofHY7qe63afOI6zwBDyi3mqn4vjuJx8eXFzkiR37/x6kiSbkiTZ0nX/IaA8juP6Po6pPpYkycqu27Xk1zg4cKddduW4ov7rJGBhkiRrdn7BY0bJW/P56WRdt2u72cfjRwmK4/gi8gv1/aKr3PqaXfjsUT+TJMmaJEk6kyTJAVfR/b+5x4wS1PUz6Zl8dfHwryikY4aLePauecBecRxPJP9/9nOB83ba537gl8BLwNnAkz19uKj/6DqvbC6wOEmS/9bDPiPIT90KcRwfSL5gtNzqx+I4rgbKkiTZ3HX/eGDnq0jcT37K523kF+/cuMO0cfV/Pf5GxGNGyfv8+8Q/dt3e180+jwL/zw5TxY8nv6ij+qmuK0j8e+DHSZJs7WGfXfnsUT8Tx/HIHb4/nAG81c1uu/KzjPqfY4ElSZJ80t2LhXbMiELwZ+feFMfxT4B/Jn/poWuSJPmHOI7/HpifJMn9cRxXATeSXwOhGTg3SZJl6SVWX4jj+DDgOWARkOt6+m+AcQBJkvwhjuM/B35NfsrnNuC3SZK8mEJc9ZE4jvfgyytLZIFbuo4Zv4IvxkVEfsXoE8lfRvXiJEnmpxJYfarrS8IKYI8kSTZ2Pbfj2PCYUSLiOL4VOBKoB9aQv7LIvUBC/nPkI/KXUW2O43gO8KskSS7reu8l5D9vAP4hSZJr+zi+dpMexsXvgEq+LDNfTpLkV3EcjyJ/Scyf9PTZ06fhtVv1MDaOJH/6SCB/6eV/kyTJqh3HRtd7v/azTF/n1+7R3bhIkmRuHMfXkT9W/GGHfQv2mGGBIUmSJEmSCp5rYEiSJEmSpIJngSFJkiRJkgqeBYYkSZIkSSp4FhiSJEmSJKngWWBIkiRJkqSCZ4EhSZIkSZIKngWGJEmSJEkqeBYYkiRJkiSp4GXTDiBJkvS5OI73BOYBxyZJsjCO41HAG8A5SZI8nWo4SZKUqiiEkHYGSZKkL8RxfDnw74A5wD3AoiRJ/irdVJIkKW2eQiJJkgpKkiRXAe8DrwAjgb9NN5EkSSoEFhiSJKkQXQVMB/4lSZLWtMNIkqT0eQqJJEkqKHEc15Bf9+Ip4CRgRpIkzemmkiRJaXMGhiRJKjS/B+YnSXIZ8CDwh5TzSJKkAmCBIUmSCkYcx6cBJwK/7nrqt8CsOI5/kV4qSZJUCDyFRJIkSZIkFTxnYEiSJEmSpIJngSFJkiRJkgqeBYYkSZIkSSp4FhiSJEmSJKngWWBIkiRJkqSCZ4EhSZIkSZIKngWGJEmSJEkqeBYYkiRJkiSp4P3/SWaRruxFRDkAAAAASUVORK5CYII=\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "plt.figure(figsize=(15,10))\n", + "\n", + "plt.plot(q[0,:],q[1,:])\n", + "plt.plot(path[0,:],path[1,:])\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.tight_layout()\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 +}