Repository: MizuhoAOKI/path_tracking_catalog Branch: master Commit: 1770cb6c6865 Files: 23 Total size: 691.0 KB Directory structure: gitextract_vv2ds5dc/ ├── .gitignore ├── .python-version ├── LICENSE.txt ├── README.md ├── media/ │ └── .gitkeep ├── notebooks/ │ ├── bangbang.ipynb │ ├── dwa_obstacle_avoidance.ipynb │ ├── dwa_pathtracking.ipynb │ ├── dynamic_bicycle_model.ipynb │ ├── fuzzy.ipynb │ ├── kinematic_bicycle_model.ipynb │ ├── lqr.ipynb │ ├── mppi_obstacle_avoidance.ipynb │ ├── mppi_pathtracking.ipynb │ ├── ovalpath.csv │ ├── pid.ipynb │ ├── preliminary/ │ │ ├── linear_quadratic_regulator_tutorial.ipynb │ │ └── state_feedback_control_tutorial.ipynb │ ├── purepursuit.ipynb │ ├── stanley.ipynb │ ├── state_feedback.ipynb │ └── unicycle_model.ipynb └── pyproject.toml ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ *.mp4 .ipynb_checkpoints __pycache__/ *.pyc # Packages *.egg !/tests/**/*.egg /*.egg-info /dist/* build _build .cache *.so # Installer logs pip-log.txt # Unit test / coverage reports .coverage .pytest_cache .DS_Store .idea/* # .python-version .vscode/* /test.py /test_*.* /setup.cfg MANIFEST.in /setup.py /docs/site/* /tests/fixtures/simple_project/setup.py /tests/fixtures/project_with_extras/setup.py .mypy_cache .venv /releases/* pip-wheel-metadata /poetry.toml poetry/core/* ================================================ FILE: .python-version ================================================ 3.10.5 ================================================ FILE: LICENSE.txt ================================================ MIT License Copyright (c) 2024 MizuhoAOKI Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: README.md ================================================ [![License: MIT](https://img.shields.io/badge/License-MIT-blue.svg)](https://opensource.org/licenses/MIT) [![Poetry](https://img.shields.io/endpoint?url=https://python-poetry.org/badge/v0.json)](https://python-poetry.org/) ![](https://geps.dev/progress/36) # Path Tracking Catalog 25 path-tracking algorithms are (goint to be) implemented with python. https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/1b2b39f6-87fc-4f3d-9b18-4ae32f71b5fd - [Vehicle Models for Simulation](#vehicle-models-for-simulation) - [x] [Dynamic Bicycle Model](#dynamic-bicycle-model) - [x] [Kinematic Bicycle Model](#kinematic-bicycle-model) - [x] [Unicycle Model](#unicycle-model) - [Control Algorithms](#control-algorithms) - [x] [Bang-Bang Control](#bang-bang-control) - [x] [PID Control](#pid-control) - [x] [Pure Pursuit Control](#pure-pursuit-control) - [x] [Stanley Control](#stanley-control) - [x] [Fuzzy Logic Control](#fuzzy-logic-control) - [ ] Genetic Algorithm - [x] [Dynamic Window Approach](#dynamic-window-approach) - [ ] State Lattice Planner - [x] [State Feedback Control](#state-feedback-control) - [x] [Linear Quadratic Regulator (infinite horizon)](#linear-quadratic-regulator) - [ ] Linear Quadratic Regulator (finite horizon) - [ ] Differential Dynamic Programming (infinite horizon) - [ ] Differential Dynamic Programming (finite horizon) - [ ] Iterative LQR (infinite horizon) - [ ] Iterative LQR (finite horizon) - [ ] Linear Model Predictive Control (formulated as Quadratic Programming) - [ ] Nonlinear Model Predictive Control (solved by C/GMRES method) - [x] [Model Predictive Path-Integral Control](#model-predictive-path-integral-control) - [ ] Sliding Mode Control - [ ] Q-Learning - [ ] Multi Layer Perceptron - [ ] Linear Quadratic Gaussian - [ ] H∞ Control (LMI) - [ ] Lyapunov - [ ] Adaptive Control ## Setup ```sh git clone https://github.com/MizuhoAOKI/path_tracking_catalog.git cd path_tracking_catalog poetry install ``` ## Vehicle Models for Simulation ### Definition of Coordinate Systems definition_of_frames ### Dynamic Bicycle Model ```math \begin{align} &\frac{\mathrm{d}}{\mathrm{d}t} \begin{bmatrix} p^G_x \\ p^G_y \\ \phi \\ v^B_x \\ v^B_y \\ \omega \\ \end{bmatrix} = \begin{bmatrix} v^B_x \cos\phi - v^B_y \sin\phi \\ v^B_x \sin\phi + v^B_y \cos\phi \\ \omega \\ {a}\cos\beta - (F_{f}^{\rm{lat}}\sin {{\delta}})/m + v^B_y \omega \\ {a}\sin\beta + F_{r}^{\rm{lat}}/m + F_{f}^{\rm{lat}} \cos{\delta}/m - v^B_x \omega \\ (F_{f}^{\rm{lat}}l_f\cos{\delta} - F_{r}^{\rm{lat}}l_r)/I_z\ \end{bmatrix}, \\ \\ & F_{f}^{\rm{lat}} = - C_f \left( \frac{v^B_y + l_f \omega}{v^B_x} - {\delta} \right), \\ \\ & F_{r}^{\rm{lat}} = - C_r \left( \frac{v^B_y - l_r \omega}{v^B_x} \right), \\ \\ & \beta = \tan^{-1} \left( \frac{v^B_y}{v^B_x} \right) \approx \frac{v^B_y}{v^B_x} \ \ \ (\because v^B_y \ll v^B_x ). \end{align} ```

DBM

https://github.com/MizuhoAOKI/path_tracking/assets/63337525/d51c3821-9b35-4e91-8235-e63b18f33f03 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/dynamic_bicycle_model.ipynb ``` ### Kinematic Bicycle Model ```math \begin{align} & \frac{\mathrm{d}}{\mathrm{d}t} \begin{bmatrix} p^G_x \\ p^G_y \\ \phi \\ V \end{bmatrix} = \begin{bmatrix} V \cos(\phi + \beta) \\ V \sin(\phi + \beta) \\ (V/l_r) \sin\beta \\ {a} \end{bmatrix},\\ \\ & \beta = \tan^{-1} \left( \frac{l_r}{l_f + l_r} \tan({\delta}) \right). \end{align} ```

KBM

https://github.com/MizuhoAOKI/path_tracking/assets/63337525/b85fe31c-3e4a-47a9-bc54-694cde225bd5 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/kinematic_bicycle_model.ipynb ``` ### Unicycle Model ```math \begin{align} \frac{\mathrm{d}}{\mathrm{d}t} \begin{bmatrix} p^G_x \\ p^G_y \\ \phi \\ V \end{bmatrix} = \begin{bmatrix} V \cos\phi \\ V \sin\phi \\ ( V / l ) \tan{\delta} \\ {a} \end{bmatrix}. \end{align} ```

UM

https://github.com/MizuhoAOKI/path_tracking/assets/63337525/8cba0010-6a21-4830-8974-b4b57c166bcf ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/unicycle_model.ipynb ``` ## Control Algorithms ### Bang-Bang Control https://github.com/MizuhoAOKI/path_tracking/assets/63337525/cc88214e-3914-4126-ac57-3f63d8397094 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/bangbang.ipynb ``` ### PID Control https://github.com/MizuhoAOKI/path_tracking/assets/63337525/83e813d9-b611-49da-abe2-45333bfb80d2 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/pid.ipynb ``` ### Pure-Pursuit Control https://github.com/MizuhoAOKI/path_tracking/assets/63337525/a23f8437-d695-4848-83fb-a8424f311683 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/purepursuit.ipynb ``` ### Stanley Control https://github.com/MizuhoAOKI/path_tracking/assets/63337525/43f3ce4f-8181-45ad-bc08-ac96f3a91e2b ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/stanley.ipynb ``` ### Fuzzy Logic Control https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/d8f9f256-4b2c-4b9e-8b57-e33f05e59e6f ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/fuzzy.ipynb ``` ### Dynamic Window Approach #### Simple Path Tracking https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/23078e50-46a5-48eb-b89b-4a2111b320f0 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/dwa_pathtracking.ipynb ``` #### Path Tracking with Obstacle Avoidance https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/7b2a80b7-8d3e-4d37-89f4-23337efcb937 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/dwa_obstacle_avoidance.ipynb ``` ### State Lattice Planner ### State Feedback Control https://github.com/MizuhoAOKI/path_tracking/assets/63337525/8e48380b-f840-49cc-91e0-07dad356b5be ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/state_feedback.ipynb ``` ### Linear Quadratic Regulator https://github.com/MizuhoAOKI/path_tracking/assets/63337525/13d6abbc-2904-422d-acba-17ede074a181 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/lqr.ipynb ``` ### Model Predictive Control ### Model Predictive Path-Integral Control #### Simple Path Tracking https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/acfefe3a-a22a-4cbc-a5c8-a83086943684 ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/mppi_pathtracking.ipynb ``` #### Path Tracking with Obstacle Avoidance https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/fe84eaf6-8795-490d-9ef1-efecc427052e ```sh cd path_tracking_catalog poetry run jupyter notebook notebooks/mppi_obstacle_avoidance.ipynb ``` ### Sliding Mode Control ### Q-Learning ================================================ FILE: media/.gitkeep ================================================ ================================================ FILE: notebooks/bangbang.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Bang-Bang Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle \n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Controller : Bang-Bang Controller" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Longitudinal Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class BangBangLongitudinalController():\n", " def __init__(\n", " self,\n", " acceleration_cmd: float = +1.0, # [m/s^2]\n", " deceleration_cmd: float = -1.0, # [m/s-2]\n", " target_velocity: float = 3.0, # [m/s]\n", " ) -> None:\n", " \"\"\"initialize bang-bang controller for keeping constant velocity\"\"\"\n", " self.acc_cmd = acceleration_cmd\n", " self.dec_cmd = deceleration_cmd\n", " self.target_vel = target_velocity\n", "\n", " def calc_control_input(self, observed_vel: float) -> None:\n", " \"\"\"calculate control input\"\"\"\n", " if observed_vel < self.target_vel:\n", " return self.acc_cmd\n", " else:\n", " return self.dec_cmd" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Keeping Speed Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 100 # [step]\n", "delta_t = 0.1 # [s]\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-3.0, 50.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, v]\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize speed controller\n", "speed_controller = BangBangLongitudinalController(\n", " acceleration_cmd = +1.0, # [m/s^2]\n", " deceleration_cmd = -1.0, # [m/s]\n", " target_velocity = 3.0, # [m/s]\n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", " current_velocity = current_state[3]\n", "\n", " # calculate control inputs\n", " steer_input = 0.0 # steering command [rad] # set zero because this is just a test run of speed controller\n", " accel_input = speed_controller.calc_control_input(observed_vel=current_velocity) # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"bangbang_speed_control_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Lateral Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class BangBangLateralController():\n", " def __init__(\n", " self,\n", " left_steering_cmd: float = 0.5, # [rad]\n", " right_steering_cmd: float = 0.5, # [rad]\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " ) -> None:\n", " \"\"\"initialize bang-bang controller for path-tracking\"\"\"\n", " # bang-bang control parameters\n", " self.left_steering_cmd = left_steering_cmd\n", " self.right_steering_cmd = right_steering_cmd\n", "\n", " # ref_path info\n", " self.ref_path = ref_path\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray) -> float:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # set vehicle state variables from observation\n", " x = observed_x[0]\n", " y = observed_x[1]\n", "\n", " # get the waypoint closest to current vehicle position \n", " _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # which side of the reference path is the car on, the right or the left?\n", " ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n", " x1, y1 = ref_x, ref_y\n", " x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0*np.sin(ref_yaw)\n", " vx, vy = x2 - x1, y2 - y1\n", " wx, wy = x - x1, y - y1\n", " s = vx * wy - vy * wx\n", "\n", " if s > 0:\n", " # vehicle is on the left\n", " return self.right_steering_cmd\n", " else:\n", " # vehicle is on the right\n", " return self.left_steering_cmd\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", "\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : Bang-Bang Controller\n", "- Lateral Control : Bang-Bang Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize bang-bang controllers for the vehicle\n", "bangbang_lon_controller = BangBangLongitudinalController(\n", " acceleration_cmd = +1.0, # [m/s^2]\n", " deceleration_cmd = -1.0, # [m/s^2]\n", " target_velocity = +5.0 # [m/s]\n", ")\n", "bangbang_lat_controller = BangBangLateralController(\n", " left_steering_cmd = +0.5, # [rad]\n", " right_steering_cmd = -0.5, # [rad]\n", " ref_path = ref_path, # ndarray, size is \n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate control inputs\n", " current_velocity = current_state[3]\n", " accel_input = bangbang_lon_controller.calc_control_input(observed_vel=current_velocity)\n", " steer_input = bangbang_lat_controller.calc_control_input(observed_x=current_state)\n", "\n", " except IndexError as ex:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"bangbang_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/dwa_obstacle_avoidance.ipynb ================================================ { "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Dynamic Window Approach" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle\n", "\n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " wheel_base: float = 2.5, # [m]\n", " vehicle_width = 3.0, # [m]\n", " vehicle_length = 4.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\n", " obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.wheel_base = wheel_base#[m]\n", " self.vehicle_w = vehicle_width\n", " self.vehicle_l = vehicle_length\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # obstacle parameters\n", " self.obstacle_circles = obstacle_circles\n", "\n", " # visualization settings\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", " self.minimap_view_x_lim_min, self.minimap_view_x_lim_max = -40.0, 40.0\n", " self.minimap_view_y_lim_min, self.minimap_view_y_lim_max = -10.0, 40.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " self.minimap_ax.set_xlim(self.minimap_view_x_lim_min, self.minimap_view_x_lim_max)\n", " self.minimap_ax.set_ylim(self.minimap_view_y_lim_min, self.minimap_view_y_lim_max)\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\n", " sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, optimal_traj: np.ndarray, sampled_traj_list: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw the predicted optimal trajectory from mppi\n", " if optimal_traj.any():\n", " optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\n", " optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\n", " frame += self.main_ax.plot(optimal_traj_x_offset, optimal_traj_y_offset, color='#005aff', linestyle=\"solid\", linewidth=1.5, zorder=5)\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " # draw the sampled trajectories from mppi\n", " if sampled_traj_list.any():\n", " min_alpha_value = 0.25\n", " max_alpha_value = 0.35\n", " for idx, sampled_traj in enumerate(sampled_traj_list):\n", " # draw darker for better samples\n", " alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\n", " sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\n", " sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\n", " frame += self.main_ax.plot(sampled_traj_x_offset, sampled_traj_y_offset, color='gray', linestyle=\"solid\", linewidth=0.2, zorder=4, alpha=alpha_value)\n", "\n", " # draw the circular obstacles in the main view\n", " for obs in self.obstacle_circles:\n", " obs_x, obs_y, obs_r = obs\n", " obs_circle = patches.Circle([obs_x-x, obs_y-y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n", " frame += [self.main_ax.add_artist(obs_circle)]\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " # draw the circular obstacles in the mini map view\n", " for obs in self.obstacle_circles:\n", " obs_x, obs_y, obs_r = obs\n", " obs_circle = patches.Circle([obs_x, obs_y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n", " frame += [self.minimap_ax.add_artist(obs_circle)]\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Controller : Dynamic Window Approach" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class DWAControllerForPathTracking():\n", " def __init__(\n", " self,\n", " delta_t: float = 0.05,\n", " wheel_base: float = 2.5, # [m]\n", " vehicle_width: float = 3.0, # [m]\n", " vehicle_length: float = 4.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " horizon_step_T: int = 30,\n", " param_steer_resolution: float = 0.01, # [rad]\n", " param_accel_resolution: float = 0.025, # [m/s^2]\n", " param_steer_window_size: float = 0.2, # [rad]\n", " param_accel_window_size: float = 0.2, # [m/s^2]\n", " stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualize_optimal_traj = True, # if True, optimal trajectory is visualized\n", " visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\n", " obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n", " collision_safety_margin_rate: float = 1.2, # safety margin for collision check\n", " ) -> None:\n", " \"\"\"initialize dwa controller for path-tracking\"\"\"\n", " # dwa parameters\n", " self.dim_x = 4 # dimension of system state vector\n", " self.dim_u = 2 # dimension of control input vector\n", " self.T = horizon_step_T # prediction horizon\n", " self.param_steer_resolution = param_steer_resolution # [rad]\n", " self.param_accel_resolution = param_accel_resolution # [m/s^2]\n", " self.param_steer_window_size = param_steer_window_size # [rad]\n", " self.param_accel_window_size = param_accel_window_size # [m/s^2]\n", " self.stage_cost_weight = stage_cost_weight\n", " self.terminal_cost_weight = terminal_cost_weight\n", " self.visualize_optimal_traj = visualize_optimal_traj\n", " self.visualze_sampled_trajs = visualze_sampled_trajs\n", "\n", " # vehicle parameters\n", " self.delta_t = delta_t #[s]\n", " self.wheel_base = wheel_base #[m]\n", " self.vehicle_w = vehicle_width #[m]\n", " self.vehicle_l = vehicle_length #[m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.ref_path = ref_path\n", "\n", " # obstacle parameters\n", " self.obstacle_circles = obstacle_circles\n", " self.collision_safety_margin_rate = collision_safety_margin_rate\n", "\n", " # dwa variables\n", " self.u_prev = np.zeros((self.T, self.dim_u))\n", "\n", " # ref_path info\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\n", " \"\"\"calculate optimal control input\"\"\"\n", " # load privious control input sequence\n", " u = self.u_prev\n", "\n", " # set initial x value from observation\n", " x0 = observed_x\n", "\n", " # get the waypoint closest to current vehicle position \n", " self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # clarify exploration range of control input space\n", " steer_prev = u[0, 0]\n", " accel_prev = u[0, 1]\n", " steer_min = np.clip(steer_prev - self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n", " steer_max = np.clip(steer_prev + self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n", " accel_min = np.clip(accel_prev - self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n", " accel_max = np.clip(accel_prev + self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n", "\n", " # sample control input sequence\n", " dynamic_window_steer = np.linspace(steer_min, steer_max, int((steer_max-steer_min)/self.param_steer_resolution)+1)\n", " dynamic_window_accel = np.linspace(accel_min, accel_max, int((accel_max-accel_min)/self.param_accel_resolution)+1)\n", " dynamic_window = np.array(np.meshgrid(dynamic_window_steer, dynamic_window_accel)).T # size is steer_sample_size x accel_sample_size x self.udim(=2)\n", " sample_num = dynamic_window.shape[0] * dynamic_window.shape[1]\n", "\n", " # prepare buffer\n", " ## sampled control input sequence\n", " v = np.zeros((sample_num, self.T, self.dim_u)) # control input sequence with noise\n", " ## cost of each sample\n", " S = np.zeros((sample_num))\n", " ## counter of samples\n", " k = 0\n", "\n", " # loop for 0 ~ K-1 samples\n", " for i, j in np.ndindex(dynamic_window.shape[0], dynamic_window.shape[1]):\n", "\n", " # set initial(t=0) state x i.e. observed state of the vehicle\n", " x = x0\n", "\n", " # loop for time step t = 1 ~ T\n", " for t in range(1, self.T+1):\n", "\n", " # get control input, which is constant during the prediction horizon\n", " v[k, t-1] = dynamic_window[i, j]\n", "\n", " # update x\n", " x = self._F(x, self._g(v[k, t-1]))\n", "\n", " # add stage cost\n", " S[k] += self._c(x)\n", "\n", " # add terminal cost\n", " S[k] += self._phi(x)\n", "\n", " # update counter of samples\n", " k += 1\n", "\n", " # get optimal control input\n", " k_opt = np.argmin(S)\n", " u = v[k_opt]\n", "\n", " # calculate optimal trajectory\n", " optimal_traj = np.zeros((self.T, self.dim_x))\n", " if self.visualize_optimal_traj:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(u[t-1]))\n", " optimal_traj[t] = x\n", "\n", " # calculate sampled trajectories\n", " sampled_traj_list = np.zeros((k, self.T, self.dim_x))\n", " sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\n", " if self.visualze_sampled_trajs:\n", " for k in sorted_idx:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(v[k, t-1]))\n", " sampled_traj_list[k, t] = x\n", "\n", " # update privious control input sequence (shift 1 step to the left)\n", " self.u_prev[:-1] = u[1:]\n", " self.u_prev[-1] = u[-1]\n", "\n", " # return optimal control input and input sequence\n", " return u[0], u, optimal_traj, sampled_traj_list\n", "\n", " def _g(self, v: np.ndarray) -> float:\n", " \"\"\"clamp input\"\"\"\n", " # limit control inputs\n", " v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\n", " v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\n", " return v\n", "\n", " def _c(self, x_t: np.ndarray) -> float:\n", " \"\"\"calculate stage cost\"\"\"\n", " # parse x_t\n", " x, y, yaw, v = x_t\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate stage cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\n", "\n", " # add penalty for collision with obstacles\n", " stage_cost += self._is_collided(x_t) * 1.0e10\n", "\n", " return stage_cost\n", "\n", " def _phi(self, x_T: np.ndarray) -> float:\n", " \"\"\"calculate terminal cost\"\"\"\n", " # parse x_T\n", " x, y, yaw, v = x_T\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate terminal cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\n", "\n", " # add penalty for collision with obstacles\n", " terminal_cost += self._is_collided(x_T) * 1.0e10\n", "\n", " return terminal_cost\n", "\n", " def _is_collided(self, x_t: np.ndarray) -> bool:\n", " \"\"\"check if the vehicle is collided with obstacles\"\"\"\n", " # vehicle shape parameters\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " safety_margin_rate = self.collision_safety_margin_rate\n", " vw, vl = vw*safety_margin_rate, vl*safety_margin_rate\n", "\n", " # get current states\n", " x, y, yaw, _ = x_t\n", "\n", " # key points for collision check\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, 0.0, +0.5*vl, +0.5*vl, +0.5*vl, 0.0, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, +0.5*vw, 0.0, -0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", "\n", " # check if the key points are inside the obstacles\n", " for obs in self.obstacle_circles: # for each circular obstacles\n", " obs_x, obs_y, obs_r = obs # [m] x, y, radius\n", " for p in range(len(rotated_vehicle_shape_x)):\n", " if (rotated_vehicle_shape_x[p]-obs_x)**2 + (rotated_vehicle_shape_y[p]-obs_y)**2 < obs_r**2:\n", " return 1.0 # collided\n", "\n", " return 0.0 # not collided\n", "\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " \"\"\"rotate shape and return location on the x-y plane.\"\"\"\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", "\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\n", "\n", " def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\n", " \"\"\"calculate next state of the vehicle\"\"\"\n", " # get previous state variables\n", " x, y, yaw, v = x_t\n", " steer, accel = v_t\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", "\n", " # return updated state\n", " x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\n", " return x_t_plus_1\n", "\n", " def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\n", " \"\"\"apply moving average filter for smoothing input sequence\n", " Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\n", " \"\"\"\n", " b = np.ones(window_size)/window_size\n", " dim = xx.shape[1]\n", " xx_mean = np.zeros(xx.shape)\n", "\n", " for d in range(dim):\n", " xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\"same\")\n", " n_conv = math.ceil(window_size/2)\n", " xx_mean[0,d] *= window_size/n_conv\n", " for i in range(1, n_conv):\n", " xx_mean[i,d] *= window_size/(i+n_conv)\n", " xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \n", " return xx_mean\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Obstacle Avoidance\n", "- Longitudinal Control : Dynamic Window Approach\n", "- Lateral Control : Dynamic Window Approach" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 150 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# obstacle params\n", "OBSTACLE_CIRCLES = np.array([\n", " [+ 8.0, +5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n", " [+18.0, -5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n", "])\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " wheel_base=2.5,\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", " obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 0.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize a dwa controller for the vehicle\n", "dwa = DWAControllerForPathTracking(\n", " delta_t = delta_t*2.0, # [s]\n", " wheel_base = 2.5, # [m]\n", " max_steer_abs = 0.523, # [rad]\n", " max_accel_abs = 2.000, # [m/s^2]\n", " ref_path = ref_path, # ndarray, size is \n", " horizon_step_T = 20, # [steps]\n", " param_steer_resolution = 0.02, # [rad]\n", " param_accel_resolution = 0.025, # [m/s^2]\n", " param_steer_window_size = 0.5, # [rad]\n", " param_accel_window_size = 0.5, # [m/s^2]\n", " stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualze_sampled_trajs = True,\n", " obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n", " collision_safety_margin_rate = 1.2, # safety margin for collision check\n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate input force with DWA\n", " optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = dwa.calc_control_input(\n", " observed_x = current_state\n", " )\n", " except IndexError as e:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={optimal_input[0]:>+6.2f}[rad], accel={optimal_input[1]:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=optimal_input, delta_t=delta_t, vehicle_traj=vehicle_trajectory, optimal_traj=optimal_traj[:, 0:2], sampled_traj_list=sampled_traj_list[:, :, 0:2])\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "# save animation\n", "# vehicle.save_animation(\"dwa_obstacle_avoidance_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/dwa_pathtracking.ipynb ================================================ { "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Dynamic Window Approach" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle\n", "\n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " wheel_base: float = 2.5, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.wheel_base = wheel_base#[m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from dwa\n", " sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from dwa\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, optimal_traj: np.ndarray, sampled_traj_list: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw the predicted optimal trajectory from dwa\n", " if optimal_traj.any():\n", " optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\n", " optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\n", " frame += self.main_ax.plot(optimal_traj_x_offset, optimal_traj_y_offset, color='#005aff', linestyle=\"solid\", linewidth=1.5, zorder=5)\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " # draw the sampled trajectories from dwa\n", " if sampled_traj_list.any():\n", " min_alpha_value = 0.25\n", " max_alpha_value = 0.35\n", " for idx, sampled_traj in enumerate(sampled_traj_list):\n", " # draw darker for better samples\n", " alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\n", " sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\n", " sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\n", " frame += self.main_ax.plot(sampled_traj_x_offset, sampled_traj_y_offset, color='gray', linestyle=\"solid\", linewidth=0.2, zorder=4, alpha=alpha_value)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Controller : Dynamic Window Approach" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class DWAControllerForPathTracking():\n", " def __init__(\n", " self,\n", " delta_t: float = 0.05,\n", " wheel_base: float = 2.5, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " horizon_step_T: int = 30,\n", " param_steer_resolution: float = 0.01, # [rad]\n", " param_accel_resolution: float = 0.025, # [m/s^2]\n", " param_steer_window_size: float = 0.2, # [rad]\n", " param_accel_window_size: float = 0.2, # [m/s^2]\n", " stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualize_optimal_traj = True, # if True, optimal trajectory is visualized\n", " visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\n", " ) -> None:\n", " \"\"\"initialize dwa controller for path-tracking\"\"\"\n", " # dwa parameters\n", " self.dim_x = 4 # dimension of system state vector\n", " self.dim_u = 2 # dimension of control input vector\n", " self.T = horizon_step_T # prediction horizon\n", " self.param_steer_resolution = param_steer_resolution # [rad]\n", " self.param_accel_resolution = param_accel_resolution # [m/s^2]\n", " self.param_steer_window_size = param_steer_window_size # [rad]\n", " self.param_accel_window_size = param_accel_window_size # [m/s^2]\n", " self.stage_cost_weight = stage_cost_weight\n", " self.terminal_cost_weight = terminal_cost_weight\n", " self.visualize_optimal_traj = visualize_optimal_traj\n", " self.visualze_sampled_trajs = visualze_sampled_trajs\n", "\n", " # vehicle parameters\n", " self.delta_t = delta_t #[s]\n", " self.wheel_base = wheel_base#[m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.ref_path = ref_path\n", "\n", " # dwa variables\n", " self.u_prev = np.zeros((self.T, self.dim_u))\n", "\n", " # ref_path info\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\n", " \"\"\"calculate optimal control input\"\"\"\n", " # load privious control input sequence\n", " u = self.u_prev\n", "\n", " # set initial x value from observation\n", " x0 = observed_x\n", "\n", " # get the waypoint closest to current vehicle position \n", " self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # clarify exploration range of control input space\n", " steer_prev = u[0, 0]\n", " accel_prev = u[0, 1]\n", " steer_min = np.clip(steer_prev - self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n", " steer_max = np.clip(steer_prev + self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n", " accel_min = np.clip(accel_prev - self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n", " accel_max = np.clip(accel_prev + self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n", "\n", " # sample control input sequence\n", " dynamic_window_steer = np.linspace(steer_min, steer_max, int((steer_max-steer_min)/self.param_steer_resolution)+1)\n", " dynamic_window_accel = np.linspace(accel_min, accel_max, int((accel_max-accel_min)/self.param_accel_resolution)+1)\n", " dynamic_window = np.array(np.meshgrid(dynamic_window_steer, dynamic_window_accel)).T # size is steer_sample_size x accel_sample_size x self.udim(=2)\n", " sample_num = dynamic_window.shape[0] * dynamic_window.shape[1]\n", "\n", " # prepare buffer\n", " ## sampled control input sequence\n", " v = np.zeros((sample_num, self.T, self.dim_u)) # control input sequence with noise\n", " ## cost of each sample\n", " S = np.zeros((sample_num))\n", " ## counter of samples\n", " k = 0\n", "\n", " # loop for 0 ~ K-1 samples\n", " for i, j in np.ndindex(dynamic_window.shape[0], dynamic_window.shape[1]):\n", "\n", " # set initial(t=0) state x i.e. observed state of the vehicle\n", " x = x0\n", "\n", " # loop for time step t = 1 ~ T\n", " for t in range(1, self.T+1):\n", "\n", " # get control input, which is constant during the prediction horizon\n", " v[k, t-1] = dynamic_window[i, j]\n", "\n", " # update x\n", " x = self._F(x, self._g(v[k, t-1]))\n", "\n", " # add stage cost\n", " S[k] += self._c(x)\n", "\n", " # add terminal cost\n", " S[k] += self._phi(x)\n", "\n", " # update counter of samples\n", " k += 1\n", "\n", " # get optimal control input\n", " k_opt = np.argmin(S)\n", " u = v[k_opt]\n", "\n", " # calculate optimal trajectory\n", " optimal_traj = np.zeros((self.T, self.dim_x))\n", " if self.visualize_optimal_traj:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(u[t-1]))\n", " optimal_traj[t] = x\n", "\n", " # calculate sampled trajectories\n", " sampled_traj_list = np.zeros((k, self.T, self.dim_x))\n", " sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\n", " if self.visualze_sampled_trajs:\n", " for k in sorted_idx:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(v[k, t-1]))\n", " sampled_traj_list[k, t] = x\n", "\n", " # update privious control input sequence (shift 1 step to the left)\n", " self.u_prev[:-1] = u[1:]\n", " self.u_prev[-1] = u[-1]\n", "\n", " # return optimal control input and input sequence\n", " return u[0], u, optimal_traj, sampled_traj_list\n", "\n", " def _g(self, v: np.ndarray) -> float:\n", " \"\"\"clamp input\"\"\"\n", " # limit control inputs\n", " v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\n", " v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\n", " return v\n", "\n", " def _c(self, x_t: np.ndarray) -> float:\n", " \"\"\"calculate stage cost\"\"\"\n", " # parse x_t\n", " x, y, yaw, v = x_t\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate stage cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\n", " return stage_cost\n", "\n", " def _phi(self, x_T: np.ndarray) -> float:\n", " \"\"\"calculate terminal cost\"\"\"\n", " # parse x_T\n", " x, y, yaw, v = x_T\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate terminal cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\n", " return terminal_cost\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", "\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\n", "\n", " def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\n", " \"\"\"calculate next state of the vehicle\"\"\"\n", " # get previous state variables\n", " x, y, yaw, v = x_t\n", " steer, accel = v_t\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", "\n", " # return updated state\n", " x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\n", " return x_t_plus_1\n", "\n", " def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\n", " \"\"\"apply moving average filter for smoothing input sequence\n", " Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\n", " \"\"\"\n", " b = np.ones(window_size)/window_size\n", " dim = xx.shape[1]\n", " xx_mean = np.zeros(xx.shape)\n", "\n", " for d in range(dim):\n", " xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\"same\")\n", " n_conv = math.ceil(window_size/2)\n", " xx_mean[0,d] *= window_size/n_conv\n", " for i in range(1, n_conv):\n", " xx_mean[i,d] *= window_size/(i+n_conv)\n", " xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \n", " return xx_mean\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : Dynamic Window Approach\n", "- Lateral Control : Dynamic Window Approach" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " wheel_base=2.5,\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize a dwa controller for the vehicle\n", "dwa = DWAControllerForPathTracking(\n", " delta_t = delta_t*2.0, # [s]\n", " wheel_base = 2.5, # [m]\n", " max_steer_abs = 0.523, # [rad]\n", " max_accel_abs = 2.000, # [m/s^2]\n", " ref_path = ref_path, # ndarray, size is \n", " horizon_step_T = 20, # [steps]\n", " param_steer_resolution = 0.01, # [rad]\n", " param_accel_resolution = 0.025, # [m/s^2]\n", " param_steer_window_size = 0.2, # [rad]\n", " param_accel_window_size = 0.2, # [m/s^2]\n", " stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualze_sampled_trajs = True\n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate input force with DWA\n", " optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = dwa.calc_control_input(\n", " observed_x = current_state\n", " )\n", " except IndexError as e:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={optimal_input[0]:>+6.2f}[rad], accel={optimal_input[1]:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=optimal_input, delta_t=delta_t, vehicle_traj=vehicle_trajectory, optimal_traj=optimal_traj[:, 0:2], sampled_traj_list=sampled_traj_list[:, :, 0:2])\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "# save animation\n", "# vehicle.save_animation(\"dwa_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/dynamic_bicycle_model.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Dynamic Bicycle Model" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Definition of Coordinate Systems\n", "\n", "\"definition_of_frames\"\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## State Equation\n", "\n", "\"DBM\"\n", "\n", "$$\n", "\\begin{align}\n", "&\\frac{\\mathrm{d}}{\\mathrm{d}t}\n", "\\begin{bmatrix}\n", "p^G_x \\\\\n", "p^G_y \\\\\n", "\\phi \\\\\n", "v^B_x \\\\\n", "v^B_y \\\\\n", "\\omega \\\\\n", "\\end{bmatrix}\n", "=\n", "\\begin{bmatrix}\n", "v^B_x \\cos\\phi - v^B_y \\sin\\phi \\\\\n", "v^B_x \\sin\\phi + v^B_y \\cos\\phi \\\\\n", "\\omega \\\\\n", "{a}\\cos\\beta - (F_{f}^{\\rm{lat}}\\sin {{\\delta}})/m + v^B_y \\omega \\\\\n", "{a}\\sin\\beta + F_{r}^{\\rm{lat}}/m + F_{f}^{\\rm{lat}} \\cos{\\delta}/m - v^B_x \\omega \\\\\n", "(F_{f}^{\\rm{lat}}l_f\\cos{\\delta} - F_{r}^{\\rm{lat}}l_r)/I_z\\\n", "\\end{bmatrix}, \\\\ \\\\\n", "& F_{f}^{\\rm{lat}} = - C_f \\left( \\frac{v^B_y + l_f \\omega}{v^B_x} - {\\delta} \\right), \\\\ \\\\\n", "& F_{r}^{\\rm{lat}} = - C_r \\left( \\frac{v^B_y - l_r \\omega}{v^B_x} \\right), \\\\ \\\\\n", "& \\beta = \\tan^{-1} \\left( \\frac{v^B_y}{v^B_x} \\right) \\approx \\frac{v^B_y}{v^B_x} \\ \\ \\ (\\because v^B_y \\ll v^B_x ).\n", "\\end{align}\n", "$$\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.1, # [m]\n", " l_r: float = 1.4, # [m]\n", " mass: float = 1000.0, # [kg]\n", " I_z: float = 1300.0, # [kg*m^2]\n", " C_f: float = 5000.0 * 2.0, # [N/rad]\n", " C_r: float = 6000.0 * 2.0, # [N/rad]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " vx: x-axis velocity [m/s]\n", " vy: y-axis velocity [m/s]\n", " omega: angular velocity [rad/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Dynamic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.mass = mass # [kg]\n", " self.I_z = I_z # [kg*m^2]\n", " self.C_f = C_f # [N/rad]\n", " self.C_r = C_r # [N/rad]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]), # [x, y, yaw, vx, vy, omega]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, vx, vy, omega = self.state\n", "\n", " # prepare params\n", " l_f = self.l_f\n", " l_r = self.l_r\n", " m = self.mass\n", " C_f = self.C_f\n", " C_r = self.C_r\n", " I_z = self.I_z\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # calculate tire forces\n", " F_fy = - C_f * ((vy + l_f * omega) / vx - steer)\n", " F_ry = - C_r * ((vy - l_r * omega) / vx)\n", "\n", " # update state variables\n", " beta = vy / vx\n", " new_x = x + (vx * np.cos(yaw) - vy * np.sin(yaw)) * dt\n", " new_y = y + (vx * np.sin(yaw) + vy * np.cos(yaw)) * dt\n", " new_yaw = yaw + omega * dt\n", " new_vx = vx + (accel * np.cos(beta) - (F_fy * np.sin(steer) / m) + vy * omega) * dt\n", " new_vy = vy + (accel * np.sin(beta) + F_ry / m + (F_fy * np.cos(steer) / m) - vx * omega) * dt\n", " new_omega = omega + ((F_fy * l_f * np.cos(steer) - F_ry * l_r) / I_z) * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_vx, new_vy, new_omega])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, vx, vy, omega = self.state\n", " v = np.sqrt(vx**2 + vy**2) # vehicle velocity\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Run Simulation" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Zig Zag Run" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 200 # [step]\n", "delta_t = 0.05 # [s] # Note : shorter delta_t is recommended for dynamic bicycle model\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-3.0, 50.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, vx, vy, omega]\n", "vehicle_trajectory = np.array(vehicle.get_state()[:2])\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", " steer_input = 0.6 * np.sin(i/5.0) # steering command [rad]\n", " accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"dbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Steady Input" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 400 # [step]\n", "delta_t = 0.05 # [s] # Note : shorter delta_t is recommended for dynamic bicycle model\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-25.0, 25.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 5.5, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, vx, vy, omega]\n", "vehicle_trajectory = np.array(vehicle.get_state()[:2])\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", " steer_input = 0.3 # steering command [rad]\n", " accel_input = 2.0 # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"dbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": ".venv", "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.12.0" }, "orig_nbformat": 4 }, "nbformat": 4, "nbformat_minor": 2 } ================================================ FILE: notebooks/fuzzy.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Fuzzy Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle \n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Longitudinal Controller : Fuzzy Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class FuzzyLongitudinalController():\n", " def __init__(\n", " self,\n", " slow_thresh_rate: float = +0.6, # [%]\n", " fast_thresh_rate: float = +1.4, # [%]\n", " target_velocity: float = 3.0, # [m/s]\n", " ) -> None:\n", " \"\"\"initialize fuzzy controller for keeping constant velocity\"\"\"\n", " # fuzzy control parameters\n", " self.target_vel = target_velocity\n", " self.slow_thresh_vel = slow_thresh_rate * target_velocity\n", " self.fast_thresh_vel = fast_thresh_rate * target_velocity\n", " self.acc_when_vel_is_slow = +2.0 # [m/s^2]\n", " self.acc_when_vel_is_mid = 0.0 # [m/s^2]\n", " self.acc_when_vel_is_fast = -2.0 # [m/s^2]\n", "\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n", " \"\"\"calculate control input\"\"\"\n", " # calculate tracking error, its integral and derivative\n", " v = observed_vel\n", "\n", " # calculate control input with fuzzy control\n", " acc_cmd = self.defuzzify(v)\n", "\n", " return acc_cmd\n", "\n", " def defuzzify(self, vel: float) -> float:\n", " \"\"\"defuzzigy control input\"\"\"\n", " w_slow = 1.0\n", " w_mid = 1.0\n", " w_fast = 1.0\n", " acc_cmd = self.acc_when_vel_is_slow * w_slow * self.speed_slow(vel) \\\n", " + self.acc_when_vel_is_mid * w_mid * self.speed_mid(vel) \\\n", " + self.acc_when_vel_is_fast * w_fast * self.speed_fast(vel)\n", "\n", " return acc_cmd\n", "\n", " def speed_slow(self, vel: float):\n", " \"\"\"fuzzy membership function for slow speed\"\"\"\n", " if vel < self.slow_thresh_vel:\n", " return 1.0\n", " elif vel < self.fast_thresh_vel:\n", " return (self.fast_thresh_vel - vel) / (self.fast_thresh_vel - self.slow_thresh_vel)\n", " else:\n", " return 0.0\n", "\n", " def speed_mid(self, vel: float):\n", " \"\"\"fuzzy membership function for middle speed\"\"\"\n", " if vel < self.slow_thresh_vel:\n", " return 0.0\n", " elif vel < self.target_vel:\n", " return (vel - self.slow_thresh_vel) / (self.target_vel - self.slow_thresh_vel)\n", " elif vel < self.fast_thresh_vel:\n", " return (self.fast_thresh_vel - vel) / (self.fast_thresh_vel - self.target_vel)\n", " else:\n", " return 0.0\n", "\n", " def speed_fast(self, vel: float):\n", " \"\"\"fuzzy membership function for fast speed\"\"\"\n", " if vel < self.slow_thresh_vel:\n", " return 0.0\n", " elif vel < self.fast_thresh_vel:\n", " return (vel - self.slow_thresh_vel) / (self.fast_thresh_vel - self.slow_thresh_vel)\n", " else:\n", " return 1.0\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Lateral Controller : Fuzzy Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class FuzzyLateralController():\n", " def __init__(\n", " self,\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " ) -> None:\n", " \"\"\"initialize fuzzy controller for path-tracking\"\"\"\n", " # fuzzy control parameters\n", " self.y_e_n_thresh = -1.0 # [m]\n", " self.y_e_z_thresh = 0.0 # [m]\n", " self.y_e_p_thresh = +1.0 # [m]\n", " self.steer_when_y_e_is_n = +0.523 # [rad]\n", " self.steer_when_y_e_is_z = 0.0 # [rad]\n", " self.steer_when_y_e_is_p = -0.523 # [rad]\n", "\n", " self.theta_e_n_thresh = -0.523/7.0 # [rad]\n", " self.theta_e_z_thresh = 0.0 # [rad]\n", " self.theta_e_p_thresh = +0.523/7.0 # [rad]\n", " self.steer_when_theta_e_is_n = +0.523 # [rad]\n", " self.steer_when_theta_e_is_z = 0.0 # [rad]\n", " self.steer_when_theta_e_is_p = -0.523 # [rad]\n", "\n", " # ref_path info\n", " self.ref_path = ref_path\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # set vehicle state variables from observation\n", " x = observed_x[0]\n", " y = observed_x[1]\n", " yaw = observed_x[2]\n", " v = observed_x[3]\n", "\n", " # get the waypoint closest to current vehicle position\n", " _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # which side of the reference path is the car on, the right or the left?\n", " ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n", " x1, y1 = ref_x, ref_y\n", " x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0 * np.sin(ref_yaw)\n", " vx, vy = x2 - x1, y2 - y1\n", " wx, wy = x - x1, y - y1\n", " s = vx * wy - vy * wx # s>0 : vehicle is on the left of the path, s<0 : vehicle is on the left of the path,\n", "\n", " # get tracking error\n", " y_e = np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # lateral error \n", " theta_e = yaw - ref_yaw # heading error\n", " theta_e = np.arctan2(np.sin(theta_e), np.cos(theta_e)) # normalize heading error to [-pi, pi]\n", "\n", " # calculate control input\n", " steer_cmd = self.defuzzify(y_e, theta_e)\n", " return steer_cmd\n", "\n", " def defuzzify(self, y_e: float, theta_e: float) -> float:\n", " \"\"\"defuzzigy control input\"\"\"\n", " w_p = 0.6\n", " w_z = 0.6\n", " w_n = 0.6\n", "\n", " steer_cmd_y_e = self.steer_when_y_e_is_p * w_p * self.y_e_p(y_e) \\\n", " + self.steer_when_y_e_is_z * w_z * self.y_e_z(y_e) \\\n", " + self.steer_when_y_e_is_n * w_n * self.y_e_n(y_e)\n", "\n", " steer_cmd_theta_e = self.steer_when_theta_e_is_p * (1 - w_p) * self.theta_e_p(theta_e) \\\n", " + self.steer_when_theta_e_is_z * (1 - w_z) * self.theta_e_z(theta_e) \\\n", " + self.steer_when_theta_e_is_n * (1 - w_n) * self.theta_e_n(theta_e)\n", "\n", " steer_cmd = steer_cmd_y_e + steer_cmd_theta_e\n", " return steer_cmd\n", "\n", " def y_e_n(self, y_e: float):\n", " \"\"\"fuzzy membership function for y_e negative\"\"\"\n", " if y_e < self.y_e_n_thresh:\n", " return 1.0\n", " elif y_e < self.y_e_z_thresh:\n", " return (self.y_e_z_thresh - y_e) / (self.y_e_z_thresh - self.y_e_n_thresh)\n", " else:\n", " return 0.0\n", "\n", " def y_e_z(self, y_e: float):\n", " \"\"\"fuzzy membership function for y_e zero\"\"\"\n", " if y_e < self.y_e_n_thresh:\n", " return 0.0\n", " elif y_e < self.y_e_z_thresh:\n", " return (y_e - self.y_e_n_thresh) / (self.y_e_z_thresh - self.y_e_n_thresh)\n", " elif y_e < self.y_e_p_thresh:\n", " return (self.y_e_p_thresh - y_e) / (self.y_e_p_thresh - self.y_e_z_thresh)\n", " else:\n", " return 0.0\n", "\n", " def y_e_p(self, y_e: float):\n", " \"\"\"fuzzy membership function for y_e positive\"\"\"\n", " if y_e < self.y_e_z_thresh:\n", " return 0.0\n", " elif y_e < self.y_e_p_thresh:\n", " return (y_e - self.y_e_z_thresh) / (self.y_e_p_thresh - self.y_e_z_thresh)\n", " else:\n", " return 1.0\n", "\n", " def theta_e_n(self, theta_e: float):\n", " \"\"\"fuzzy membership function for theta_e negative\"\"\"\n", " if theta_e < self.theta_e_n_thresh:\n", " return 1.0\n", " elif theta_e < self.theta_e_z_thresh:\n", " return (self.theta_e_z_thresh - theta_e) / (self.theta_e_z_thresh - self.theta_e_n_thresh)\n", " else:\n", " return 0.0\n", "\n", " def theta_e_z(self, theta_e: float):\n", " \"\"\"fuzzy membership function for theta_e zero\"\"\"\n", " if theta_e < self.theta_e_n_thresh:\n", " return 0.0\n", " elif theta_e < self.theta_e_z_thresh:\n", " return (theta_e - self.theta_e_n_thresh) / (self.theta_e_z_thresh - self.theta_e_n_thresh)\n", " elif theta_e < self.theta_e_p_thresh:\n", " return (self.theta_e_p_thresh - theta_e) / (self.theta_e_p_thresh - self.theta_e_z_thresh)\n", " else:\n", " return 0.0\n", "\n", " def theta_e_p(self, theta_e: float):\n", " \"\"\"fuzzy membership function for theta_e positive\"\"\"\n", " if theta_e < self.theta_e_z_thresh:\n", " return 0.0\n", " elif theta_e < self.theta_e_p_thresh:\n", " return (theta_e - self.theta_e_z_thresh) / (self.theta_e_p_thresh - self.theta_e_z_thresh)\n", " else:\n", " return 1.0\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : Fuzzy Controller\n", "- Lateral Control : Fuzzy Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize fuzzy controller for acceleration control\n", "fuzzy_lon_controller = FuzzyLongitudinalController(\n", " target_velocity = +5.0 # [m/s]\n", ")\n", "\n", "# initialize fuzzy controller for steering control\n", "fuzzy_lat_controller = FuzzyLateralController(\n", " ref_path = ref_path, # ndarray, size is \n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate control inputs\n", " current_velocity = current_state[3]\n", " accel_input = fuzzy_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\n", " steer_input = fuzzy_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\n", "\n", " except IndexError as ex:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"fuzzy_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/kinematic_bicycle_model.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Kinematic Bicycle Model" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Definition of Coordinate Systems\n", "\n", "\"definition_of_frames\"\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## State Equation\n", "\n", "\"KBM\"\n", "\n", "$$\n", "\\begin{align}\n", "& \\frac{\\mathrm{d}}{\\mathrm{d}t}\n", "\\begin{bmatrix}\n", "p^G_x \\\\\n", "p^G_y \\\\\n", "\\phi \\\\\n", "V\n", "\\end{bmatrix}\n", "=\n", "\\begin{bmatrix}\n", "V \\cos(\\phi + \\beta) \\\\\n", "V \\sin(\\phi + \\beta) \\\\\n", "(V/l_r) \\sin\\beta \\\\\n", "{a}\n", "\\end{bmatrix},\\\\ \\\\\n", "& \\beta = \\tan^{-1} \\left( \\frac{l_r}{l_f + l_r} \\tan({\\delta}) \\right).\n", "\\end{align}\n", "$$\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Run Simulation" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Zig Zag Run" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 100 # [step]\n", "delta_t = 0.1 # [s]\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-3.0, 50.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, v]\n", "vehicle_trajectory = np.array(vehicle.get_state()[:2])\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", " steer_input = 0.6 * np.sin(i/5.0) # steering command [rad]\n", " accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"kbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Steady Input" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 200 # [step]\n", "delta_t = 0.1 # [s]\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-25.0, 25.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 5.5])) # set initial state of the vehicle, [x, y, yaw, v]\n", "vehicle_trajectory = np.array(vehicle.get_state()[:2])\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", " steer_input = 0.3 # steering command [rad]\n", " accel_input = 2.0 # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"kbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": ".venv", "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.10.5" }, "orig_nbformat": 4 }, "nbformat": 4, "nbformat_minor": 2 } ================================================ FILE: notebooks/lqr.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Linear Quadratic Regulator (LQR)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle \n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Longitudinal Controller : PID Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class PIDLongitudinalController():\n", " def __init__(\n", " self,\n", " p_gain: float = +1.2, # P Gain\n", " i_gain: float = +0.4, # I Gain\n", " d_gain: float = +0.1, # D Gain\n", " target_velocity: float = 3.0, # [m/s]\n", " ) -> None:\n", " \"\"\"initialize pid controller for keeping constant velocity\"\"\"\n", " # pid control parameters\n", " self.K_p = p_gain\n", " self.K_i = i_gain\n", " self.K_d = d_gain\n", " self.target_vel = target_velocity\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # calculate tracking error, its integral and derivative\n", " r = self.target_vel\n", " y = observed_vel\n", " e = r - y # tracking error to the traget velocity\n", " ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\n", " de = (e - self.pre_e) / delta_t # derivative of the tracking error\n", "\n", " # calculate control input\n", " acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\n", "\n", " # update previous tracking error\n", " self.pre_e = e\n", "\n", " return acc_cmd\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Lateral Controller : Linear Quadratic Regulator (LQR)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class LQRLateralController():\n", " def __init__(\n", " self,\n", " wheel_base: float = 2.5, # [m] wheel_base\n", " Q: np.ndarray = np.diag([1.0, 1.0]), # weight matrix for state variables\n", " R: np.ndarray = np.diag([1.0]), # weight matrix for control inputs\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " ) -> None:\n", " \"\"\"initialize lqr controller for path-tracking\"\"\"\n", " # vehicle parameters\n", " self.l = wheel_base # [m] wheel base\n", "\n", " # weight matrices for LQR\n", " self.Q = Q # weight matrix for state variables\n", " self.R = R # weight matrix for control inputs\n", "\n", " # ref_path info\n", " self.ref_path = ref_path\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # set vehicle state variables from observation\n", " x = observed_x[0]\n", " y = observed_x[1]\n", " yaw = observed_x[2]\n", " v = observed_x[3]\n", "\n", " # get the waypoint closest to current vehicle position\n", " _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # which side of the reference path is the car on, the right or the left?\n", " ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n", " x1, y1 = ref_x, ref_y\n", " x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0 * np.sin(ref_yaw)\n", " vx, vy = x2 - x1, y2 - y1\n", " wx, wy = x - x1, y - y1\n", " s = vx * wy - vy * wx # s>0 : vehicle is on the left of the path, s<0 : vehicle is on the left of the path,\n", "\n", " # get tracking error\n", " y_e = np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # lateral error \n", " theta_e = yaw - ref_yaw # heading error\n", " theta_e = np.arctan2(np.sin(theta_e), np.cos(theta_e)) # normalize heading error to [-pi, pi]\n", "\n", " # define A, B matrices and solve algebraic riccati equation to get feedback gain matrix f for LQR\n", " delta_ref = 0.0 # [rad] reference steering angle (assuming the reference path is straight line)\n", " A = np.array([\n", " [0, v],\n", " [0, 0],\n", " ])\n", " B = np.array([\n", " [0],\n", " [v / (self.l * (np.cos(delta_ref))**2)],\n", " ])\n", "\n", " # calculate control input\n", " P = self.solve_are(A, B, self.Q, self.R)\n", " f = np.linalg.inv(self.R) @ B.T @ P\n", " steer_cmd = -f @ np.array([y_e, theta_e])\n", "\n", " return steer_cmd[0].real # TODO : why does steer_cmd have imaginary part?\n", "\n", " def solve_are(self, A, B, Q, R):\n", " \"\"\"solve algebraic riccati equation with the Arimoto-Potter algorithm\n", " Ref: https://qiita.com/trgkpc/items/8210927d5b035912a153\n", " \"\"\"\n", " # define hamiltonian matrix\n", " H = np.block([[A, -B @ np.linalg.inv(R) @ B.T],\n", " [-Q , -A.T]])\n", "\n", " # solve eigenvalue problem\n", " eigenvalue, w = np.linalg.eig(H)\n", "\n", " # define Y and Z, which are used to calculate P\n", " Y_, Z_ = [], []\n", " n = len(w[0])//2\n", "\n", " # sort eigenvalues\n", " index_array = sorted([i for i in range(2*n)],\n", " key = lambda x:eigenvalue[x].real)\n", "\n", " # choose n eigenvalues which have smaller real part\n", " for i in index_array[:n]:\n", " Y_.append(w.T[i][:n])\n", " Z_.append(w.T[i][n:])\n", " Y = np.array(Y_).T\n", " Z = np.array(Z_).T\n", "\n", " # calculate P\n", " if np.linalg.det(Y) != 0:\n", " return Z @ np.linalg.inv(Y)\n", " else:\n", " print(\"Warning: Y is not regular matrix. Result may be wrong!\") # TODO : need to consider mathmatical meaning of this case.\n", " return Z @ np.linalg.pinv(Y)\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : PID Controller\n", "- Lateral Control : LQR Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize pid controller for acceleration control\n", "pid_lon_controller = PIDLongitudinalController(\n", " p_gain = 1.2,\n", " i_gain = 0.4,\n", " d_gain = 0.1,\n", " target_velocity = +5.0 # [m/s]\n", ")\n", "\n", "# initialize lqr controller for steering control\n", "lqr_lat_controller = LQRLateralController(\n", " Q = np.diag([10.0, 30.0]), # weight matrix for state variables\n", " R = np.diag([6.0]), # weight matrix for control inputs\n", " ref_path = ref_path, # ndarray, size is \n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate control inputs\n", " current_velocity = current_state[3]\n", " accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\n", " steer_input = lqr_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\n", "\n", " except IndexError as ex:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(steer_input)\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " print(\"steer_input\", steer_input)\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"lqr_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/mppi_obstacle_avoidance.ipynb ================================================ { "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# MPPI (Model Predictive Path-Integral) Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle\n", "\n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " wheel_base: float = 2.5, # [m]\n", " vehicle_width = 3.0, # [m]\n", " vehicle_length = 4.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\n", " obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.wheel_base = wheel_base#[m]\n", " self.vehicle_w = vehicle_width\n", " self.vehicle_l = vehicle_length\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # obstacle parameters\n", " self.obstacle_circles = obstacle_circles\n", "\n", " # visualization settings\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", " self.minimap_view_x_lim_min, self.minimap_view_x_lim_max = -40.0, 40.0\n", " self.minimap_view_y_lim_min, self.minimap_view_y_lim_max = -10.0, 40.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " self.minimap_ax.set_xlim(self.minimap_view_x_lim_min, self.minimap_view_x_lim_max)\n", " self.minimap_ax.set_ylim(self.minimap_view_y_lim_min, self.minimap_view_y_lim_max)\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\n", " sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, optimal_traj: np.ndarray, sampled_traj_list: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw the predicted optimal trajectory from mppi\n", " if optimal_traj.any():\n", " optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\n", " optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\n", " frame += self.main_ax.plot(optimal_traj_x_offset, optimal_traj_y_offset, color='#005aff', linestyle=\"solid\", linewidth=1.5, zorder=5)\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " # draw the sampled trajectories from mppi\n", " if sampled_traj_list.any():\n", " min_alpha_value = 0.25\n", " max_alpha_value = 0.35\n", " for idx, sampled_traj in enumerate(sampled_traj_list):\n", " # draw darker for better samples\n", " alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\n", " sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\n", " sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\n", " frame += self.main_ax.plot(sampled_traj_x_offset, sampled_traj_y_offset, color='gray', linestyle=\"solid\", linewidth=0.2, zorder=4, alpha=alpha_value)\n", "\n", " # draw the circular obstacles in the main view\n", " for obs in self.obstacle_circles:\n", " obs_x, obs_y, obs_r = obs\n", " obs_circle = patches.Circle([obs_x-x, obs_y-y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n", " frame += [self.main_ax.add_artist(obs_circle)]\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " # draw the circular obstacles in the mini map view\n", " for obs in self.obstacle_circles:\n", " obs_x, obs_y, obs_r = obs\n", " obs_circle = patches.Circle([obs_x, obs_y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n", " frame += [self.minimap_ax.add_artist(obs_circle)]\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Controller : MPPI Controller\n", "\n", "### Note\n", "The following MPPI implementation follows Algorithms 1 and 2 of the reference paper. \n", "\n", "### Reference\n", "1. G. Williams et al. \"Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving\" \n", " - URL : https://ieeexplore.ieee.org/document/8558663\n", " - PDF : https://arxiv.org/pdf/1707.02342.pdf\n", "\n", "### Brief overview of MPPI algorithm\n", "Here is a general process flow to calculate optimal input with mppi algorithm.\n", "\n", "**[Step 1]** ramdomly sample input sequence\n", "\n", "Mean input sequence $U$ and ramdomly sampled input sequence $V$ are defined as follows. \n", "Usually, optimal input sequence on the previous step is used as $U$. \n", "$$\n", " \\begin{align}\n", " & (\\mathbf{u}_0, \\mathbf{u}_1, ... \\mathbf{u}_{T-1}) = U \\in \\mathbb{R}^{m \\times T}, \\nonumber \\\\\n", " & (\\mathbf{v}_0, \\mathbf{v}_1, ... \\mathbf{v}_{T-1}) = V \\in \\mathbb{R}^{m \\times T}, \\nonumber \\\\\n", " & \\mathbf{v}_t = \\mathbf{u}_t + \\epsilon_t, \\nonumber \\\\\n", " & \\epsilon_t \\sim \\mathcal{N}(0, \\Sigma).\\nonumber \n", " \\end{align}\n", "$$\n", "\n", "\n", "**[Step 2]** predict future states and evaluate cost for each sample\n", "\n", "We assume a discrete time, continuous state-action dynamical system as a control target. \n", "$\\mathbf{x}$ is a system state, and $\\mathbf{v}$ is a sampled control input.\n", "$$\n", "\\begin{align}\n", "\\mathbf{x}_t &\\in \\mathbb{R}^{n}, \\nonumber \\\\\n", "\\mathbf{x}_{t+1} &= \\mathbf{F}(\\mathbf{x}_t, \\mathbf{v}_t).\\nonumber \n", "\\end{align}\n", "$$\n", "\n", "Then costs (i.e. penalties to be minimized) for sampled sequences $S(V; \\mathbf{x}_0)$ can be evaluated with following formulations.\n", "$$\n", " \\begin{align}\n", " & S(V; \\mathbf{x}_0) = C(\\mathcal{H}(V; \\mathbf{x}_0)), \\nonumber \\\\\n", " & C(\\mathbf{x}_0, \\mathbf{x}_1, ... \\mathbf{x}_T) = \\phi(\\mathbf{x}_T) + \\sum_{t=0}^{T-1}c(\\mathbf{x}_t), \\nonumber \\\\\n", " & \\mathcal{H}(V; \\mathbf{x}_0) = \\left( \\mathbf{x}_0, \\mathbf{F}(\\mathbf{x}_0, \\mathbf{v}_0), \\mathbf{F}(\\mathbf{F}(\\mathbf{x}_0, \\mathbf{v}_0), \\mathbf{v}_1), ... \\right).\\nonumber \n", " \\end{align}\n", "$$\n", "\n", "**[Step 3]** calculate weight for each sample sequence\n", "\n", "Weight for a each sample sequence is derived on the basis of information theory. \n", "There are K sample sequences in total, represented with an index k. \n", "Good control sequence with small cost value get more weight, and vice versa. \n", "\n", "$$\n", "\\begin{align}\n", "& w(V) = \\frac{1}{\\eta} \\exp\n", "\\left( \n", " -\\frac{1}{\\lambda}\n", " \\left(\n", " S(V) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t) - \\rho\n", " \\right)\n", "\\right) \\nonumber \\\\\n", "& \\eta = \n", "\\sum_{k=1}^K \\exp\n", "\\left( \n", " -\\frac{1}{\\lambda}\n", " \\left(\n", " S(U + \\mathcal{E}_k) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t^k) - \\rho\n", " \\right)\n", "\\right)\\nonumber \\\\\n", "& \\rho = \n", "\\min_k \n", "\\left( S(V_k) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t^k) \\right)\\nonumber\n", "\\end{align}\n", "$$\n", "\n", "Note that $\\rho$ is inserted into the formulation to avoid overflow errors during implementation.\n", "\n", "**[Step 4]** get optimal control input sequence\n", "\n", "Finally, optimal input trajectory for the next ($i+1$) step is given adding weighted sample sequences to the previous solution.\n", "\n", "$$\n", "\\begin{align}\n", " \\mathbf{u}_t^{i+1} % &= \\mathbb{E}_{\\mathbb{Q}_{\\hat{U}, \\Sigma}}[w(V)\\mathbf{v}_t]\n", " = u_t^i + \\sum_{k=1}^K w(V_k) \\epsilon_t^k \\nonumber \n", "\\end{align}\n", "$$\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class MPPIControllerForPathTracking():\n", " def __init__(\n", " self,\n", " delta_t: float = 0.05,\n", " wheel_base: float = 2.5, # [m]\n", " vehicle_width: float = 3.0, # [m]\n", " vehicle_length: float = 4.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " horizon_step_T: int = 30,\n", " number_of_samples_K: int = 1000,\n", " param_exploration: float = 0.0,\n", " param_lambda: float = 50.0,\n", " param_alpha: float = 1.0,\n", " sigma: np.ndarray = np.array([[0.5, 0.0], [0.0, 0.1]]), \n", " stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualize_optimal_traj = True, # if True, optimal trajectory is visualized\n", " visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\n", " obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n", " collision_safety_margin_rate: float = 1.2, # safety margin for collision check\n", " ) -> None:\n", " \"\"\"initialize mppi controller for path-tracking\"\"\"\n", " # mppi parameters\n", " self.dim_x = 4 # dimension of system state vector\n", " self.dim_u = 2 # dimension of control input vector\n", " self.T = horizon_step_T # prediction horizon\n", " self.K = number_of_samples_K # number of sample trajectories\n", " self.param_exploration = param_exploration # constant parameter of mppi\n", " self.param_lambda = param_lambda # constant parameter of mppi\n", " self.param_alpha = param_alpha # constant parameter of mppi\n", " self.param_gamma = self.param_lambda * (1.0 - (self.param_alpha)) # constant parameter of mppi\n", " self.Sigma = sigma # deviation of noise\n", " self.stage_cost_weight = stage_cost_weight\n", " self.terminal_cost_weight = terminal_cost_weight\n", " self.visualize_optimal_traj = visualize_optimal_traj\n", " self.visualze_sampled_trajs = visualze_sampled_trajs\n", "\n", " # vehicle parameters\n", " self.delta_t = delta_t #[s]\n", " self.wheel_base = wheel_base #[m]\n", " self.vehicle_w = vehicle_width #[m]\n", " self.vehicle_l = vehicle_length #[m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.ref_path = ref_path\n", "\n", " # obstacle parameters\n", " self.obstacle_circles = obstacle_circles\n", " self.collision_safety_margin_rate = collision_safety_margin_rate\n", "\n", " # mppi variables\n", " self.u_prev = np.zeros((self.T, self.dim_u))\n", "\n", " # ref_path info\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\n", " \"\"\"calculate optimal control input\"\"\"\n", " # load privious control input sequence\n", " u = self.u_prev\n", "\n", " # set initial x value from observation\n", " x0 = observed_x\n", "\n", " # get the waypoint closest to current vehicle position \n", " self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # prepare buffer\n", " S = np.zeros((self.K)) # state cost list\n", "\n", " # sample noise\n", " epsilon = self._calc_epsilon(self.Sigma, self.K, self.T, self.dim_u) # size is self.K x self.T\n", "\n", " # prepare buffer of sampled control input sequence\n", " v = np.zeros((self.K, self.T, self.dim_u)) # control input sequence with noise\n", "\n", " # loop for 0 ~ K-1 samples\n", " for k in range(self.K): \n", "\n", " # set initial(t=0) state x i.e. observed state of the vehicle\n", " x = x0\n", "\n", " # loop for time step t = 1 ~ T\n", " for t in range(1, self.T+1):\n", "\n", " # get control input with noise\n", " if k < (1.0-self.param_exploration)*self.K:\n", " v[k, t-1] = u[t-1] + epsilon[k, t-1] # sampling for exploitation\n", " else:\n", " v[k, t-1] = epsilon[k, t-1] # sampling for exploration\n", "\n", " # update x\n", " x = self._F(x, self._g(v[k, t-1]))\n", "\n", " # add stage cost\n", " S[k] += self._c(x) + self.param_gamma * u[t-1].T @ np.linalg.inv(self.Sigma) @ v[k, t-1]\n", "\n", " # add terminal cost\n", " S[k] += self._phi(x)\n", "\n", " # compute information theoretic weights for each sample\n", " w = self._compute_weights(S)\n", "\n", " # calculate w_k * epsilon_k\n", " w_epsilon = np.zeros((self.T, self.dim_u))\n", " for t in range(self.T): # loop for time step t = 0 ~ T-1\n", " for k in range(self.K):\n", " w_epsilon[t] += w[k] * epsilon[k, t]\n", "\n", " # apply moving average filter for smoothing input sequence\n", " w_epsilon = self._moving_average_filter(xx=w_epsilon, window_size=10)\n", "\n", " # update control input sequence\n", " u += w_epsilon\n", "\n", " # calculate optimal trajectory\n", " optimal_traj = np.zeros((self.T, self.dim_x))\n", " if self.visualize_optimal_traj:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(u[t-1]))\n", " optimal_traj[t] = x\n", "\n", " # calculate sampled trajectories\n", " sampled_traj_list = np.zeros((self.K, self.T, self.dim_x))\n", " sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\n", " if self.visualze_sampled_trajs:\n", " for k in sorted_idx:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(v[k, t-1]))\n", " sampled_traj_list[k, t] = x\n", "\n", " # update privious control input sequence (shift 1 step to the left)\n", " self.u_prev[:-1] = u[1:]\n", " self.u_prev[-1] = u[-1]\n", "\n", " # return optimal control input and input sequence\n", " return u[0], u, optimal_traj, sampled_traj_list\n", "\n", " def _calc_epsilon(self, sigma: np.ndarray, size_sample: int, size_time_step: int, size_dim_u: int) -> np.ndarray:\n", " \"\"\"sample epsilon\"\"\"\n", " # check if sigma row size == sigma col size == size_dim_u and size_dim_u > 0\n", " if sigma.shape[0] != sigma.shape[1] or sigma.shape[0] != size_dim_u or size_dim_u < 1:\n", " print(\"[ERROR] sigma must be a square matrix with the size of size_dim_u.\")\n", " raise ValueError\n", "\n", " # sample epsilon\n", " mu = np.zeros((size_dim_u)) # set average as a zero vector\n", " epsilon = np.random.multivariate_normal(mu, sigma, (size_sample, size_time_step))\n", " return epsilon\n", "\n", " def _g(self, v: np.ndarray) -> float:\n", " \"\"\"clamp input\"\"\"\n", " # limit control inputs\n", " v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\n", " v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\n", " return v\n", "\n", " def _c(self, x_t: np.ndarray) -> float:\n", " \"\"\"calculate stage cost\"\"\"\n", " # parse x_t\n", " x, y, yaw, v = x_t\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate stage cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\n", " \n", " # add penalty for collision with obstacles\n", " stage_cost += self._is_collided(x_t) * 1.0e10\n", "\n", " return stage_cost\n", "\n", " def _phi(self, x_T: np.ndarray) -> float:\n", " \"\"\"calculate terminal cost\"\"\"\n", " # parse x_T\n", " x, y, yaw, v = x_T\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate terminal cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\n", " \n", " # add penalty for collision with obstacles\n", " terminal_cost += self._is_collided(x_T) * 1.0e10\n", " \n", " return terminal_cost\n", "\n", " def _is_collided(self, x_t: np.ndarray) -> bool:\n", " \"\"\"check if the vehicle is collided with obstacles\"\"\"\n", " # vehicle shape parameters\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " safety_margin_rate = self.collision_safety_margin_rate\n", " vw, vl = vw*safety_margin_rate, vl*safety_margin_rate\n", "\n", " # get current states\n", " x, y, yaw, _ = x_t\n", "\n", " # key points for collision check\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, 0.0, +0.5*vl, +0.5*vl, +0.5*vl, 0.0, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, +0.5*vw, 0.0, -0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", "\n", " # check if the key points are inside the obstacles\n", " for obs in self.obstacle_circles: # for each circular obstacles\n", " obs_x, obs_y, obs_r = obs # [m] x, y, radius\n", " for p in range(len(rotated_vehicle_shape_x)):\n", " if (rotated_vehicle_shape_x[p]-obs_x)**2 + (rotated_vehicle_shape_y[p]-obs_y)**2 < obs_r**2:\n", " return 1.0 # collided\n", "\n", " return 0.0 # not collided\n", "\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " \"\"\"rotate shape and return location on the x-y plane.\"\"\"\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", "\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\n", "\n", " def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\n", " \"\"\"calculate next state of the vehicle\"\"\"\n", " # get previous state variables\n", " x, y, yaw, v = x_t\n", " steer, accel = v_t\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", "\n", " # return updated state\n", " x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\n", " return x_t_plus_1\n", "\n", " def _compute_weights(self, S: np.ndarray) -> np.ndarray:\n", " \"\"\"compute weights for each sample\"\"\"\n", " # prepare buffer\n", " w = np.zeros((self.K))\n", "\n", " # calculate rho\n", " rho = S.min()\n", "\n", " # calculate eta\n", " eta = 0.0\n", " for k in range(self.K):\n", " eta += np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\n", "\n", " # calculate weight\n", " for k in range(self.K):\n", " w[k] = (1.0 / eta) * np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\n", " return w\n", "\n", " def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\n", " \"\"\"apply moving average filter for smoothing input sequence\n", " Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\n", " \"\"\"\n", " b = np.ones(window_size)/window_size\n", " dim = xx.shape[1]\n", " xx_mean = np.zeros(xx.shape)\n", "\n", " for d in range(dim):\n", " xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\"same\")\n", " n_conv = math.ceil(window_size/2)\n", " xx_mean[0,d] *= window_size/n_conv\n", " for i in range(1, n_conv):\n", " xx_mean[i,d] *= window_size/(i+n_conv)\n", " xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \n", " return xx_mean\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Obstacle Avoidance\n", "- Longitudinal Control : MPPI Controller\n", "- Lateral Control : MPPI Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 150 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# obstacle params\n", "OBSTACLE_CIRCLES = np.array([\n", " [+ 8.0, +5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n", " [+18.0, -5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n", "])\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " wheel_base=2.5,\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", " obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 0.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize a mppi controller for the vehicle\n", "mppi = MPPIControllerForPathTracking(\n", " delta_t = delta_t*2.0, # [s]\n", " wheel_base = 2.5, # [m]\n", " max_steer_abs = 0.523, # [rad]\n", " max_accel_abs = 2.000, # [m/s^2]\n", " ref_path = ref_path, # ndarray, size is \n", " horizon_step_T = 20, # [steps]\n", " number_of_samples_K = 500, # [samples]\n", " param_exploration = 0.05,\n", " param_lambda = 100.0,\n", " param_alpha = 0.98,\n", " sigma = np.array([[0.075, 0.0], [0.0, 2.0]]),\n", " stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualze_sampled_trajs = True, # if True, sampled trajectories are visualized\n", " obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n", " collision_safety_margin_rate = 1.2, # safety margin for collision check\n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate input force with MPPI\n", " optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = mppi.calc_control_input(\n", " observed_x = current_state\n", " )\n", " except IndexError as e:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={optimal_input[0]:>+6.2f}[rad], accel={optimal_input[1]:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=optimal_input, delta_t=delta_t, vehicle_traj=vehicle_trajectory, optimal_traj=optimal_traj[:, 0:2], sampled_traj_list=sampled_traj_list[:, :, 0:2])\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "# save animation\n", "# vehicle.save_animation(\"mppi_obstacle_avoidance_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/mppi_pathtracking.ipynb ================================================ { "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# MPPI (Model Predictive Path-Integral) Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle\n", "\n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " wheel_base: float = 2.5, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.wheel_base = wheel_base#[m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\n", " sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, optimal_traj: np.ndarray, sampled_traj_list: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw the predicted optimal trajectory from mppi\n", " if optimal_traj.any():\n", " optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\n", " optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\n", " frame += self.main_ax.plot(optimal_traj_x_offset, optimal_traj_y_offset, color='#005aff', linestyle=\"solid\", linewidth=1.5, zorder=5)\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " # draw the sampled trajectories from mppi\n", " if sampled_traj_list.any():\n", " min_alpha_value = 0.25\n", " max_alpha_value = 0.35\n", " for idx, sampled_traj in enumerate(sampled_traj_list):\n", " # draw darker for better samples\n", " alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\n", " sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\n", " sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\n", " frame += self.main_ax.plot(sampled_traj_x_offset, sampled_traj_y_offset, color='gray', linestyle=\"solid\", linewidth=0.2, zorder=4, alpha=alpha_value)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Controller : MPPI Controller\n", "\n", "### Note\n", "The following MPPI implementation follows Algorithms 1 and 2 of the reference paper. \n", "\n", "### Reference\n", "1. G. Williams et al. \"Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving\" \n", " - URL : https://ieeexplore.ieee.org/document/8558663\n", " - PDF : https://arxiv.org/pdf/1707.02342.pdf\n", "\n", "### Brief overview of MPPI algorithm\n", "Here is a general process flow to calculate optimal input with mppi algorithm.\n", "\n", "**[Step 1]** ramdomly sample input sequence\n", "\n", "Mean input sequence $U$ and ramdomly sampled input sequence $V$ are defined as follows. \n", "Usually, optimal input sequence on the previous step is used as $U$. \n", "$$\n", " \\begin{align}\n", " & (\\mathbf{u}_0, \\mathbf{u}_1, ... \\mathbf{u}_{T-1}) = U \\in \\mathbb{R}^{m \\times T}, \\nonumber \\\\\n", " & (\\mathbf{v}_0, \\mathbf{v}_1, ... \\mathbf{v}_{T-1}) = V \\in \\mathbb{R}^{m \\times T}, \\nonumber \\\\\n", " & \\mathbf{v}_t = \\mathbf{u}_t + \\epsilon_t, \\nonumber \\\\\n", " & \\epsilon_t \\sim \\mathcal{N}(0, \\Sigma).\\nonumber \n", " \\end{align}\n", "$$\n", "\n", "\n", "**[Step 2]** predict future states and evaluate cost for each sample\n", "\n", "We assume a discrete time, continuous state-action dynamical system as a control target. \n", "$\\mathbf{x}$ is a system state, and $\\mathbf{v}$ is a sampled control input.\n", "$$\n", "\\begin{align}\n", "\\mathbf{x}_t &\\in \\mathbb{R}^{n}, \\nonumber \\\\\n", "\\mathbf{x}_{t+1} &= \\mathbf{F}(\\mathbf{x}_t, \\mathbf{v}_t).\\nonumber \n", "\\end{align}\n", "$$\n", "\n", "Then costs (i.e. penalties to be minimized) for sampled sequences $S(V; \\mathbf{x}_0)$ can be evaluated with following formulations.\n", "$$\n", " \\begin{align}\n", " & S(V; \\mathbf{x}_0) = C(\\mathcal{H}(V; \\mathbf{x}_0)), \\nonumber \\\\\n", " & C(\\mathbf{x}_0, \\mathbf{x}_1, ... \\mathbf{x}_T) = \\phi(\\mathbf{x}_T) + \\sum_{t=0}^{T-1}c(\\mathbf{x}_t), \\nonumber \\\\\n", " & \\mathcal{H}(V; \\mathbf{x}_0) = \\left( \\mathbf{x}_0, \\mathbf{F}(\\mathbf{x}_0, \\mathbf{v}_0), \\mathbf{F}(\\mathbf{F}(\\mathbf{x}_0, \\mathbf{v}_0), \\mathbf{v}_1), ... \\right).\\nonumber \n", " \\end{align}\n", "$$\n", "\n", "**[Step 3]** calculate weight for each sample sequence\n", "\n", "Weight for a each sample sequence is derived on the basis of information theory. \n", "There are K sample sequences in total, represented with an index k. \n", "Good control sequence with small cost value get more weight, and vice versa. \n", "\n", "$$\n", "\\begin{align}\n", "& w(V) = \\frac{1}{\\eta} \\exp\n", "\\left( \n", " -\\frac{1}{\\lambda}\n", " \\left(\n", " S(V) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t) - \\rho\n", " \\right)\n", "\\right) \\nonumber \\\\\n", "& \\eta = \n", "\\sum_{k=1}^K \\exp\n", "\\left( \n", " -\\frac{1}{\\lambda}\n", " \\left(\n", " S(U + \\mathcal{E}_k) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t^k) - \\rho\n", " \\right)\n", "\\right)\\nonumber \\\\\n", "& \\rho = \n", "\\min_k \n", "\\left( S(V_k) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t^k) \\right)\\nonumber\n", "\\end{align}\n", "$$\n", "\n", "Note that $\\rho$ is inserted into the formulation to avoid overflow errors during implementation.\n", "\n", "**[Step 4]** get optimal control input sequence\n", "\n", "Finally, optimal input trajectory for the next ($i+1$) step is given adding weighted sample sequences to the previous solution.\n", "\n", "$$\n", "\\begin{align}\n", " \\mathbf{u}_t^{i+1} % &= \\mathbb{E}_{\\mathbb{Q}_{\\hat{U}, \\Sigma}}[w(V)\\mathbf{v}_t]\n", " = u_t^i + \\sum_{k=1}^K w(V_k) \\epsilon_t^k \\nonumber \n", "\\end{align}\n", "$$\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class MPPIControllerForPathTracking():\n", " def __init__(\n", " self,\n", " delta_t: float = 0.05,\n", " wheel_base: float = 2.5, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " horizon_step_T: int = 30,\n", " number_of_samples_K: int = 1000,\n", " param_exploration: float = 0.0,\n", " param_lambda: float = 50.0,\n", " param_alpha: float = 1.0,\n", " sigma: np.ndarray = np.array([[0.5, 0.0], [0.0, 0.1]]), \n", " stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualize_optimal_traj = True, # if True, optimal trajectory is visualized\n", " visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\n", " ) -> None:\n", " \"\"\"initialize mppi controller for path-tracking\"\"\"\n", " # mppi parameters\n", " self.dim_x = 4 # dimension of system state vector\n", " self.dim_u = 2 # dimension of control input vector\n", " self.T = horizon_step_T # prediction horizon\n", " self.K = number_of_samples_K # number of sample trajectories\n", " self.param_exploration = param_exploration # constant parameter of mppi\n", " self.param_lambda = param_lambda # constant parameter of mppi\n", " self.param_alpha = param_alpha # constant parameter of mppi\n", " self.param_gamma = self.param_lambda * (1.0 - (self.param_alpha)) # constant parameter of mppi\n", " self.Sigma = sigma # deviation of noise\n", " self.stage_cost_weight = stage_cost_weight\n", " self.terminal_cost_weight = terminal_cost_weight\n", " self.visualize_optimal_traj = visualize_optimal_traj\n", " self.visualze_sampled_trajs = visualze_sampled_trajs\n", "\n", " # vehicle parameters\n", " self.delta_t = delta_t #[s]\n", " self.wheel_base = wheel_base#[m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.ref_path = ref_path\n", "\n", " # mppi variables\n", " self.u_prev = np.zeros((self.T, self.dim_u))\n", "\n", " # ref_path info\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\n", " \"\"\"calculate optimal control input\"\"\"\n", " # load privious control input sequence\n", " u = self.u_prev\n", "\n", " # set initial x value from observation\n", " x0 = observed_x\n", "\n", " # get the waypoint closest to current vehicle position \n", " self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # prepare buffer\n", " S = np.zeros((self.K)) # state cost list\n", "\n", " # sample noise\n", " epsilon = self._calc_epsilon(self.Sigma, self.K, self.T, self.dim_u) # size is self.K x self.T\n", "\n", " # prepare buffer of sampled control input sequence\n", " v = np.zeros((self.K, self.T, self.dim_u)) # control input sequence with noise\n", "\n", " # loop for 0 ~ K-1 samples\n", " for k in range(self.K): \n", "\n", " # set initial(t=0) state x i.e. observed state of the vehicle\n", " x = x0\n", "\n", " # loop for time step t = 1 ~ T\n", " for t in range(1, self.T+1):\n", "\n", " # get control input with noise\n", " if k < (1.0-self.param_exploration)*self.K:\n", " v[k, t-1] = u[t-1] + epsilon[k, t-1] # sampling for exploitation\n", " else:\n", " v[k, t-1] = epsilon[k, t-1] # sampling for exploration\n", "\n", " # update x\n", " x = self._F(x, self._g(v[k, t-1]))\n", "\n", " # add stage cost\n", " S[k] += self._c(x) + self.param_gamma * u[t-1].T @ np.linalg.inv(self.Sigma) @ v[k, t-1]\n", "\n", " # add terminal cost\n", " S[k] += self._phi(x)\n", "\n", " # compute information theoretic weights for each sample\n", " w = self._compute_weights(S)\n", "\n", " # calculate w_k * epsilon_k\n", " w_epsilon = np.zeros((self.T, self.dim_u))\n", " for t in range(self.T): # loop for time step t = 0 ~ T-1\n", " for k in range(self.K):\n", " w_epsilon[t] += w[k] * epsilon[k, t]\n", "\n", " # apply moving average filter for smoothing input sequence\n", " w_epsilon = self._moving_average_filter(xx=w_epsilon, window_size=10)\n", "\n", " # update control input sequence\n", " u += w_epsilon\n", "\n", " # calculate optimal trajectory\n", " optimal_traj = np.zeros((self.T, self.dim_x))\n", " if self.visualize_optimal_traj:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(u[t-1]))\n", " optimal_traj[t] = x\n", "\n", " # calculate sampled trajectories\n", " sampled_traj_list = np.zeros((self.K, self.T, self.dim_x))\n", " sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\n", " if self.visualze_sampled_trajs:\n", " for k in sorted_idx:\n", " x = x0\n", " for t in range(self.T):\n", " x = self._F(x, self._g(v[k, t-1]))\n", " sampled_traj_list[k, t] = x\n", "\n", " # update privious control input sequence (shift 1 step to the left)\n", " self.u_prev[:-1] = u[1:]\n", " self.u_prev[-1] = u[-1]\n", "\n", " # return optimal control input and input sequence\n", " return u[0], u, optimal_traj, sampled_traj_list\n", "\n", " def _calc_epsilon(self, sigma: np.ndarray, size_sample: int, size_time_step: int, size_dim_u: int) -> np.ndarray:\n", " \"\"\"sample epsilon\"\"\"\n", " # check if sigma row size == sigma col size == size_dim_u and size_dim_u > 0\n", " if sigma.shape[0] != sigma.shape[1] or sigma.shape[0] != size_dim_u or size_dim_u < 1:\n", " print(\"[ERROR] sigma must be a square matrix with the size of size_dim_u.\")\n", " raise ValueError\n", "\n", " # sample epsilon\n", " mu = np.zeros((size_dim_u)) # set average as a zero vector\n", " epsilon = np.random.multivariate_normal(mu, sigma, (size_sample, size_time_step))\n", " return epsilon\n", "\n", " def _g(self, v: np.ndarray) -> float:\n", " \"\"\"clamp input\"\"\"\n", " # limit control inputs\n", " v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\n", " v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\n", " return v\n", "\n", " def _c(self, x_t: np.ndarray) -> float:\n", " \"\"\"calculate stage cost\"\"\"\n", " # parse x_t\n", " x, y, yaw, v = x_t\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate stage cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\n", " return stage_cost\n", "\n", " def _phi(self, x_T: np.ndarray) -> float:\n", " \"\"\"calculate terminal cost\"\"\"\n", " # parse x_T\n", " x, y, yaw, v = x_T\n", " yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n", "\n", " # calculate terminal cost\n", " _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n", " terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\n", " self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\n", " return terminal_cost\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", "\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\n", "\n", " def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\n", " \"\"\"calculate next state of the vehicle\"\"\"\n", " # get previous state variables\n", " x, y, yaw, v = x_t\n", " steer, accel = v_t\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t\n", "\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", "\n", " # return updated state\n", " x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\n", " return x_t_plus_1\n", "\n", " def _compute_weights(self, S: np.ndarray) -> np.ndarray:\n", " \"\"\"compute weights for each sample\"\"\"\n", " # prepare buffer\n", " w = np.zeros((self.K))\n", "\n", " # calculate rho\n", " rho = S.min()\n", "\n", " # calculate eta\n", " eta = 0.0\n", " for k in range(self.K):\n", " eta += np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\n", "\n", " # calculate weight\n", " for k in range(self.K):\n", " w[k] = (1.0 / eta) * np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\n", " return w\n", "\n", " def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\n", " \"\"\"apply moving average filter for smoothing input sequence\n", " Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\n", " \"\"\"\n", " b = np.ones(window_size)/window_size\n", " dim = xx.shape[1]\n", " xx_mean = np.zeros(xx.shape)\n", "\n", " for d in range(dim):\n", " xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\"same\")\n", " n_conv = math.ceil(window_size/2)\n", " xx_mean[0,d] *= window_size/n_conv\n", " for i in range(1, n_conv):\n", " xx_mean[i,d] *= window_size/(i+n_conv)\n", " xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \n", " return xx_mean\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : MPPI Controller\n", "- Lateral Control : MPPI Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " wheel_base=2.5,\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize a mppi controller for the vehicle\n", "mppi = MPPIControllerForPathTracking(\n", " delta_t = delta_t*2.0, # [s]\n", " wheel_base = 2.5, # [m]\n", " max_steer_abs = 0.523, # [rad]\n", " max_accel_abs = 2.000, # [m/s^2]\n", " ref_path = ref_path, # ndarray, size is \n", " horizon_step_T = 20, # [steps]\n", " number_of_samples_K = 500, # [samples]\n", " param_exploration = 0.0,\n", " param_lambda = 100.0,\n", " param_alpha = 0.98,\n", " sigma = np.array([[0.075, 0.0], [0.0, 2.0]]),\n", " stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n", " visualze_sampled_trajs = True\n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate input force with MPPI\n", " optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = mppi.calc_control_input(\n", " observed_x = current_state\n", " )\n", " except IndexError as e:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={optimal_input[0]:>+6.2f}[rad], accel={optimal_input[1]:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=optimal_input, delta_t=delta_t, vehicle_traj=vehicle_trajectory, optimal_traj=optimal_traj[:, 0:2], sampled_traj_list=sampled_traj_list[:, :, 0:2])\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "# save animation\n", "# vehicle.save_animation(\"mppi_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/ovalpath.csv ================================================ x,y,yaw,ref_v 0,0,0,5.0 0.1,0,0,5.0 0.2,0,0,5.0 0.3,0,0,5.0 0.4,0,0,5.0 0.5,0,0,5.0 0.6,0,0,5.0 0.7,0,0,5.0 0.8,0,0,5.0 0.9,0,0,5.0 1,0,0,5.0 1.1,0,0,5.0 1.2,0,0,5.0 1.3,0,0,5.0 1.4,0,0,5.0 1.5,0,0,5.0 1.6,0,0,5.0 1.7,0,0,5.0 1.8,0,0,5.0 1.9,0,0,5.0 2,0,0,5.0 2.1,0,0,5.0 2.2,0,0,5.0 2.3,0,0,5.0 2.4,0,0,5.0 2.5,0,0,5.0 2.6,0,0,5.0 2.7,0,0,5.0 2.8,0,0,5.0 2.9,0,0,5.0 3,0,0,5.0 3.1,0,0,5.0 3.2,0,0,5.0 3.3,0,0,5.0 3.4,0,0,5.0 3.5,0,0,5.0 3.6,0,0,5.0 3.7,0,0,5.0 3.8,0,0,5.0 3.9,0,0,5.0 4,0,0,5.0 4.1,0,0,5.0 4.2,0,0,5.0 4.3,0,0,5.0 4.4,0,0,5.0 4.5,0,0,5.0 4.6,0,0,5.0 4.7,0,0,5.0 4.8,0,0,5.0 4.9,0,0,5.0 5,0,0,5.0 5.1,0,0,5.0 5.2,0,0,5.0 5.3,0,0,5.0 5.4,0,0,5.0 5.5,0,0,5.0 5.6,0,0,5.0 5.7,0,0,5.0 5.8,0,0,5.0 5.9,0,0,5.0 6,0,0,5.0 6.1,0,0,5.0 6.2,0,0,5.0 6.3,0,0,5.0 6.4,0,0,5.0 6.5,0,0,5.0 6.6,0,0,5.0 6.7,0,0,5.0 6.8,0,0,5.0 6.9,0,0,5.0 7,0,0,5.0 7.1,0,0,5.0 7.2,0,0,5.0 7.3,0,0,5.0 7.4,0,0,5.0 7.5,0,0,5.0 7.6,0,0,5.0 7.7,0,0,5.0 7.8,0,0,5.0 7.9,0,0,5.0 8,0,0,5.0 8.1,0,0,5.0 8.2,0,0,5.0 8.3,0,0,5.0 8.4,0,0,5.0 8.5,0,0,5.0 8.6,0,0,5.0 8.7,0,0,5.0 8.8,0,0,5.0 8.9,0,0,5.0 9,0,0,5.0 9.1,0,0,5.0 9.2,0,0,5.0 9.3,0,0,5.0 9.4,0,0,5.0 9.5,0,0,5.0 9.6,0,0,5.0 9.7,0,0,5.0 9.8,0,0,5.0 9.9,0,0,5.0 10,0,0,5.0 10.1,0,0,5.0 10.2,0,0,5.0 10.3,0,0,5.0 10.4,0,0,5.0 10.5,0,0,5.0 10.6,0,0,5.0 10.7,0,0,5.0 10.8,0,0,5.0 10.9,0,0,5.0 11,0,0,5.0 11.1,0,0,5.0 11.2,0,0,5.0 11.3,0,0,5.0 11.4,0,0,5.0 11.5,0,0,5.0 11.6,0,0,5.0 11.7,0,0,5.0 11.8,0,0,5.0 11.9,0,0,5.0 12,0,0,5.0 12.1,0,0,5.0 12.2,0,0,5.0 12.3,0,0,5.0 12.4,0,0,5.0 12.5,0,0,5.0 12.6,0,0,5.0 12.7,0,0,5.0 12.8,0,0,5.0 12.9,0,0,5.0 13,0,0,5.0 13.1,0,0,5.0 13.2,0,0,5.0 13.3,0,0,5.0 13.4,0,0,5.0 13.5,0,0,5.0 13.6,0,0,5.0 13.7,0,0,5.0 13.8,0,0,5.0 13.9,0,0,5.0 14,0,0,5.0 14.1,0,0,5.0 14.2,0,0,5.0 14.3,0,0,5.0 14.4,0,0,5.0 14.5,0,0,5.0 14.6,0,0,5.0 14.7,0,0,5.0 14.8,0,0,5.0 14.9,0,0,5.0 15,0,0,5.0 15.15707676,0.00082246,0.005235991,5.0 15.3141363,0.003289748,0.015707961,5.0 15.47116139,0.007401595,0.026179941,5.0 15.62813481,0.013157549,0.036651914,5.0 15.78503934,0.020556979,0.047123891,5.0 15.94185779,0.029599074,0.057595866,5.0 16.09857296,0.040282842,0.068067838,5.0 16.25516765,0.052607111,0.078539814,5.0 16.4116247,0.066570531,0.089011794,5.0 16.56792695,0.082171569,0.099483764,5.0 16.72405726,0.099408516,0.109955745,5.0 16.8799985,0.11827948,0.120427721,5.0 17.03573359,0.138782393,0.13089969,5.0 17.19124543,0.160915006,0.141371673,5.0 17.34651698,0.184674891,0.151843639,5.0 17.5015312,0.210059444,0.162315627,5.0 17.6562711,0.23706588,0.172787597,5.0 17.81071972,0.265691239,0.183259567,5.0 17.96486011,0.295932381,0.193731546,5.0 18.11867536,0.327785989,0.204203529,5.0 18.27214862,0.361248571,0.214675497,5.0 18.42526305,0.396316457,0.225147475,5.0 18.57800186,0.432985801,0.235619449,5.0 18.73034831,0.471252583,0.246091417,5.0 18.88228568,0.511112606,0.256563401,5.0 19.03379731,0.552561498,0.267035378,5.0 19.18486659,0.595594715,0.277507355,5.0 19.33547695,0.640207537,0.287979332,5.0 19.48561188,0.686395072,0.2984513,5.0 19.63525492,0.734152256,0.308923266,5.0 19.78438964,0.78347385,0.319395259,5.0 19.9329997,0.834354446,0.329867228,5.0 20.0810688,0.886788466,0.340339214,5.0 20.22858071,0.940770158,0.350811172,5.0 20.37551924,0.996293603,0.361283163,5.0 20.52186829,1.053352712,0.371755123,5.0 20.6676118,1.111941228,0.38222711,5.0 20.8127338,1.172052726,0.392699066,5.0 20.95721836,1.233680615,0.403171067,5.0 21.10104965,1.296818135,0.41364302,5.0 21.24421188,1.361458364,0.424115029,5.0 21.38668937,1.427594213,0.434586984,5.0 21.52846649,1.495218429,0.445058952,5.0 21.66952769,1.564323596,0.455530924,5.0 21.8098575,1.634902137,0.466002907,5.0 21.94944053,1.706946312,0.476474891,5.0 22.08826147,1.780448219,0.486946877,5.0 22.22630511,1.855399799,0.49741883,5.0 22.3635563,1.931792833,0.507890822,5.0 22.5,2.009618943,0.518362774,5.0 22.63562124,2.088869595,0.528834753,5.0 22.77040514,2.169536098,0.539306755,5.0 22.90433693,2.251609605,0.549778717,5.0 23.03740192,2.335081117,0.560250696,5.0 23.16958553,2.419941481,0.570722636,5.0 23.30087324,2.506181389,0.58119465,5.0 23.43125067,2.593791386,0.591666616,5.0 23.56070352,2.682761863,0.602138582,5.0 23.68921759,2.773083064,0.612610567,5.0 23.81677878,2.864745084,0.623082576,5.0 23.94337312,2.957737872,0.63355452,5.0 24.06898672,3.05205123,0.644026491,5.0 24.1936058,3.147674814,0.654498473,5.0 24.3172167,3.24459814,0.664970444,5.0 24.43980587,3.342810578,0.675442387,5.0 24.56135985,3.442301358,0.685914397,5.0 24.68186532,3.54305957,0.696386369,5.0 24.80130906,3.645074165,0.706858365,5.0 24.91967798,3.748333956,0.717330326,5.0 25.0369591,3.852827618,0.727802278,5.0 25.15313955,3.958543693,0.738274271,5.0 25.26820659,4.065470589,0.748746268,5.0 25.38214761,4.173596579,0.759218221,5.0 25.49495011,4.282909806,0.769690199,5.0 25.60660172,4.393398282,0.780162173,5.0 25.71709019,4.505049892,0.790634181,5.0 25.82640342,4.617852392,0.801106114,5.0 25.93452941,4.731793411,0.811578101,5.0 26.04145631,4.846860455,0.822050058,5.0 26.14717238,4.963040905,0.832522079,5.0 26.25166604,5.08032202,0.842994037,5.0 26.35492583,5.19869094,0.853466006,5.0 26.45694043,5.318134684,0.863937954,5.0 26.55769864,5.438640154,0.874409967,5.0 26.65718942,5.560194134,0.88488193,5.0 26.75540186,5.682783296,0.895353898,5.0 26.85232519,5.806394195,0.905825859,5.0 26.94794877,5.931013277,0.916297882,5.0 27.04226213,6.056626876,0.926769822,5.0 27.13525492,6.183221216,0.937241796,5.0 27.22691694,6.310782415,0.947713784,5.0 27.31723814,6.439296485,0.958185765,5.0 27.40620861,6.568749332,0.96865777,5.0 27.49381861,6.699126761,0.979129692,5.0 27.58005852,6.830414475,0.98960168,5.0 27.66491888,6.962598075,1.000073677,5.0 27.74839039,7.095663068,1.010545652,5.0 27.8304639,7.229594859,1.021017597,5.0 27.91113041,7.364378764,1.03148955,5.0 27.99038106,7.5,1.041961572,5.0 28.06820717,7.636443696,1.05243354,5.0 28.1446002,7.773694888,1.062905534,5.0 28.21955178,7.911738527,1.073377493,5.0 28.29305369,8.050559473,1.083849451,5.0 28.36509786,8.190142504,1.094321467,5.0 28.4356764,8.330472312,1.10479342,5.0 28.50478157,8.471533509,1.115265377,5.0 28.57240579,8.613310627,1.125737347,5.0 28.63854164,8.755788116,1.136209334,5.0 28.70318186,8.898950354,1.146681371,5.0 28.76631939,9.04278164,1.157153238,5.0 28.82794727,9.187266203,1.16762532,5.0 28.88805877,9.332388198,1.178097236,5.0 28.94664729,9.47813171,1.188569198,5.0 29.0037064,9.624480757,1.199041191,5.0 29.05922984,9.77141929,1.2095132,5.0 29.11321153,9.918931196,1.219985158,5.0 29.16564555,10.0670003,1.230457121,5.0 29.21652615,10.21561036,1.240929074,5.0 29.26584774,10.36474508,1.251401092,5.0 29.31360493,10.51438812,1.261873025,5.0 29.35979246,10.66452305,1.272345057,5.0 29.40440529,10.81513341,1.282816946,5.0 29.4474385,10.96620269,1.293289015,5.0 29.48888739,11.11771432,1.303760961,5.0 29.52874742,11.26965169,1.314232883,5.0 29.5670142,11.42199814,1.324704922,5.0 29.60368354,11.57473695,1.335176902,5.0 29.63875143,11.72785138,1.345648827,5.0 29.67221401,11.88132464,1.356120843,5.0 29.70406762,12.03513989,1.366592785,5.0 29.73430876,12.18928028,1.377064794,5.0 29.76293412,12.3437289,1.387536754,5.0 29.78994056,12.4984688,1.398008705,5.0 29.81532511,12.65348302,1.408480719,5.0 29.83908499,12.80875457,1.418952719,5.0 29.86121761,12.96426641,1.429424609,5.0 29.88172052,13.1200015,1.439896655,5.0 29.90059148,13.27594274,1.450368631,5.0 29.91782843,13.43207305,1.460840563,5.0 29.93342947,13.5883753,1.471312551,5.0 29.94739289,13.74483235,1.481784533,5.0 29.95971716,13.90142704,1.492256507,5.0 29.97040093,14.05814221,1.502728476,5.0 29.97944302,14.21496066,1.513200493,5.0 29.98684245,14.37186519,1.523672436,5.0 29.99259841,14.52883861,1.534144374,5.0 29.99671025,14.6858637,1.544616431,5.0 29.99917754,14.84292324,1.555088353,5.0 30,15,1.565560336,5.0 29.99917754,15.15707676,1.576032318,5.0 29.99671025,15.3141363,1.5865043,5.0 29.99259841,15.47116139,1.596976223,5.0 29.98684245,15.62813481,1.607448279,5.0 29.97944302,15.78503934,1.617920218,5.0 29.97040093,15.94185779,1.628392161,5.0 29.95971716,16.09857296,1.638864177,5.0 29.94739289,16.25516765,1.649336147,5.0 29.93342947,16.4116247,1.65980812,5.0 29.91782843,16.56792695,1.670280103,5.0 29.90059148,16.72405726,1.680752091,5.0 29.88172052,16.8799985,1.691224023,5.0 29.86121761,17.03573359,1.701695998,5.0 29.83908499,17.19124543,1.712168044,5.0 29.81532511,17.34651698,1.722639934,5.0 29.78994056,17.5015312,1.733111934,5.0 29.76293412,17.6562711,1.743583949,5.0 29.73430876,17.81071972,1.7540559,5.0 29.70406762,17.96486011,1.76452786,5.0 29.67221401,18.11867536,1.774999868,5.0 29.63875143,18.27214862,1.785471811,5.0 29.60368354,18.42526305,1.795943826,5.0 29.5670142,18.57800186,1.806415751,5.0 29.52874742,18.73034831,1.816887731,5.0 29.48888739,18.88228568,1.827359771,5.0 29.4474385,19.03379731,1.837831692,5.0 29.40440529,19.18486659,1.848303639,5.0 29.35979246,19.33547695,1.858775708,5.0 29.31360493,19.48561188,1.869247596,5.0 29.26584774,19.63525492,1.879719629,5.0 29.21652615,19.78438964,1.890191562,5.0 29.16564555,19.9329997,1.900663579,5.0 29.11321153,20.0810688,1.911135541,5.0 29.05922984,20.22858071,1.921607486,5.0 29.0037064,20.37551924,1.93207946,5.0 28.94664729,20.52186829,1.942551456,5.0 28.88805877,20.6676118,1.95302346,5.0 28.82794727,20.8127338,1.963495405,5.0 28.76631939,20.95721836,1.973967341,5.0 28.70318186,21.10104965,1.984439406,5.0 28.63854164,21.24421188,1.994911304,5.0 28.57240579,21.38668937,2.005383317,5.0 28.50478157,21.52846649,2.015855301,5.0 28.4356764,21.66952769,2.026327268,5.0 28.36509786,21.8098575,2.036799228,5.0 28.29305369,21.94944053,2.04727119,5.0 28.21955178,22.08826147,2.057743221,5.0 28.1446002,22.22630511,2.068215157,5.0 28.06820717,22.3635563,2.078687126,5.0 27.99038106,22.5,2.089159101,5.0 27.91113041,22.63562124,2.099631069,5.0 27.8304639,22.77040514,2.11010312,5.0 27.74839039,22.90433693,2.12057506,5.0 27.66491888,23.03740192,2.131047012,5.0 27.58005852,23.16958553,2.141518942,5.0 27.49381861,23.30087324,2.151990988,5.0 27.40620861,23.43125067,2.162462958,5.0 27.31723814,23.56070352,2.172934872,5.0 27.22691694,23.68921759,2.183406888,5.0 27.13525492,23.81677878,2.193878903,5.0 27.04226213,23.94337312,2.204350857,5.0 26.94794877,24.06898672,2.214822828,5.0 26.85232519,24.1936058,2.22529478,5.0 26.75540186,24.3172167,2.235766791,5.0 26.65718942,24.43980587,2.246238724,5.0 26.55769864,24.56135985,2.256710724,5.0 26.45694043,24.68186532,2.267182686,5.0 26.35492583,24.80130906,2.277654716,5.0 26.25166604,24.91967798,2.288126648,5.0 26.14717238,25.0369591,2.298598595,5.0 26.04145631,25.15313955,2.309070575,5.0 25.93452941,25.26820659,2.319542613,5.0 25.82640342,25.38214761,2.330014548,5.0 25.71709019,25.49495011,2.340486539,5.0 25.60660172,25.60660172,2.350958473,5.0 25.49495011,25.71709019,2.361430508,5.0 25.38214761,25.82640342,2.371902441,5.0 25.26820659,25.93452941,2.382374432,5.0 25.15313955,26.04145631,2.392846367,5.0 25.0369591,26.14717238,2.403318406,5.0 24.91967798,26.25166604,2.413790385,5.0 24.80130906,26.35492583,2.424262333,5.0 24.68186532,26.45694043,2.434734265,5.0 24.56135985,26.55769864,2.445206294,5.0 24.43980587,26.65718942,2.455678257,5.0 24.3172167,26.75540186,2.466150257,5.0 24.1936058,26.85232519,2.47662219,5.0 24.06898672,26.94794877,2.487094201,5.0 23.94337312,27.04226213,2.497566153,5.0 23.81677878,27.13525492,2.508038123,5.0 23.68921759,27.22691694,2.518510078,5.0 23.56070352,27.31723814,2.528982092,5.0 23.43125067,27.40620861,2.539454108,5.0 23.30087324,27.49381861,2.549926022,5.0 23.16958553,27.58005852,2.560397993,5.0 23.03740192,27.66491888,2.570870039,5.0 22.90433693,27.74839039,2.581341968,5.0 22.77040514,27.8304639,2.59181392,5.0 22.63562124,27.91113041,2.60228586,5.0 22.5,27.99038106,2.612757912,5.0 22.3635563,28.06820717,2.623229879,5.0 22.22630511,28.1446002,2.633701854,5.0 22.08826147,28.21955178,2.644173823,5.0 21.94944053,28.29305369,2.65464576,5.0 21.8098575,28.36509786,2.66511779,5.0 21.66952769,28.4356764,2.675589752,5.0 21.52846649,28.50478157,2.686061712,5.0 21.38668937,28.57240579,2.696533679,5.0 21.24421188,28.63854164,2.707005664,5.0 21.10104965,28.70318186,2.717477677,5.0 20.95721836,28.76631939,2.727949575,5.0 20.8127338,28.82794727,2.73842164,5.0 20.6676118,28.88805877,2.748893575,5.0 20.52186829,28.94664729,2.75936552,5.0 20.37551924,29.0037064,2.769837525,5.0 20.22858071,29.05922984,2.78030952,5.0 20.0810688,29.11321153,2.790781494,5.0 19.9329997,29.16564555,2.801253439,5.0 19.78438964,29.21652615,2.811725401,5.0 19.63525492,29.26584774,2.822197419,5.0 19.48561188,29.31360493,2.832669351,5.0 19.33547695,29.35979246,2.843141384,5.0 19.18486659,29.40440529,2.853613273,5.0 19.03379731,29.4474385,2.864085342,5.0 18.88228568,29.48888739,2.874557288,5.0 18.73034831,29.52874742,2.885029209,5.0 18.57800186,29.5670142,2.895501249,5.0 18.42526305,29.60368354,2.905973229,5.0 18.27214862,29.63875143,2.916445154,5.0 18.11867536,29.67221401,2.926917169,5.0 17.96486011,29.70406762,2.937389112,5.0 17.81071972,29.73430876,2.94786112,5.0 17.6562711,29.76293412,2.95833308,5.0 17.5015312,29.78994056,2.968805031,5.0 17.34651698,29.81532511,2.979277046,5.0 17.19124543,29.83908499,2.989749046,5.0 17.03573359,29.86121761,3.000220936,5.0 16.8799985,29.88172052,3.010692982,5.0 16.72405726,29.90059148,3.021164958,5.0 16.56792695,29.91782843,3.03163689,5.0 16.4116247,29.93342947,3.042108877,5.0 16.25516765,29.94739289,3.05258086,5.0 16.09857296,29.95971716,3.063052833,5.0 15.94185779,29.97040093,3.073524803,5.0 15.78503934,29.97944302,3.08399682,5.0 15.62813481,29.98684245,3.094468763,5.0 15.47116139,29.99259841,3.104940701,5.0 15.3141363,29.99671025,3.115412757,5.0 15.15707676,29.99917754,3.12588468,5.0 15,30,3.136356663,5.0 14.9,30,3.141592654,5.0 14.8,30,3.141592654,5.0 14.7,30,3.141592654,5.0 14.6,30,3.141592654,5.0 14.5,30,3.141592654,5.0 14.4,30,3.141592654,5.0 14.3,30,3.141592654,5.0 14.2,30,3.141592654,5.0 14.1,30,3.141592654,5.0 14,30,3.141592654,5.0 13.9,30,3.141592654,5.0 13.8,30,3.141592654,5.0 13.7,30,3.141592654,5.0 13.6,30,3.141592654,5.0 13.5,30,3.141592654,5.0 13.4,30,3.141592654,5.0 13.3,30,3.141592654,5.0 13.2,30,3.141592654,5.0 13.1,30,3.141592654,5.0 13,30,3.141592654,5.0 12.9,30,3.141592654,5.0 12.8,30,3.141592654,5.0 12.7,30,3.141592654,5.0 12.6,30,3.141592654,5.0 12.5,30,3.141592654,5.0 12.4,30,3.141592654,5.0 12.3,30,3.141592654,5.0 12.2,30,3.141592654,5.0 12.1,30,3.141592654,5.0 12,30,3.141592654,5.0 11.9,30,3.141592654,5.0 11.8,30,3.141592654,5.0 11.7,30,3.141592654,5.0 11.6,30,3.141592654,5.0 11.5,30,3.141592654,5.0 11.4,30,3.141592654,5.0 11.3,30,3.141592654,5.0 11.2,30,3.141592654,5.0 11.1,30,3.141592654,5.0 11,30,3.141592654,5.0 10.9,30,3.141592654,5.0 10.8,30,3.141592654,5.0 10.7,30,3.141592654,5.0 10.6,30,3.141592654,5.0 10.5,30,3.141592654,5.0 10.4,30,3.141592654,5.0 10.3,30,3.141592654,5.0 10.2,30,3.141592654,5.0 10.1,30,3.141592654,5.0 10,30,3.141592654,5.0 9.9,30,3.141592654,5.0 9.8,30,3.141592654,5.0 9.7,30,3.141592654,5.0 9.6,30,3.141592654,5.0 9.5,30,3.141592654,5.0 9.4,30,3.141592654,5.0 9.3,30,3.141592654,5.0 9.2,30,3.141592654,5.0 9.1,30,3.141592654,5.0 9,30,3.141592654,5.0 8.9,30,3.141592654,5.0 8.8,30,3.141592654,5.0 8.7,30,3.141592654,5.0 8.6,30,3.141592654,5.0 8.5,30,3.141592654,5.0 8.4,30,3.141592654,5.0 8.3,30,3.141592654,5.0 8.2,30,3.141592654,5.0 8.1,30,3.141592654,5.0 8,30,3.141592654,5.0 7.9,30,3.141592654,5.0 7.8,30,3.141592654,5.0 7.7,30,3.141592654,5.0 7.6,30,3.141592654,5.0 7.5,30,3.141592654,5.0 7.4,30,3.141592654,5.0 7.3,30,3.141592654,5.0 7.2,30,3.141592654,5.0 7.1,30,3.141592654,5.0 7,30,3.141592654,5.0 6.9,30,3.141592654,5.0 6.8,30,3.141592654,5.0 6.7,30,3.141592654,5.0 6.6,30,3.141592654,5.0 6.5,30,3.141592654,5.0 6.4,30,3.141592654,5.0 6.3,30,3.141592654,5.0 6.2,30,3.141592654,5.0 6.1,30,3.141592654,5.0 6,30,3.141592654,5.0 5.9,30,3.141592654,5.0 5.8,30,3.141592654,5.0 5.7,30,3.141592654,5.0 5.6,30,3.141592654,5.0 5.5,30,3.141592654,5.0 5.4,30,3.141592654,5.0 5.3,30,3.141592654,5.0 5.2,30,3.141592654,5.0 5.1,30,3.141592654,5.0 5,30,3.141592654,5.0 4.9,30,3.141592654,5.0 4.8,30,3.141592654,5.0 4.7,30,3.141592654,5.0 4.6,30,3.141592654,5.0 4.5,30,3.141592654,5.0 4.4,30,3.141592654,5.0 4.3,30,3.141592654,5.0 4.2,30,3.141592654,5.0 4.1,30,3.141592654,5.0 4,30,3.141592654,5.0 3.9,30,3.141592654,5.0 3.8,30,3.141592654,5.0 3.7,30,3.141592654,5.0 3.6,30,3.141592654,5.0 3.5,30,3.141592654,5.0 3.4,30,3.141592654,5.0 3.3,30,3.141592654,5.0 3.2,30,3.141592654,5.0 3.1,30,3.141592654,5.0 3,30,3.141592654,5.0 2.9,30,3.141592654,5.0 2.8,30,3.141592654,5.0 2.7,30,3.141592654,5.0 2.6,30,3.141592654,5.0 2.5,30,3.141592654,5.0 2.4,30,3.141592654,5.0 2.3,30,3.141592654,5.0 2.2,30,3.141592654,5.0 2.1,30,3.141592654,5.0 2,30,3.141592654,5.0 1.9,30,3.141592654,5.0 1.8,30,3.141592654,5.0 1.7,30,3.141592654,5.0 1.6,30,3.141592654,5.0 1.5,30,3.141592654,5.0 1.4,30,3.141592654,5.0 1.3,30,3.141592654,5.0 1.2,30,3.141592654,5.0 1.1,30,3.141592654,5.0 1,30,3.141592654,5.0 0.9,30,3.141592654,5.0 0.8,30,3.141592654,5.0 0.7,30,3.141592654,5.0 0.6,30,3.141592654,5.0 0.5,30,3.141592654,5.0 0.4,30,3.141592654,5.0 0.3,30,3.141592654,5.0 0.2,30,3.141592654,5.0 0.1,30,3.141592654,5.0 0,30,3.141592654,5.0 -0.1,30,3.141592654,5.0 -0.2,30,3.141592654,5.0 -0.3,30,3.141592654,5.0 -0.4,30,3.141592654,5.0 -0.5,30,3.141592654,5.0 -0.6,30,3.141592654,5.0 -0.7,30,3.141592654,5.0 -0.8,30,3.141592654,5.0 -0.9,30,3.141592654,5.0 -1,30,3.141592654,5.0 -1.1,30,3.141592654,5.0 -1.2,30,3.141592654,5.0 -1.3,30,3.141592654,5.0 -1.4,30,3.141592654,5.0 -1.5,30,3.141592654,5.0 -1.6,30,3.141592654,5.0 -1.7,30,3.141592654,5.0 -1.8,30,3.141592654,5.0 -1.9,30,3.141592654,5.0 -2,30,3.141592654,5.0 -2.1,30,3.141592654,5.0 -2.2,30,3.141592654,5.0 -2.3,30,3.141592654,5.0 -2.4,30,3.141592654,5.0 -2.5,30,3.141592654,5.0 -2.6,30,3.141592654,5.0 -2.7,30,3.141592654,5.0 -2.8,30,3.141592654,5.0 -2.9,30,3.141592654,5.0 -3,30,3.141592654,5.0 -3.1,30,3.141592654,5.0 -3.2,30,3.141592654,5.0 -3.3,30,3.141592654,5.0 -3.4,30,3.141592654,5.0 -3.5,30,3.141592654,5.0 -3.6,30,3.141592654,5.0 -3.7,30,3.141592654,5.0 -3.8,30,3.141592654,5.0 -3.9,30,3.141592654,5.0 -4,30,3.141592654,5.0 -4.1,30,3.141592654,5.0 -4.2,30,3.141592654,5.0 -4.3,30,3.141592654,5.0 -4.4,30,3.141592654,5.0 -4.5,30,3.141592654,5.0 -4.6,30,3.141592654,5.0 -4.7,30,3.141592654,5.0 -4.8,30,3.141592654,5.0 -4.9,30,3.141592654,5.0 -5,30,3.141592654,5.0 -5.1,30,3.141592654,5.0 -5.2,30,3.141592654,5.0 -5.3,30,3.141592654,5.0 -5.4,30,3.141592654,5.0 -5.5,30,3.141592654,5.0 -5.6,30,3.141592654,5.0 -5.7,30,3.141592654,5.0 -5.8,30,3.141592654,5.0 -5.9,30,3.141592654,5.0 -6,30,3.141592654,5.0 -6.1,30,3.141592654,5.0 -6.2,30,3.141592654,5.0 -6.3,30,3.141592654,5.0 -6.4,30,3.141592654,5.0 -6.5,30,3.141592654,5.0 -6.6,30,3.141592654,5.0 -6.7,30,3.141592654,5.0 -6.8,30,3.141592654,5.0 -6.9,30,3.141592654,5.0 -7,30,3.141592654,5.0 -7.1,30,3.141592654,5.0 -7.2,30,3.141592654,5.0 -7.3,30,3.141592654,5.0 -7.4,30,3.141592654,5.0 -7.5,30,3.141592654,5.0 -7.6,30,3.141592654,5.0 -7.7,30,3.141592654,5.0 -7.8,30,3.141592654,5.0 -7.9,30,3.141592654,5.0 -8,30,3.141592654,5.0 -8.1,30,3.141592654,5.0 -8.2,30,3.141592654,5.0 -8.3,30,3.141592654,5.0 -8.4,30,3.141592654,5.0 -8.5,30,3.141592654,5.0 -8.6,30,3.141592654,5.0 -8.7,30,3.141592654,5.0 -8.8,30,3.141592654,5.0 -8.9,30,3.141592654,5.0 -9,30,3.141592654,5.0 -9.1,30,3.141592654,5.0 -9.2,30,3.141592654,5.0 -9.3,30,3.141592654,5.0 -9.4,30,3.141592654,5.0 -9.5,30,3.141592654,5.0 -9.6,30,3.141592654,5.0 -9.7,30,3.141592654,5.0 -9.8,30,3.141592654,5.0 -9.9,30,3.141592654,5.0 -10,30,3.141592654,5.0 -10.1,30,3.141592654,5.0 -10.2,30,3.141592654,5.0 -10.3,30,3.141592654,5.0 -10.4,30,3.141592654,5.0 -10.5,30,3.141592654,5.0 -10.6,30,3.141592654,5.0 -10.7,30,3.141592654,5.0 -10.8,30,3.141592654,5.0 -10.9,30,3.141592654,5.0 -11,30,3.141592654,5.0 -11.1,30,3.141592654,5.0 -11.2,30,3.141592654,5.0 -11.3,30,3.141592654,5.0 -11.4,30,3.141592654,5.0 -11.5,30,3.141592654,5.0 -11.6,30,3.141592654,5.0 -11.7,30,3.141592654,5.0 -11.8,30,3.141592654,5.0 -11.9,30,3.141592654,5.0 -12,30,3.141592654,5.0 -12.1,30,3.141592654,5.0 -12.2,30,3.141592654,5.0 -12.3,30,3.141592654,5.0 -12.4,30,3.141592654,5.0 -12.5,30,3.141592654,5.0 -12.6,30,3.141592654,5.0 -12.7,30,3.141592654,5.0 -12.8,30,3.141592654,5.0 -12.9,30,3.141592654,5.0 -13,30,3.141592654,5.0 -13.1,30,3.141592654,5.0 -13.2,30,3.141592654,5.0 -13.3,30,3.141592654,5.0 -13.4,30,3.141592654,5.0 -13.5,30,3.141592654,5.0 -13.6,30,3.141592654,5.0 -13.7,30,3.141592654,5.0 -13.8,30,3.141592654,5.0 -13.9,30,3.141592654,5.0 -14,30,3.141592654,5.0 -14.1,30,3.141592654,5.0 -14.2,30,3.141592654,5.0 -14.3,30,3.141592654,5.0 -14.4,30,3.141592654,5.0 -14.5,30,3.141592654,5.0 -14.6,30,3.141592654,5.0 -14.7,30,3.141592654,5.0 -14.8,30,3.141592654,5.0 -14.9,30,3.141592654,5.0 -15,30,3.141592654,5.0 -15.15707676,29.99917754,3.146828644,5.0 -15.3141363,29.99671025,3.157300627,5.0 -15.47116139,29.99259841,3.16777255,5.0 -15.62813481,29.98684245,3.178244606,5.0 -15.78503934,29.97944302,3.188716545,5.0 -15.94185779,29.97040093,3.199188488,5.0 -16.09857296,29.95971716,3.209660504,5.0 -16.25516765,29.94739289,3.220132474,5.0 -16.4116247,29.93342947,3.230604447,5.0 -16.56792695,29.91782843,3.24107643,5.0 -16.72405726,29.90059148,3.251548417,5.0 -16.8799985,29.88172052,3.26202035,5.0 -17.03573359,29.86121761,3.272492325,5.0 -17.19124543,29.83908499,3.282964371,5.0 -17.34651698,29.81532511,3.293436261,5.0 -17.5015312,29.78994056,3.303908261,5.0 -17.6562711,29.76293412,3.314380276,5.0 -17.81071972,29.73430876,3.324852227,5.0 -17.96486011,29.70406762,3.335324187,5.0 -18.11867536,29.67221401,3.345796195,5.0 -18.27214862,29.63875143,3.356268138,5.0 -18.42526305,29.60368354,3.366740153,5.0 -18.57800186,29.5670142,3.377212078,5.0 -18.73034831,29.52874742,3.387684058,5.0 -18.88228568,29.48888739,3.398156098,5.0 -19.03379731,29.4474385,3.408628019,5.0 -19.18486659,29.40440529,3.419099965,5.0 -19.33547695,29.35979246,3.429572034,5.0 -19.48561188,29.31360493,3.440043923,5.0 -19.63525492,29.26584774,3.450515956,5.0 -19.78438964,29.21652615,3.460987889,5.0 -19.9329997,29.16564555,3.471459906,5.0 -20.0810688,29.11321153,3.481931868,5.0 -20.22858071,29.05922984,3.492403813,5.0 -20.37551924,29.0037064,3.502875787,5.0 -20.52186829,28.94664729,3.513347783,5.0 -20.6676118,28.88805877,3.523819787,5.0 -20.8127338,28.82794727,3.534291732,5.0 -20.95721836,28.76631939,3.544763667,5.0 -21.10104965,28.70318186,3.555235732,5.0 -21.24421188,28.63854164,3.56570763,5.0 -21.38668937,28.57240579,3.576179644,5.0 -21.52846649,28.50478157,3.586651628,5.0 -21.66952769,28.4356764,3.597123595,5.0 -21.8098575,28.36509786,3.607595555,5.0 -21.94944053,28.29305369,3.618067517,5.0 -22.08826147,28.21955178,3.628539548,5.0 -22.22630511,28.1446002,3.639011484,5.0 -22.3635563,28.06820717,3.649483453,5.0 -22.5,27.99038106,3.659955428,5.0 -22.63562124,27.91113041,3.670427396,5.0 -22.77040514,27.8304639,3.680899447,5.0 -22.90433693,27.74839039,3.691371387,5.0 -23.03740192,27.66491888,3.701843339,5.0 -23.16958553,27.58005852,3.712315269,5.0 -23.30087324,27.49381861,3.722787314,5.0 -23.43125067,27.40620861,3.733259285,5.0 -23.56070352,27.31723814,3.743731199,5.0 -23.68921759,27.22691694,3.754203215,5.0 -23.81677878,27.13525492,3.764675229,5.0 -23.94337312,27.04226213,3.775147184,5.0 -24.06898672,26.94794877,3.785619154,5.0 -24.1936058,26.85232519,3.796091106,5.0 -24.3172167,26.75540186,3.806563117,5.0 -24.43980587,26.65718942,3.81703505,5.0 -24.56135985,26.55769864,3.82750705,5.0 -24.68186532,26.45694043,3.837979013,5.0 -24.80130906,26.35492583,3.848451043,5.0 -24.91967798,26.25166604,3.858922975,5.0 -25.0369591,26.14717238,3.869394922,5.0 -25.15313955,26.04145631,3.879866901,5.0 -25.26820659,25.93452941,3.89033894,5.0 -25.38214761,25.82640342,3.900810875,5.0 -25.49495011,25.71709019,3.911282866,5.0 -25.60660172,25.60660172,3.921754799,5.0 -25.71709019,25.49495011,3.932226835,5.0 -25.82640342,25.38214761,3.942698768,5.0 -25.93452941,25.26820659,3.953170759,5.0 -26.04145631,25.15313955,3.963642694,5.0 -26.14717238,25.0369591,3.974114733,5.0 -26.25166604,24.91967798,3.984586712,5.0 -26.35492583,24.80130906,3.995058659,5.0 -26.45694043,24.68186532,4.005530591,5.0 -26.55769864,24.56135985,4.016002621,5.0 -26.65718942,24.43980587,4.026474583,5.0 -26.75540186,24.3172167,4.036946584,5.0 -26.85232519,24.1936058,4.047418517,5.0 -26.94794877,24.06898672,4.057890528,5.0 -27.04226213,23.94337312,4.06836248,5.0 -27.13525492,23.81677878,4.07883445,5.0 -27.22691694,23.68921759,4.089306405,5.0 -27.31723814,23.56070352,4.099778419,5.0 -27.40620861,23.43125067,4.110250435,5.0 -27.49381861,23.30087324,4.120722349,5.0 -27.58005852,23.16958553,4.13119432,5.0 -27.66491888,23.03740192,4.141666365,5.0 -27.74839039,22.90433693,4.152138295,5.0 -27.8304639,22.77040514,4.162610247,5.0 -27.91113041,22.63562124,4.173082187,5.0 -27.99038106,22.5,4.183554238,5.0 -28.06820717,22.3635563,4.194026206,5.0 -28.1446002,22.22630511,4.204498181,5.0 -28.21955178,22.08826147,4.21497015,5.0 -28.29305369,21.94944053,4.225442086,5.0 -28.36509786,21.8098575,4.235914117,5.0 -28.4356764,21.66952769,4.246386079,5.0 -28.50478157,21.52846649,4.256858039,5.0 -28.57240579,21.38668937,4.267330006,5.0 -28.63854164,21.24421188,4.27780199,5.0 -28.70318186,21.10104965,4.288274004,5.0 -28.76631939,20.95721836,4.298745902,5.0 -28.82794727,20.8127338,4.309217967,5.0 -28.88805877,20.6676118,4.319689902,5.0 -28.94664729,20.52186829,4.330161847,5.0 -29.0037064,20.37551924,4.340633851,5.0 -29.05922984,20.22858071,4.351105847,5.0 -29.11321153,20.0810688,4.361577821,5.0 -29.16564555,19.9329997,4.372049766,5.0 -29.21652615,19.78438964,4.382521728,5.0 -29.26584774,19.63525492,4.392993745,5.0 -29.31360493,19.48561188,4.403465678,5.0 -29.35979246,19.33547695,4.413937711,5.0 -29.40440529,19.18486659,4.4244096,5.0 -29.4474385,19.03379731,4.434881669,5.0 -29.48888739,18.88228568,4.445353615,5.0 -29.52874742,18.73034831,4.455825536,5.0 -29.5670142,18.57800186,4.466297576,5.0 -29.60368354,18.42526305,4.476769556,5.0 -29.63875143,18.27214862,4.487241481,5.0 -29.67221401,18.11867536,4.497713496,5.0 -29.70406762,17.96486011,4.508185439,5.0 -29.73430876,17.81071972,4.518657447,5.0 -29.76293412,17.6562711,4.529129407,5.0 -29.78994056,17.5015312,4.539601358,5.0 -29.81532511,17.34651698,4.550073373,5.0 -29.83908499,17.19124543,4.560545373,5.0 -29.86121761,17.03573359,4.571017263,5.0 -29.88172052,16.8799985,4.581489309,5.0 -29.90059148,16.72405726,4.591961284,5.0 -29.91782843,16.56792695,4.602433217,5.0 -29.93342947,16.4116247,4.612905204,5.0 -29.94739289,16.25516765,4.623377187,5.0 -29.95971716,16.09857296,4.63384916,5.0 -29.97040093,15.94185779,4.64432113,5.0 -29.97944302,15.78503934,4.654793146,5.0 -29.98684245,15.62813481,4.665265089,5.0 -29.99259841,15.47116139,4.675737028,5.0 -29.99671025,15.3141363,4.686209084,5.0 -29.99917754,15.15707676,4.696681007,5.0 -30,15,4.707152989,5.0 -29.99917754,14.84292324,4.717624971,5.0 -29.99671025,14.6858637,4.728096954,5.0 -29.99259841,14.52883861,4.738568877,5.0 -29.98684245,14.37186519,4.749040933,5.0 -29.97944302,14.21496066,4.759512871,5.0 -29.97040093,14.05814221,4.769984814,5.0 -29.95971716,13.90142704,4.780456831,5.0 -29.94739289,13.74483235,4.790928801,5.0 -29.93342947,13.5883753,4.801400774,5.0 -29.91782843,13.43207305,4.811872757,5.0 -29.90059148,13.27594274,4.822344744,5.0 -29.88172052,13.1200015,4.832816676,5.0 -29.86121761,12.96426641,4.843288652,5.0 -29.83908499,12.80875457,4.853760698,5.0 -29.81532511,12.65348302,4.864232588,5.0 -29.78994056,12.4984688,4.874704588,5.0 -29.76293412,12.3437289,4.885176603,5.0 -29.73430876,12.18928028,4.895648554,5.0 -29.70406762,12.03513989,4.906120514,5.0 -29.67221401,11.88132464,4.916592522,5.0 -29.63875143,11.72785138,4.927064465,5.0 -29.60368354,11.57473695,4.93753648,5.0 -29.5670142,11.42199814,4.948008405,5.0 -29.52874742,11.26965169,4.958480385,5.0 -29.48888739,11.11771432,4.968952425,5.0 -29.4474385,10.96620269,4.979424346,5.0 -29.40440529,10.81513341,4.989896292,5.0 -29.35979246,10.66452305,5.000368361,5.0 -29.31360493,10.51438812,5.01084025,5.0 -29.26584774,10.36474508,5.021312283,5.0 -29.21652615,10.21561036,5.031784215,5.0 -29.16564555,10.0670003,5.042256233,5.0 -29.11321153,9.918931196,5.052728186,5.0 -29.05922984,9.77141929,5.063200149,5.0 -29.0037064,9.624480757,5.073672107,5.0 -28.94664729,9.47813171,5.084144116,5.0 -28.88805877,9.332388198,5.094616109,5.0 -28.82794727,9.187266203,5.105088071,5.0 -28.76631939,9.04278164,5.115559987,5.0 -28.70318186,8.898950354,5.126032069,5.0 -28.63854164,8.755788116,5.136503936,5.0 -28.57240579,8.613310627,5.146975973,5.0 -28.50478157,8.471533509,5.15744796,5.0 -28.4356764,8.330472312,5.16791993,5.0 -28.36509786,8.190142504,5.178391887,5.0 -28.29305369,8.050559473,5.188863841,5.0 -28.21955178,7.911738527,5.199335857,5.0 -28.1446002,7.773694888,5.209807814,5.0 -28.06820717,7.636443696,5.220279773,5.0 -27.99038106,7.5,5.230751767,5.0 -27.91113041,7.364378764,5.241223735,5.0 -27.8304639,7.229594859,5.251695757,5.0 -27.74839039,7.095663068,5.262167711,5.0 -27.66491888,6.962598075,5.272639655,5.0 -27.58005852,6.830414475,5.28311163,5.0 -27.49381861,6.699126761,5.293583627,5.0 -27.40620861,6.568749332,5.304055616,5.0 -27.31723814,6.439296485,5.314527537,5.0 -27.22691694,6.310782415,5.324999542,5.0 -27.13525492,6.183221216,5.335471523,5.0 -27.04226213,6.056626876,5.345943511,5.0 -26.94794877,5.931013277,5.356415485,5.0 -26.85232519,5.806394195,5.366887425,5.0 -26.75540186,5.682783296,5.377359448,5.0 -26.65718942,5.560194134,5.387831409,5.0 -26.55769864,5.438640154,5.398303377,5.0 -26.45694043,5.318134684,5.40877534,5.0 -26.35492583,5.19869094,5.419247353,5.0 -26.25166604,5.08032202,5.429719301,5.0 -26.14717238,4.963040905,5.44019127,5.0 -26.04145631,4.846860455,5.450663228,5.0 -25.93452941,4.731793411,5.461135249,5.0 -25.82640342,4.617852392,5.471607206,5.0 -25.71709019,4.505049892,5.482079193,5.0 -25.60660172,4.393398282,5.492551126,5.0 -25.49495011,4.282909806,5.503023134,5.0 -25.38214761,4.173596579,5.513495109,5.0 -25.26820659,4.065470589,5.523967086,5.0 -25.15313955,3.958543693,5.53443904,5.0 -25.0369591,3.852827618,5.544911036,5.0 -24.91967798,3.748333956,5.555383029,5.0 -24.80130906,3.645074165,5.565854981,5.0 -24.68186532,3.54305957,5.576326942,5.0 -24.56135985,3.442301358,5.586798938,5.0 -24.43980587,3.342810578,5.59727091,5.0 -24.3172167,3.24459814,5.60774292,5.0 -24.1936058,3.147674814,5.618214863,5.0 -24.06898672,3.05205123,5.628686834,5.0 -23.94337312,2.957737872,5.639158817,5.0 -23.81677878,2.864745084,5.649630787,5.0 -23.68921759,2.773083064,5.660102731,5.0 -23.56070352,2.682761863,5.670574741,5.0 -23.43125067,2.593791386,5.681046725,5.0 -23.30087324,2.506181389,5.691518691,5.0 -23.16958553,2.419941481,5.701990657,5.0 -23.03740192,2.335081117,5.712462671,5.0 -22.90433693,2.251609605,5.722934611,5.0 -22.77040514,2.169536098,5.73340659,5.0 -22.63562124,2.088869595,5.743878552,5.0 -22.5,2.009618943,5.754350554,5.0 -22.3635563,1.931792833,5.764822533,5.0 -22.22630511,1.855399799,5.775294486,5.0 -22.08826147,1.780448219,5.785766477,5.0 -21.94944053,1.706946312,5.79623843,5.0 -21.8098575,1.634902137,5.806710416,5.0 -21.66952769,1.564323596,5.8171824,5.0 -21.52846649,1.495218429,5.827654383,5.0 -21.38668937,1.427594213,5.838126356,5.0 -21.24421188,1.361458364,5.848598323,5.0 -21.10104965,1.296818135,5.859070278,5.0 -20.95721836,1.233680615,5.869542287,5.0 -20.8127338,1.172052726,5.880014241,5.0 -20.6676118,1.111941228,5.890486241,5.0 -20.52186829,1.053352712,5.900958197,5.0 -20.37551924,0.996293603,5.911430184,5.0 -20.22858071,0.940770158,5.921902144,5.0 -20.0810688,0.886788466,5.932374136,5.0 -19.9329997,0.834354446,5.942846093,5.0 -19.78438964,0.78347385,5.953318079,5.0 -19.63525492,0.734152256,5.963790048,5.0 -19.48561188,0.686395072,5.974262041,5.0 -19.33547695,0.640207537,5.984734007,5.0 -19.18486659,0.595594715,5.995205975,5.0 -19.03379731,0.552561498,6.005677953,5.0 -18.88228568,0.511112606,6.016149929,5.0 -18.73034831,0.471252583,6.026621906,5.0 -18.57800186,0.432985801,6.037093891,5.0 -18.42526305,0.396316457,6.047565858,5.0 -18.27214862,0.361248571,6.058037833,5.0 -18.11867536,0.327785989,6.068509811,5.0 -17.96486011,0.295932381,6.078981778,5.0 -17.81071972,0.265691239,6.089453762,5.0 -17.6562711,0.23706588,6.09992574,5.0 -17.5015312,0.210059444,6.11039771,5.0 -17.34651698,0.184674891,6.120869681,5.0 -17.19124543,0.160915006,6.131341668,5.0 -17.03573359,0.138782393,6.141813634,5.0 -16.8799985,0.11827948,6.152285617,5.0 -16.72405726,0.099408516,6.162757586,5.0 -16.56792695,0.082171569,6.173229562,5.0 -16.4116247,0.066570531,6.183701544,5.0 -16.25516765,0.052607111,6.194173514,5.0 -16.09857296,0.040282842,6.204645493,5.0 -15.94185779,0.029599074,6.215117469,5.0 -15.78503934,0.020556979,6.225589441,5.0 -15.62813481,0.013157549,6.236061416,5.0 -15.47116139,0.007401595,6.246533393,5.0 -15.3141363,0.003289748,6.257005366,5.0 -15.15707676,0.00082246,6.267477347,5.0 -15,0,6.277949316,5.0 -14.9,0,0,5.0 -14.8,0,0,5.0 -14.7,0,0,5.0 -14.6,0,0,5.0 -14.5,0,0,5.0 -14.4,0,0,5.0 -14.3,0,0,5.0 -14.2,0,0,5.0 -14.1,0,0,5.0 -14,0,0,5.0 -13.9,0,0,5.0 -13.8,0,0,5.0 -13.7,0,0,5.0 -13.6,0,0,5.0 -13.5,0,0,5.0 -13.4,0,0,5.0 -13.3,0,0,5.0 -13.2,0,0,5.0 -13.1,0,0,5.0 -13,0,0,5.0 -12.9,0,0,5.0 -12.8,0,0,5.0 -12.7,0,0,5.0 -12.6,0,0,5.0 -12.5,0,0,5.0 -12.4,0,0,5.0 -12.3,0,0,5.0 -12.2,0,0,5.0 -12.1,0,0,5.0 -12,0,0,5.0 -11.9,0,0,5.0 -11.8,0,0,5.0 -11.7,0,0,5.0 -11.6,0,0,5.0 -11.5,0,0,5.0 -11.4,0,0,5.0 -11.3,0,0,5.0 -11.2,0,0,5.0 -11.1,0,0,5.0 -11,0,0,5.0 -10.9,0,0,5.0 -10.8,0,0,5.0 -10.7,0,0,5.0 -10.6,0,0,5.0 -10.5,0,0,5.0 -10.4,0,0,5.0 -10.3,0,0,5.0 -10.2,0,0,5.0 -10.1,0,0,5.0 -10,0,0,5.0 -9.9,0,0,5.0 -9.8,0,0,5.0 -9.7,0,0,5.0 -9.6,0,0,5.0 -9.5,0,0,5.0 -9.4,0,0,5.0 -9.3,0,0,5.0 -9.2,0,0,5.0 -9.1,0,0,5.0 -9,0,0,5.0 -8.9,0,0,5.0 -8.8,0,0,5.0 -8.7,0,0,5.0 -8.6,0,0,5.0 -8.5,0,0,5.0 -8.4,0,0,5.0 -8.3,0,0,5.0 -8.2,0,0,5.0 -8.1,0,0,5.0 -8,0,0,5.0 -7.9,0,0,5.0 -7.8,0,0,5.0 -7.7,0,0,5.0 -7.6,0,0,5.0 -7.5,0,0,5.0 -7.4,0,0,5.0 -7.3,0,0,5.0 -7.2,0,0,5.0 -7.1,0,0,5.0 -7,0,0,5.0 -6.9,0,0,5.0 -6.8,0,0,5.0 -6.7,0,0,5.0 -6.6,0,0,5.0 -6.5,0,0,5.0 -6.4,0,0,5.0 -6.3,0,0,5.0 -6.2,0,0,5.0 -6.1,0,0,5.0 -6,0,0,5.0 -5.9,0,0,5.0 -5.8,0,0,5.0 -5.7,0,0,5.0 -5.6,0,0,5.0 -5.5,0,0,5.0 -5.4,0,0,5.0 -5.3,0,0,5.0 -5.2,0,0,5.0 -5.1,0,0,5.0 -5,0,0,5.0 -4.9,0,0,5.0 -4.8,0,0,5.0 -4.7,0,0,5.0 -4.6,0,0,5.0 -4.5,0,0,5.0 -4.4,0,0,5.0 -4.3,0,0,5.0 -4.2,0,0,5.0 -4.1,0,0,5.0 -4,0,0,5.0 -3.9,0,0,5.0 -3.8,0,0,5.0 -3.7,0,0,5.0 -3.6,0,0,5.0 -3.5,0,0,5.0 -3.4,0,0,5.0 -3.3,0,0,5.0 -3.2,0,0,5.0 -3.1,0,0,5.0 -3,0,0,5.0 -2.9,0,0,5.0 -2.8,0,0,5.0 -2.7,0,0,5.0 -2.6,0,0,5.0 -2.5,0,0,5.0 -2.4,0,0,5.0 -2.3,0,0,5.0 -2.2,0,0,5.0 -2.1,0,0,5.0 -2,0,0,5.0 -1.9,0,0,5.0 -1.8,0,0,5.0 -1.7,0,0,5.0 -1.6,0,0,5.0 -1.5,0,0,5.0 -1.4,0,0,5.0 -1.3,0,0,5.0 -1.2,0,0,5.0 -1.1,0,0,5.0 -1,0,0,5.0 -0.9,0,0,5.0 -0.8,0,0,5.0 -0.7,0,0,5.0 -0.6,0,0,5.0 -0.5,0,0,5.0 -0.4,0,0,5.0 -0.3,0,0,5.0 -0.2,0,0,5.0 -0.1,0,0,5.0 0,0,0,5.0 ================================================ FILE: notebooks/pid.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# PID Control\n", "PID stands for Proportional Integral Derivative." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle \n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Controller : PID Controller" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Longitudinal Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class PIDLongitudinalController():\n", " def __init__(\n", " self,\n", " p_gain: float = +1.2, # P Gain\n", " i_gain: float = +0.4, # I Gain\n", " d_gain: float = +0.1, # D Gain\n", " target_velocity: float = 3.0, # [m/s]\n", " ) -> None:\n", " \"\"\"initialize pid controller for keeping constant velocity\"\"\"\n", " # pid control parameters\n", " self.K_p = p_gain\n", " self.K_i = i_gain\n", " self.K_d = d_gain\n", " self.target_vel = target_velocity\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # calculate tracking error, its integral and derivative\n", " r = self.target_vel\n", " y = observed_vel\n", " e = r - y # tracking error to the traget velocity\n", " ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\n", " de = (e - self.pre_e) / delta_t # derivative of the tracking error\n", "\n", " # calculate control input\n", " acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\n", "\n", " # update previous tracking error\n", " self.pre_e = e\n", "\n", " return acc_cmd\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Keeping Speed Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 100 # [step]\n", "delta_t = 0.1 # [s]\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-3.0, 50.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, v]\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize speed controller\n", "speed_controller = PIDLongitudinalController(\n", " p_gain = 1.2,\n", " i_gain = 0.4,\n", " d_gain = 0.1,\n", " target_velocity = 3.0, # [m/s]\n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", " current_velocity = current_state[3]\n", "\n", " # calculate control inputs\n", " steer_input = 0.0 # steering command [rad] # set zero because this is just a test run of speed controller\n", " accel_input = speed_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t) # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"pid_speed_control_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Lateral Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class PIDLateralController():\n", " def __init__(\n", " self,\n", " p_gain: float = +0.8, # P Gain\n", " i_gain: float = +0.1, # I Gain\n", " d_gain: float = +0.1, # D Gain\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " ) -> None:\n", " \"\"\"initialize pid controller for path-tracking\"\"\"\n", " # pid control parameters\n", " self.K_p = p_gain\n", " self.K_i = i_gain\n", " self.K_d = d_gain\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " # ref_path info\n", " self.ref_path = ref_path\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # set vehicle state variables from observation\n", " x = observed_x[0]\n", " y = observed_x[1]\n", "\n", " # get the waypoint closest to current vehicle position \n", " _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # which side of the reference path is the car on, the right or the left?\n", " ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n", " x1, y1 = ref_x, ref_y\n", " x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0*np.sin(ref_yaw)\n", " vx, vy = x2 - x1, y2 - y1\n", " wx, wy = x - x1, y - y1\n", " s = vx * wy - vy * wx # s>0 : vehicle is on the left of the path, s<0 : vehicle is on the left of the path,\n", "\n", " # get tracking error, its integral and derivative\n", " e = -np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # tracking error to the reference path\n", " ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\n", " de = (e - self.pre_e) / delta_t # derivative of the tracking error\n", "\n", " # calculate control input\n", " steer_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\n", "\n", " # update previous tracking error\n", " self.pre_e = e\n", "\n", " return steer_cmd\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", "\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : PID Controller\n", "- Lateral Control : PID Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize pid controllers for accelration control\n", "pid_lon_controller = PIDLongitudinalController(\n", " p_gain = 1.2,\n", " i_gain = 0.4,\n", " d_gain = 0.1,\n", " target_velocity = +5.0 # [m/s]\n", ")\n", "\n", "# initialize pid controllers for steering control\n", "pid_lat_controller = PIDLateralController(\n", " p_gain = 1.2,\n", " i_gain = 10.0,\n", " d_gain = 0.001,\n", " ref_path = ref_path, # ndarray, size is \n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate control inputs\n", " current_velocity = current_state[3]\n", " accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\n", " steer_input = pid_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\n", "\n", " except IndexError as ex:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"pid_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/preliminary/linear_quadratic_regulator_tutorial.ipynb ================================================ { "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Stabilize A Linear System With Linear Quadratic Regulator\n", "\n", "\n", "## Control Target System\n", "$$\n", "\\dot{x}(t) = \\begin{bmatrix} 2 & 0 \\\\ 0 & -5 \\end{bmatrix} x(t) + \\begin{bmatrix} 1 \\\\ -2 \\end{bmatrix} u(t)\n", "$$" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Case.1 : zero input\n", "\n", "$$\n", "\\begin{align}\n", "{u}(t) = \\begin{bmatrix} 0 \\end{bmatrix} \\nonumber\n", "\\end{align}\n", "$$" ] }, { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "t = 0.00 [s], x = [3.00, -5.00]\n", "t = 0.10 [s], x = [3.60, -2.50]\n", "t = 0.20 [s], x = [4.32, -1.25]\n", "t = 0.30 [s], x = [5.18, -0.62]\n", "t = 0.40 [s], x = [6.22, -0.31]\n", "t = 0.50 [s], x = [7.46, -0.16]\n", "t = 0.60 [s], x = [8.96, -0.08]\n", "t = 0.70 [s], x = [10.75, -0.04]\n", "t = 0.80 [s], x = [12.90, -0.02]\n", "t = 0.90 [s], x = [15.48, -0.01]\n", "t = 1.00 [s], x = [18.58, -0.00]\n", "t = 1.10 [s], x = [22.29, -0.00]\n", "t = 1.20 [s], x = [26.75, -0.00]\n", "t = 1.30 [s], x = [32.10, -0.00]\n", "t = 1.40 [s], x = [38.52, -0.00]\n", "t = 1.50 [s], x = [46.22, -0.00]\n", "t = 1.60 [s], x = [55.47, -0.00]\n", "t = 1.70 [s], x = [66.56, -0.00]\n", "t = 1.80 [s], x = [79.87, -0.00]\n", "t = 1.90 [s], x = [95.84, -0.00]\n", "t = 2.00 [s], x = [115.01, -0.00]\n", "t = 2.10 [s], x = [138.02, -0.00]\n", "t = 2.20 [s], x = [165.62, -0.00]\n", "t = 2.30 [s], x = [198.74, -0.00]\n", "t = 2.40 [s], x = [238.49, -0.00]\n", "t = 2.50 [s], x = [286.19, -0.00]\n", "t = 2.60 [s], x = [343.43, -0.00]\n", "t = 2.70 [s], x = [412.11, -0.00]\n", "t = 2.80 [s], x = [494.53, -0.00]\n", "t = 2.90 [s], x = [593.44, -0.00]\n", "t = 3.00 [s], x = [712.13, -0.00]\n", "t = 3.10 [s], x = [854.55, -0.00]\n", "t = 3.20 [s], x = [1025.47, -0.00]\n", "t = 3.30 [s], x = [1230.56, -0.00]\n", "t = 3.40 [s], x = [1476.67, -0.00]\n", "t = 3.50 [s], x = [1772.00, -0.00]\n", "t = 3.60 [s], x = [2126.41, -0.00]\n", "t = 3.70 [s], x = [2551.69, -0.00]\n", "t = 3.80 [s], x = [3062.02, -0.00]\n", "t = 3.90 [s], x = [3674.43, -0.00]\n", "t = 4.00 [s], x = [4409.31, -0.00]\n", "t = 4.10 [s], x = [5291.18, -0.00]\n", "t = 4.20 [s], x = [6349.41, -0.00]\n", "t = 4.30 [s], x = [7619.30, -0.00]\n", "t = 4.40 [s], x = [9143.15, -0.00]\n", "t = 4.50 [s], x = [10971.79, -0.00]\n", "t = 4.60 [s], x = [13166.14, -0.00]\n", "t = 4.70 [s], x = [15799.37, -0.00]\n", "t = 4.80 [s], x = [18959.25, -0.00]\n", "t = 4.90 [s], x = [22751.10, -0.00]\n", "t = 5.00 [s], x = [27301.31, -0.00]\n", "t = 5.10 [s], x = [32761.58, -0.00]\n", "t = 5.20 [s], x = [39313.89, -0.00]\n", "t = 5.30 [s], x = [47176.67, -0.00]\n", "t = 5.40 [s], x = [56612.01, -0.00]\n", "t = 5.50 [s], x = [67934.41, -0.00]\n", "t = 5.60 [s], x = [81521.29, -0.00]\n", "t = 5.70 [s], x = [97825.55, -0.00]\n", "t = 5.80 [s], x = [117390.65, -0.00]\n", "t = 5.90 [s], x = [140868.79, -0.00]\n", "t = 6.00 [s], x = [169042.54, -0.00]\n", "t = 6.10 [s], x = [202851.05, -0.00]\n", "t = 6.20 [s], x = [243421.26, -0.00]\n", "t = 6.30 [s], x = [292105.51, -0.00]\n", "t = 6.40 [s], x = [350526.62, -0.00]\n", "t = 6.50 [s], x = [420631.94, -0.00]\n", "t = 6.60 [s], x = [504758.33, -0.00]\n", "t = 6.70 [s], x = [605709.99, -0.00]\n", "t = 6.80 [s], x = [726851.99, -0.00]\n", "t = 6.90 [s], x = [872222.39, -0.00]\n", "t = 7.00 [s], x = [1046666.87, -0.00]\n", "t = 7.10 [s], x = [1256000.24, -0.00]\n", "t = 7.20 [s], x = [1507200.29, -0.00]\n", "t = 7.30 [s], x = [1808640.35, -0.00]\n", "t = 7.40 [s], x = [2170368.42, -0.00]\n", "t = 7.50 [s], x = [2604442.11, -0.00]\n", "t = 7.60 [s], x = [3125330.53, -0.00]\n", "t = 7.70 [s], x = [3750396.64, -0.00]\n", "t = 7.80 [s], x = [4500475.96, -0.00]\n", "t = 7.90 [s], x = [5400571.16, -0.00]\n", "t = 8.00 [s], x = [6480685.39, -0.00]\n", "t = 8.10 [s], x = [7776822.46, -0.00]\n", "t = 8.20 [s], x = [9332186.96, -0.00]\n", "t = 8.30 [s], x = [11198624.35, -0.00]\n", "t = 8.40 [s], x = [13438349.22, -0.00]\n", "t = 8.50 [s], x = [16126019.06, -0.00]\n", "t = 8.60 [s], x = [19351222.87, -0.00]\n", "t = 8.70 [s], x = [23221467.45, -0.00]\n", "t = 8.80 [s], x = [27865760.94, -0.00]\n", "t = 8.90 [s], x = [33438913.12, -0.00]\n", "t = 9.00 [s], x = [40126695.75, -0.00]\n", "t = 9.10 [s], x = [48152034.90, -0.00]\n", "t = 9.20 [s], x = [57782441.88, -0.00]\n", "t = 9.30 [s], x = [69338930.25, -0.00]\n", "t = 9.40 [s], x = [83206716.30, -0.00]\n", "t = 9.50 [s], x = [99848059.56, -0.00]\n", "t = 9.60 [s], x = [119817671.47, -0.00]\n", "t = 9.70 [s], x = [143781205.77, -0.00]\n", "t = 9.80 [s], x = [172537446.92, -0.00]\n", "t = 9.90 [s], x = [207044936.31, -0.00]\n" ] }, { "data": { "image/png": "iVBORw0KGgoAAAANSUhEUgAAA90AAAPdCAYAAACXzguGAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjguMCwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy81sbWrAAAACXBIWXMAAA9hAAAPYQGoP6dpAACRdElEQVR4nOzdeXhTZf738U+6F9oG2VoKBYqiyL6XgiOolUX0J6OioiPLKD46gCKOCozCuFERnUGURZyRqiPKMAoqKg4WgUGQ1YprAWUTaAGRphRpS5Lnj2NTKk0pkJOTJu/XdZ2rJ6ff03yDyW0/vc9ic7vdbgEAAAAAAJ8Ls7oBAAAAAACCFaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQun1o1apVuuaaa5ScnCybzabFixef8c/46KOP1KNHD8XHx6tBgwa6/vrrtXPnTp/3CgAAAAAwH6Hbh4qKitShQwfNnDnzrPbfsWOHrr32Wl1++eXKycnRRx99pEOHDum6667zcacAAAAAAH+wud1ut9VNBCObzaZFixZp0KBBnm3FxcX6y1/+ojfeeENHjhxR27ZtNXXqVPXp00eS9J///EdDhgxRcXGxwsKMv4e89957uvbaa1VcXKzIyEgLXgkAAAAA4Gwx0+1Ho0eP1tq1a/Xmm29qy5YtGjx4sPr3769t27ZJkrp06aKwsDDNmzdPTqdTBQUFeu2115SRkUHgBgAAAIAaiJluk/x2pnv37t1q0aKFdu/ereTkZE9dRkaGunfvrilTpkiSVq5cqRtvvFE//fSTnE6n0tPT9cEHH6hOnToWvAoAAAAAwLlgpttPvvzySzmdTl144YWKi4vzLCtXrtT3338vScrLy9PIkSM1bNgwbdiwQStXrlRUVJRuuOEG8bcRAAAAAKh5IqxuIFQcPXpU4eHh2rRpk8LDwyt8Ly4uTpI0c+ZM2e12Pf30057v/etf/1JKSorWrVunHj16+LVnAAAAAMC5IXT7SadOneR0OnXgwAH97ne/q7Tm2LFjnguolSkL6C6Xy/QeAQAAAAC+xeHlPnT06FHl5OQoJydHknELsJycHO3evVsXXnihbr31Vg0dOlRvv/22duzYofXr1yszM1Pvv/++JGngwIHasGGDHnvsMW3btk2bN2/WiBEj1KxZM3Xq1MnCVwYAAAAAOBtcSM2HVqxYocsuu+yU7cOGDVNWVpZKS0v1xBNP6NVXX9XevXtVv3599ejRQ48++qjatWsnSXrzzTf19NNPa+vWrapVq5bS09M1depUtWrVyt8vBwAAAABwjgjdAAAAAACYhMPLAQAAAAAwCRdS8wGXy6V9+/YpPj5eNpvN6nYAAAAAACZzu90qLCxUcnLyKRfEPhmh2wf27dunlJQUq9sAAAAAAPjZnj171KRJE6/fJ3T7QHx8vCTjHzshIcHibgAAAAAAZnM4HEpJSfHkQW8I3T5Qdkh5QkICoRsAAAAAQsjpTjHmQmoAAAAAAJiE0A0AAAAAgEkI3QAAAAAAmITQDQAAAACASYIqdGdmZqpbt26Kj49Xw4YNNWjQIOXm5la5T1ZWlmw2W4UlJibGTx0DAAAAAIJZUIXulStXatSoUfrss8+0bNkylZaWqm/fvioqKqpyv4SEBO3fv9+z7Nq1y08dAwAAAACCWVDdMmzp0qUVHmdlZalhw4batGmTLr30Uq/72Ww2JSUlmd0eAAAAACDEBNVM928VFBRIkurWrVtl3dGjR9WsWTOlpKTo2muv1ddff11lfXFxsRwOR4UFAAAAAOAbbrfVHfhO0IZul8ulsWPHqlevXmrbtq3Xuosuukgvv/yy3nnnHf3rX/+Sy+VSz5499eOPP3rdJzMzU3a73bOkpKSY8RIAAAAAICT16yelpUnr1lndybmzud3B9DeEcnfffbc+/PBDrV69Wk2aNKn2fqWlpbr44os1ZMgQPf7445XWFBcXq7i42PPY4XAoJSVFBQUFSkhIOOfeAQAAACBUOZ2S3S4VFUlffy21bm11R5VzOByy2+2nzYFBdU53mdGjR2vJkiVatWrVGQVuSYqMjFSnTp20fft2rzXR0dGKjo4+1zYBAAAAAL/x3XdG4I6Lky66yOpuzl1QHV7udrs1evRoLVq0SMuXL1dqauoZ/wyn06kvv/xSjRo1MqFDAAAAAEBVNmwwvnbuLIWHW9uLLwTVTPeoUaM0f/58vfPOO4qPj1deXp4kyW63KzY2VpI0dOhQNW7cWJmZmZKkxx57TD169NAFF1ygI0eOaNq0adq1a5fuuOMOy14HAAAAAISqstDdrZu1ffhKUIXu2bNnS5L69OlTYfu8efM0fPhwSdLu3bsVFlY+wf/zzz9r5MiRysvL03nnnacuXbpozZo1ah2oJw4AAAAAQBDbuNH42rWrtX34StBeSM2fqnsCPQAAAADAu5ISKT7e+Lp9u3T++VZ35F11c2BQndMNAAAAAKi5vvrKCNznnSe1aGF1N75B6AYAAAAABISy87m7dpVsNmt78RVCNwAAAAAgIATbRdQkQjcAAAAAIEAE20XUJEI3AAAAACAAHDtmnNMtMdMNAAAAAIBPffGF5HRKSUlS48ZWd+M7hG4AAAAAgOWC8SJqEqEbAAAAABAAgvEiahKhGwAAAAAQAILxImoSoRsAAAAAYDGHQ8rNNdaZ6QYAAAAAwIc2b5bcbqlZM6lBA6u78S1CNwAAAADAUidfRC3YELoBAAAAAJYK1ouoSYRuAAAAAIDFgvUiahKhGwAAAABgoUOHpB07jPUuXaztxQyEbgAAAACAZcpmuS+8UKpTx9JWTEHoBgAAAABYJpgPLZcI3QAAAAAACwXzRdQkQjcAAAAAwEJlM92EbgAAAAAAfGjfPmMJC5M6drS6G3MQugEAAAAAlig7tLxNG6l2bWt7MQuhGwAAAABgiWC/iJpE6AYAAAAAWCTYL6ImEboBAAAAABZwu4P/ImoSoRsAAAAAYIGdO6WffpIiI6V27azuxjyEbgAAAACA35UdWt6hgxQdbW0vZiJ0AwAAAAD8LhQuoiYRugEAAAAAFgiFi6hJhG4AAAAAgJ+5XNKmTcY6oRsAAAAAAB/aulUqLJRiY6WLL7a6G3MRugEAAAAAflV2aHnnzlJEhLW9mI3QDQAAAADwq1C5iJpE6AYAAAAA+FmoXERNInQDAAAAAPyotFT6/HNjndANAAAAAIAPbdkiHT8u2e3SBRdY3Y35CN0AAAAAAL9ZudL4esklUlgIJNIQeIkAAAAAgEBRFrp797a2D38hdAMAAAAA/MLplFatMtb79LG0Fb8JqtCdmZmpbt26KT4+Xg0bNtSgQYOUm5t72v0WLlyoVq1aKSYmRu3atdMHH3zgh24BAAAAILR8+aV05IgUHy916mR1N/4RVKF75cqVGjVqlD777DMtW7ZMpaWl6tu3r4qKirzus2bNGg0ZMkS33367Pv/8cw0aNEiDBg3SV1995cfOAQAAACD4lR1a3quXFBFhbS/+YnO73W6rmzDLwYMH1bBhQ61cuVKXXnpppTU33XSTioqKtGTJEs+2Hj16qGPHjpozZ06l+xQXF6u4uNjz2OFwKCUlRQUFBUpISPDtiwAAAACAIHHdddKiRVJmpjR+vNXdnBuHwyG73X7aHBhUM92/VVBQIEmqW7eu15q1a9cqIyOjwrZ+/fpp7dq1XvfJzMyU3W73LCkpKb5pGAAAAACClMtVfj53qFxETQri0O1yuTR27Fj16tVLbdu29VqXl5enxMTECtsSExOVl5fndZ8JEyaooKDAs+zZs8dnfQMAAABAMPrmG+mnn6RataSuXa3uxn+C9ij6UaNG6auvvtLq1at9/rOjo6MVHR3t858LAAAAAMGq7Hzunj2lyEhre/GnoAzdo0eP1pIlS7Rq1So1adKkytqkpCTl5+dX2Jafn6+kpCQzWwQAAACAkLJihfE1lA4tl4Ls8HK3263Ro0dr0aJFWr58uVJTU0+7T3p6urKzsytsW7ZsmdLT081qEwAAAABCitsdevfnLhNUM92jRo3S/Pnz9c477yg+Pt5zXrbdbldsbKwkaejQoWrcuLEyMzMlSffee6969+6tZ599VgMHDtSbb76pjRs3au7cuZa9DgAAAAAIJt99Jx04IMXESN26Wd2NfwXVTPfs2bNVUFCgPn36qFGjRp5lwYIFnprdu3dr//79nsc9e/bU/PnzNXfuXHXo0EH/+c9/tHjx4iovvgYAAAAAqL6y87nT06VQuzxWUM10V+eW4yvKTiQ4yeDBgzV48GATOgIAAAAAlIXuUDufWwqymW4AAAAAQGBxuwndAAAAAACYYvt2af9+KSpKSkuzuhv/I3QDAAAAAExTNsudlib9en3rkELoBgAAAACYJlTvz12G0A0AAAAAMMXJ53OH2v25yxC6AQAAAACm2LFD+vFHKTLSuF1YKCJ0AwAAAABMUTbL3a2bVKuWtb1YhdANAAAAADBFKN8qrAyhGwAAAABgCkI3oRsAAAAAYIJdu6SdO6XwcKlnT6u7sQ6hGwAAAADgc2Wz3F26SPHx1vZiJUI3AAAAAMDnOLTcQOgGAAAAAPhcqN+fuwyhGwAAAADgU3v3St9/L4WFSZdcYnU31iJ0AwAAAAB8qmyWu1MnKSHB2l6sRugGAAAAAPgU53OXI3QDAAAAAHyK0F2O0A0AAAAA8Jn9+6XcXMlmk373O6u7sR6hGwAAAADgM2Wz3O3bS+edZ20vgYDQDQAAAADwmQ8+ML5mZFjbR6AgdAMAAAAAfMLplD780Fi/5hprewkUhG4AAAAAgE+sXy8dOiTZ7VLPnlZ3ExgI3QAAAAAAn1iyxPjav78UGWltL4GC0A0AAAAA8In33ze+Xn21tX0EEkI3AAAAAOCc7dkjffGFFBZmzHTDQOgGAAAAAJyzslnuHj2k+vWt7SWQELoBAAAAAOes7HxuDi2viNANAAAAADgnx45J2dnGOqG7IkI3AAAAAOCcfPKJdPy41LSp1Lat1d0EFkI3AAAAAOCclB1aPnCgZLNZ20ugIXQDAAAAAM6a28353FUhdAMAAAAAztqXX0o//ijFxkqXXWZ1N4GH0A0AAAAAOGtls9wZGUbwRkWEbgAAAADAWePQ8qoRugEAAAAAZ+XgQemzz4z1q66ytpdARegGAAAAAJyVpUuNC6l17Cg1aWJ1N4GJ0A0AAAAAOCscWn56QRW6V61apWuuuUbJycmy2WxavHhxlfUrVqyQzWY7ZcnLy/NPwwAAAABQQ5WWGjPdEqG7KkEVuouKitShQwfNnDnzjPbLzc3V/v37PUvDhg1N6hAAAAAAgsPq1ZLDITVoIHXrZnU3gSvC6gZ8acCAARowYMAZ79ewYUPVqVPH9w0BAAAAQJB6/33j61VXSWFBNZ3rW/zTSOrYsaMaNWqkK6+8Up9++ulp64uLi+VwOCosAAAAABBKOJ+7ekI6dDdq1Ehz5szRW2+9pbfeekspKSnq06ePNm/eXOV+mZmZstvtniUlJcVPHQMAAACA9bZtk3JzpYgIqW9fq7sJbDa32+22ugkz2Gw2LVq0SIMGDTqj/Xr37q2mTZvqtdde81pTXFys4uJiz2OHw6GUlBQVFBQoISHhbFsGAAAAgBph+nTpvvukyy+XsrOt7sYaDodDdrv9tDkwqM7p9oXu3btr9erVVdZER0crOjraTx0BAAAAQGApO5+bQ8tPL6QPL69MTk6OGjVqZHUbAAAAABCQHA5p5UpjndB9ekE103306FFt377d83jHjh3KyclR3bp11bRpU02YMEF79+7Vq6++KkmaPn26UlNT1aZNGx0/flz/+Mc/tHz5cv33v/+16iUAAAAAQEBbtsy4R/eFF0otW1rdTeALqtC9ceNGXXbZZZ7H48aNkyQNGzZMWVlZ2r9/v3bv3u35fklJie6//37t3btXtWrVUvv27fXxxx9X+BkAAAAAgHLvvGN8HTjQ2j5qiqC9kJo/VfcEegAAAACoyX75RWrYUDp6VFq9WurVy+qOrFPdHMg53QAAAACAannvPSNwN28u9expdTc1A6EbAAAAAFAtr79ufL3lFslms7aXmoLQDQAAAAA4rcOHpQ8/NNZvvdXaXmoSQjcAAAAA4LQWLjSuWt6xo9S6tdXd1ByEbgAAAADAaZ18aDmqj9ANAAAAAKjSrl3S//5nnMc9ZIjV3dQshG4AAAAAQJXeeMP42ru31KSJtb3UNIRuAAAAAECVyg4t5wJqZ47QDQAAAADwassW6auvpKgo6YYbrO6m5iF0AwAAAAC8mj/f+DpwoFSnjqWt1EiEbgAAAABApVyu8tDNoeVnh9ANAAAAAKjU6tXSnj2S3W7MdOPMEboBAAAAAJUqu4Da9ddLMTHW9lJTBXToXrdundUtAAAAAEBIKimRFi401m+5xdpearKADt2DBw+2ugUAAAAACEkffij9/LPUqJHUp4/V3dRcEVY3cOONN1a63e126/Dhw37uBgAAAAAglR9aPmSIFB5ubS81meWh++OPP9Zrr72muLi4CtvdbrdWrVplUVcAAAAAELocDum994x1rlp+biwP3X369FF8fLwuvfTSU77Xvn17CzoCAAAAgND29tvS8eNSq1ZSp05Wd1OzWRa6CwsLFR8fr7fffttrzbJly/zYEQAAAABAKj+0/NZbJZvN2l5qOssupPa73/1OeXl5Vj09AAAAAKAS+/dLy5cb61y1/NxZFro7deqktLQ0fffddxW25+Tk6KqrrrKoKwAAAAAIbQsWSC6XlJ4utWhhdTc1n2Whe968eRo+fLguueQSrV69Wlu3btWNN96oLl26KJxL4wEAAACAJV57zfjKBdR8w9ILqT366KOKjo7WlVdeKafTqSuuuEJr165V9+7drWwLAAAAAELSunXS5s1SdLTk5e7OOEOWzXTn5+fr3nvv1RNPPKHWrVsrMjJSw4cPJ3ADAAAAgEWef974evPNUoMG1vYSLCwL3ampqVq1apUWLlyoTZs26a233tKdd96padOmWdUSAAAAAISsvDzp3/821seMsbaXYGLZ4eUvv/yybr75Zs/j/v3765NPPtHVV1+tnTt3aubMmVa1BgAAAAAhZ+5cqbTUuIBaly5WdxM8LJvpPjlwl+ncubPWrFmj5WXXpwcAAAAAmK6kRJozx1hnltu3LAvd3jRv3lxr1qyxug0AAAAACBlvvWXcnzspSbr+equ7CS4BF7ol6bzzzrO6BQAAAAAIGWUXULvrLikqytpegk1Ahm4AAAAAgH9s2iStXStFRkr/7/9Z3U3wIXQDAAAAQAgrm+UePNg4vBy+RegGAAAAgBB18KD05pvGOhdQMwehGwAAAABC1EsvScXFUteuUlqa1d0EJ0I3AAAAAISgEyek2bON9XvukWw2a/sJVoRuAAAAAAhBixdLP/4oNWwo3Xij1d0EL0I3AAAAAISgsguo3XmnFB1tbS/BjNANAAAAACFmyxZp1SopIsK4NzfMQ+gGAAAAgBBTNst93XVS48bW9hLsgip0r1q1Stdcc42Sk5Nls9m0ePHi0+6zYsUKde7cWdHR0brggguUlZVlep8AAAAAYJXDh6XXXzfWuU2Y+YIqdBcVFalDhw6aOXNmtep37NihgQMH6rLLLlNOTo7Gjh2rO+64Qx999JHJnQIAAACANf75T+mXX6SOHaVevazuJvhFWN2ALw0YMEADBgyodv2cOXOUmpqqZ599VpJ08cUXa/Xq1fr73/+ufv36ed2vuLhYxcXFnscOh+PsmwYAAAAAP3E6pVmzjPUxY7hNmD8E1Uz3mVq7dq0yMjIqbOvXr5/Wrl1b5X6ZmZmy2+2eJSUlxcw2AQAAAMAn5s+Xdu6U6tWThgyxupvQENKhOy8vT4mJiRW2JSYmyuFw6JdffvG634QJE1RQUOBZ9uzZY3arAAAAAHBOSkulRx811h94QIqNtbafUBFUh5f7S3R0tKK5kR0AAACAGuTVV6Xvv5caNJBGj7a6m9AR0jPdSUlJys/Pr7AtPz9fCQkJiuXPPgAAAACCREmJ9Pjjxvr48VLt2tb2E0pCOnSnp6crOzu7wrZly5YpPT3doo4AAAAAwPf++U9p1y6pUSPp7rut7ia0BFXoPnr0qHJycpSTkyPJuCVYTk6Odu/eLck4F3vo0KGe+rvuuks//PCDHnzwQX333XeaNWuW/v3vf+u+++6zon0AAAAA8Lnjx6UnnzTWJ07kXG5/C6rQvXHjRnXq1EmdOnWSJI0bN06dOnXSpEmTJEn79+/3BHBJSk1N1fvvv69ly5apQ4cOevbZZ/WPf/yjytuFAQAAAEBN8uKL0t69UkqKNHKk1d2EHpvb7XZb3URN53A4ZLfbVVBQoISEBKvbAQAAAABJ0rFjUosWUn6+Eb7vvNPqjoJHdXNgUM10AwAAAADKzZplBO7UVGnECKu7CU2EbgAAAAAIQoWF0tSpxvqkSVJkpLX9hCpCNwAAAAAEoeeflw4dklq2lP7wB6u7CV2EbgAAAAAIMgUF0jPPGOuTJ0sREdb2E8oI3QAAAAAQZP7+d+nnn6XWraWbb7a6m9BG6AYAAACAIHL4sBG6Jemvf5XCwy1tJ+QRugEAAAAgiDzzjORwSO3bS9dfb3U3IHQDAAAAQJA4eFCaMcNYf+wxKYzEZzn+EwAAAABAkJg4USoqkrp0kf7v/6zuBhKhGwAAAACCwpo10j/+YaxPny7ZbJa2g18RugEAAACghjtxQrr7bmN9xAjpkkus7QflCN0AAAAAUMPNmCFt2SLVrSs9/bTV3eBkhG4AAAAAqMF+/FGaPNlYnzpVql/f2n5QEaEbAAAAAGqwsWOlo0el9HTpj3+0uhv8FqEbAAAAAGqoDz+U3npLCg+X5szhFmGBiP8kAAAAAFAD/fKLNHq0sX7vvVL79tb2g8oRugEAAACgBpoyRfrhB6lxY+mvf7W6G3hD6AYAAACAGiY317hommRcuTw+3tp+4B2hGwAAAABqELdb+tOfpNJS6aqrpN//3uqOUBVCNwAAAADUIG+8IS1fLsXESM8/L9lsVneEqhC6AQAAAKCGOHJEuu8+Y/3hh6UWLSxtB9VA6AYAAACAGuLPf5YOHJAuushYR+AjdAMAAABADfDmm9I//2kcTv7ii1J0tNUdoToI3QAAAAAQ4L7/XrrzTmP9L3+Reve2th9UH6EbAAAAAAJYSYl0881SYaF0ySXS5MlWd4QzQegGAAAAgAA2YYK0caN03nnS/PlSRITVHeFMELoBAAAAIEC9/770t78Z61lZUkqKpe3gLBC6AQAAACAA7d0rDRtmrN9zj/R//2dtPzg7hG4AAAAACDBOp3TrrdJPP0mdOklPP211RzhbhG4AAAAACDBPPCGtXCnFxUkLFnB7sJqM0A0AAAAAAWTlSumxx4z12bOlli2t7QfnhtANAAAAAAHi0CHpllskl0saPlz6wx+s7gjnitANAAAAAAHgxAnpttukffukiy6Snn/e6o7gC4RuAAAAALCY2y396U/S0qVSTIxxHndcnNVdwRcI3QAAAABgsSlTpJdekmw26Y03pA4drO4IvkLoBgAAAAALvfKK9PDDxvrzz0uDBlnaDnyM0A0AAAAAFlm2TLrjDmP9wQelUaOs7Qe+F5She+bMmWrevLliYmKUlpam9evXe63NysqSzWarsMTExPixWwAAAAChKCdHuv564wJqQ4ZImZlWdwQzBF3oXrBggcaNG6fJkydr8+bN6tChg/r166cDBw543SchIUH79+/3LLt27fJjxwAAAABCze7d0lVXSYWF0mWXSfPmSWFBl84gBWHo/tvf/qaRI0dqxIgRat26tebMmaNatWrp5Zdf9rqPzWZTUlKSZ0lMTPRjxwAAAABCyc8/S/37S/v3S23bSm+/LUVHW90VzBJUobukpESbNm1SRkaGZ1tYWJgyMjK0du1ar/sdPXpUzZo1U0pKiq699lp9/fXXVT5PcXGxHA5HhQUAAAAATuf4ceNCad9+KzVuLH3wgVSnjtVdwUxBFboPHTokp9N5ykx1YmKi8vLyKt3noosu0ssvv6x33nlH//rXv+RyudSzZ0/9+OOPXp8nMzNTdrvds6SkpPj0dQAAAAAIPk6nNGyYtGqVlJAgffihRJQIfkEVus9Genq6hg4dqo4dO6p37956++231aBBA7344ote95kwYYIKCgo8y549e/zYMQAAAICapqTEuFjav/8tRUZKixZJ7dpZ3RX8IcLqBnypfv36Cg8PV35+foXt+fn5SkpKqtbPiIyMVKdOnbR9+3avNdHR0YrmpAsAAAAA1XDsmHTDDcbMdlSU9Oab0uWXW90V/CWoZrqjoqLUpUsXZWdne7a5XC5lZ2crPT29Wj/D6XTqyy+/VKNGjcxqEwAAAECIcDiMi6Z9+KEUGyu99570+99b3RX8KahmuiVp3LhxGjZsmLp27aru3btr+vTpKioq0ogRIyRJQ4cOVePGjZX5603wHnvsMfXo0UMXXHCBjhw5omnTpmnXrl26o+wO9QAAAABwFg4dkgYMkDZuNM7hfv996ZJLrO4K/hZ0ofumm27SwYMHNWnSJOXl5aljx45aunSp5+Jqu3fvVthJN8D7+eefNXLkSOXl5em8885Tly5dtGbNGrVu3dqqlwAAAACghtu3T7rySumbb6T69aWPPpI6d7a6K1jB5na73VY3UdM5HA7Z7XYVFBQoISHB6nYAAAAAWGjHDikjQ/rhB+O2YMuWSRdfbHVX8LXq5sCgm+kGAAAAAKt8+60RuPftk1q0kD7+WEpNtborWCmoLqQGAAAAAFb59FPp0kuNwN26tfS//xG4QegGAAAAgHPidksvvCD16WNcPK1LF2nlSik52erOEAgI3QAAAABwlo4dk4YOlcaMkU6ckG68UVqxwrh4GiBxTjcAAAAAnJUffpCuu0764gspPFx6+mnpvvskm83qzhBICN0AAAAAcIY+/FC65RbpyBGpYUNpwQLj8HLgtzi8HAAAAACqyeWSHntMGjjQCNxpadKmTQRueMdMNwAAAABUw88/G+dvL1liPL7rLmn6dCk62tK2EOCY6QYAAACA03j3XalNGyNwR0dL8+ZJs2cTuHF6zHQDAAAAgBeHDkn33CO98Ybx+MILjfXOna3tCzUHM90AAAAA8Btut/Tvf0utWxshOyxMevBBKSeHwI0zw0w3AAAAAJxk/37pT3+SFi82HrdtK738stStm6VtoYZiphsAAAAAZMxuZ2UZs9uLF0sREdLkycbVyQncOFvMdAMAAAAIeZ9/Lv35z9Ly5cbjLl2M2e327a3tCzUfM90AAAAAQtbOndIf/mCcp718uXE18qeekj77jMAN32CmGwAAAEDIOXxYmjJFev55qaTE2HbLLdITT0ipqdb2huBC6AYAAAAQMo4fN4L2lCnSkSPGtssvl55+2jikHPA1QjcAAACAoFdaKr3+unFhtN27jW3t2hlhu18/yWaztj8EL0I3AAAAgKDlcEgvvSRNny79+KOxrUkT6fHHpdtuk8LDLW0PIYDQDQAAACDo/PijNGOG9OKLRvCWpMREadw4acwYKTbW2v4QOgjdAAAAAILGli3SM89Ib7whnThhbGvVyrgd2K23SjEx1vaH0EPoBgAAAFCjlZRI770nzZ0r/fe/5dt79zbC9lVXSWHcLBkWIXQDAAAAqJFycqR584wLpP30k7EtLEy64Qbp/vul7t0tbQ+QROgGAAAAUIP89JMRsufNM0J3mUaNpKFDpTvvlFq0sKw94BSEbgAAAAAB7ehR6aOPpDfflN55x7j9lyRFRUn/93/SiBFS375SBOkGAYi3JQAAAICAc+iQcZ72okXSsmXS8ePl3+vUyQjat9wi1atnXY9AdRC6AQAAAASE3bulxYuNoL1qleRylX+vRQvp97837q3doYNlLQJnjNANAAAAwBKFhUa4zs42li1bKn6/Y0cjaP/+91LbtpLNZkmbwDkhdAMAAADwi5ISad06I2B//LGxXnYvbckI1ZdcYoTsQYOk1FTLWgV8htANAAAAwBQ//yytXy999pm0dq20erVUVFSxJjVVuuIKKSNDuvxyqUEDa3oFzELoBgAAAHDOTpyQvv7aCNhly3ffnVpXv74RsssWbu+FYEfoBgAAAHBGCguN86+/+KJ8+fJL6dixU2svuEDq0cNYLrlEatdOCgvzf8+AVQjdAAAAACp17Ji0dauUmyt9+60RrrdskX74ofL6+HgpLa08ZKelGTPbQCgjdAMAAAAhrLRU2rNH2r7dCNcnL3v2eN+vcWPj1l0dOkjt2xtfL7xQCg/3X+9ATUDoBgAAAIJYSYm0f7+0a5e0Y4e0c6fxtWz9xx8r3g/7t+rWlS66yFjKwnWHDlK9ev56BUDNRugGAAAAaqDSUungQSk/31j27j112bdPOnDg9D8rJsa4inhZuD55IVwD54bQDQAAAFjM7ZYcDumnn6TDh42vv13y840AXfb18OHq//yoKCklxQjWqalS8+YVvyYmGvfIBuB7hG4AAADgHJSUGFfzrmopKJCOHCn/+tv1I0eMW26dqfBw477WDRtKycnGedZNmhhfT17q1SNUA1YJytA9c+ZMTZs2TXl5eerQoYOef/55de/e3Wv9woUL9cgjj2jnzp1q2bKlpk6dqquuusqPHQMAAOBcuVxGAC4pkYqLK64fP24s3tZ/+aXqpajI+3I2Ydmb2FgjINeta3wtW+rWNWajExONgF22Xrcut98CAl3Qhe4FCxZo3LhxmjNnjtLS0jR9+nT169dPubm5atiw4Sn1a9as0ZAhQ5SZmamrr75a8+fP16BBg7R582a1bdv2jJ67pKREJSUlp2wPCwtTREREhTpvbDabIiMjz6q2tLRUbrfbr7WSFBUVdVa1J06ckKuKq3acSW1kZKRsv/751qxap9Mpp9Ppk9qIiAiF/fp/yECodblcOlHFbwzh4eEK//VSpIFQ63a7VVpa6pPakz+fZtVKVX+WGSMqr2WMYIwwc4wICyv7ue5f35f6dd+KX222MIWHR3hqS0pKK61zu8tr3W6j9sQJ4+dWtpxc63Ybn+Wqam228trS0hK5XBVryh/bFBYW6XlcUlKx1uUqX9xum2y2SM/j0tJSOZ1uT33ZdqdTcrlscrsjPY9PnCivdTp/u9jkckWe9LhUJ064f92vvK5svbQ0SqWlZY+N2hMnjMdl20tLjeX4caO2pERyuU7oxAmX53slJRXDb2lp+RgREXFCNpv3z3JpaaQk26/vjxMKCzuz2rIhMSZGioszbpsVFyfVqhWp+Hib4uMlu92pOnWcSkiQ6tSR7PaKS716kapf36bY2DP/LJeUMEbwewS/R/y21h+/R1T173cym7uqrmugtLQ0devWTS+88IIk4wOYkpKiMWPGaPz48afU33TTTSoqKtKSJUs823r06KGOHTtqzpw5lT5HcXGxiouLPY8dDodSUlI0fvx4xcTEnFLfsmVL3XLLLZ7HU6ZM8foha9asmYYPH+55PG3aNB07dqzS2uTkZI0cOdLzePr06SooKKi09qefGmj+/D95Ht9yyyzVq3ew0lqHw65XXhnreXzjjS8pMXFfpbW//FJL//jHA57Hv/99lpo02VVpbWlppObMmeh5fM0189W8+bZKayXp+ecne9b791+oli2/8Vo7e/YEnThhfHAyMhbr4ou/8Fr70kt/1vHjtSVJvXu/r/btN3qtzcq6V4WFdSRJvXr9V507r/Va+/rrd+vwYeMPO927r1Ba2kqvtQsW3KEDBxpLkjp1+lSXXPKx19q33x6mvXubS5LatVuvPn0+9Fr73ntDtHPnhZKkVq1ydOWV73it/fDDG7R9extJ0gUXfK0BA/7jtXbZsmv13XcdJUnNm2/VNde84bV2xYoB+vJL48iSxo136rrrXvFau3p1hj7/vJckqWHDvbrppn94rV23rrfWr+8jSapb94BuvXW219rNm9P16ad9JUnx8Uc0fPhzXmu3bOmqlSsHSpJiYoo0cuQzXmu//baDPv54kCQpIqJEd9+d6bV227bWWrp0sOfxmDGPeq3dubOl3nuvfIy4664pioysfIz48cdmWrRouOfxHXdMU2xs5WNEfn6y/v3v8jFi2LDpSkgInjFixozyMWLAgKrHiFmzKo4RrVtXPUb88osxRvTpU/UY8fLL5WPEJZf8V126eB8jXnutfIxIS1uhHj28jxFvvHGH8vONMaJLl0/1u995HyMWLhymH39sLknq0GG9Lr/c+xixePEQ/fCDMUa0bp2j/v29jxHvvXeDtm41xogLL/xa11zjfYz44INr9fXXHSVJqalbdcMN3seIZcsGaPNmY4xISdmpW27xPkYsX56hzz4zxoikpL364x+9jxErV/bWqlV9JEn16x/Q3Xd7HyM+/TRdy5YZY0SdOkc0dqz3MWL9+q764ANjjKhVq0gPPuh9jMjJ6aDFiwdJkiIjS/SXv3gfI77+urUWLiwfI/76V+9jxNatLTV/fvkYMXHiFEVFVT5G7NzZTFlZwz2PH3hgmmrXrnyM2Ls3WS+9VD5GjB07XXXqVD5GHDjQQLNmlY8Rf/rTLDVsWPkYceSIXdOnj/U8HjnyJTVuXPkYUVRUS9OmlY8Rw4dnqXnzyseIkpJITZlSPkbccst8XXih9zHir3+drIgI41zm3/++6jHiiy8mKDo6SrGxUsOGixUb632M6Nz5z6pTp7Zq15a+/fZ9bd/ufYy49957VadOHUnSf//7X61d632MuPvuuz0TRCtWrNDKld7HiDvuuEONGxtjxKeffqqPP/Y+RgwbNkzNmzeXJK1fv14ffuh9jBgyZIguvNAYI3JycvTOO97HiBtuuEFt2hhjxNdff63//Mf7GHHttdeqY8eOkqStW7fqjTe8jxEDBgzwHKG6c+dOvfKK9zEiIyNDvXoZY8TevXv1j394HyN69+6tPn36SJIOHDig2bO9jxHp6enq29cYI44cOaLnnvM+RnTt2lUDBxpjRFFRkZ55xvsY0aFDBw0aNEiSEdYyM72PEa1bt9bgweVjxKOPeh8jAiFrNGjQQH/6U/kYMWvWLB08WPkYYbfbNXbsWM/jl156Sfv2VT5G1KpVSw88UD5GZGVladeuyseIyMhITZxYPkbMnz9f27Z5HyMmTy7/PWLhwoX65hvvY8SECRM8IX3x4sX64gtjjDh+/LieeuopFRQUKCEhwev+QTXTXVJSok2bNmnChAmebWFhYcrIyPA6yK1du1bjxo2rsK1fv35avHix1+fJzMys8o0fiJxO4wIcJz/2xuWqWFvVIVNnUut2V6w93R+GzqT28GHjr9yScahYVX7+WSobW44fr7q27BwryTi07HS1ZT2frragoLzWyzjnUXZRFck4hM1XtYWF5bWJiVXXFhWV19atW/3a+Piqa48dK6+Njq669pdfymtPdxjdybVVvdcl4z1QVlurVtW1xcXltSf9gbZSJSUV38NnUlvVn0JPnKhYW9UtXs6ktiaOESdfQOh0tT//XD5GVKe2umOEw1E+RlSn9uefq1dbWFj9saeoyBhTqlvrcFSvh19+Mfqozs8tKSkfc043BpeUlP+80/23OHGivOZ0h++63eXv26re64HGZjv9ebbh4casaVltVfVRUcb5u2V1Vd0vOTZWatvWqAkPN/b1pnZtqW9foy4srOrxPS5OGjmy/Oeed17VP3faNGNMjYw03sve3hcxMdLy5UZdVJS0aVPVV+V2Osv/f7FwoVTF79N6883y1794sfSF98ytyy83+paM23ABwOkE1Uz3vn371LhxY61Zs0bp6eme7Q8++KBWrlypdevWnbJPVFSUXnnlFQ0ZMsSzbdasWXr00UeVn59f6fN4m+k+ePBgpX/hsPqQjx9/lI4etSkiorz2xIlSSd7+01esdTqrPowjIiLqLGtPyO32/pvRmdSGh5982KY5tS6XUy6X9yRyZrURstnCfF4bFnbyoV5nUuuSy+X9N9qwsJMPxax+rdvtktNpRq1bTmdVh2RVv/bkQ0fNqpWkEyeq+ixXXVvxF+zffparSi1n/7n3XW3Fz/KZ1JoxRthsZo4nTknVGyOcTqfc7sprbbaKn8+qaqVTP/fVqbXZqh4jftvD6T73FQ/Xrry27H1cVmv0UP65/22QNHo4+RDPimPEyfUn19psFT+fv60r66Gq2pO/2mxhiows+3wah4z/tq5sPTzc+P+9sd396/u9YlguD8McOlomkA8d9WUtp6BweDmHl595rRTYY4TD4VCDBg1Ca6bbX6KjoxVdybRcVFRUhf943lSn5mxqI71MvbVoUWl1tX+uebVn8vYLhNrwX5dgrA2TVN33WiDU2mpYrQKkNhA+94wRNbM2ED73wT1GmPW7gS9+jwjU2pNDRk2oPTnIBVttWFhYtd9rgVBrs9lqVK3EGHE2tf743Ff7/Vntn14D1K9fX+Hh4afMUOfn5yspKanSfZKSks6oHgAAAACA6gqq0B0VFaUuXbooOzvbs83lcik7O7vC4eYnS09Pr1AvScuWLfNaDwAAAABAdQXd4eXjxo3TsGHD1LVrV3Xv3l3Tp09XUVGRRowYIUkaOnSoGjdu7Lla4L333qvevXvr2Wef1cCBA/Xmm29q48aNmjt3rpUvAwAAAAAQBIIudN900006ePCgJk2apLy8PHXs2FFLly5V4q+XZ969e7fnog+S1LNnT82fP18PP/ywJk6cqJYtW2rx4sVnfI9uAAAAAAB+K6iuXm4Vh8Mhu91+2qvWAQAAAACCQ3VzYFCd0w0AAAAAQCAhdAMAAAAAYBJCNwAAAAAAJgm6C6lZoey0eIfDYXEnAAAAAAB/KMt/p7tMGqHbBwoLCyVJKSkpFncCAAAAAPCnwsJC2e12r9/n6uU+4HK5tG/fPsXHx8tms1ndzikcDodSUlK0Z88erq6OgMB7EoGG9yQCDe9JBBrekwg0gfCedLvdKiwsVHJycoXbUv8WM90+EBYWpiZNmljdxmklJCQwSCKg8J5EoOE9iUDDexKBhvckAo3V78mqZrjLcCE1AAAAAABMQugGAAAAAMAkhO4QEB0drcmTJys6OtrqVgBJvCcReHhPItDwnkSg4T2JQFOT3pNcSA0AAAAAAJMw0w0AAAAAgEkI3QAAAAAAmITQDQAAAACASQjdAAAAAACYhNANAAAAAIBJCN0hYObMmWrevLliYmKUlpam9evXW90SQlRmZqa6deum+Ph4NWzYUIMGDVJubq7VbQEeTz31lGw2m8aOHWt1Kwhhe/fu1R/+8AfVq1dPsbGxateunTZu3Gh1WwhRTqdTjzzyiFJTUxUbG6vzzz9fjz/+uLgBEvxl1apVuuaaa5ScnCybzabFixdX+L7b7dakSZPUqFEjxcbGKiMjQ9u2bbOmWS8I3UFuwYIFGjdunCZPnqzNmzerQ4cO6tevnw4cOGB1awhBK1eu1KhRo/TZZ59p2bJlKi0tVd++fVVUVGR1a4A2bNigF198Ue3bt7e6FYSwn3/+Wb169VJkZKQ+/PBDffPNN3r22Wd13nnnWd0aQtTUqVM1e/ZsvfDCC/r22281depUPf3003r++eetbg0hoqioSB06dNDMmTMr/f7TTz+tGTNmaM6cOVq3bp1q166tfv366fjx437u1Dvu0x3k0tLS1K1bN73wwguSJJfLpZSUFI0ZM0bjx4+3uDuEuoMHD6phw4ZauXKlLr30UqvbQQg7evSoOnfurFmzZumJJ55Qx44dNX36dKvbQggaP368Pv30U/3vf/+zuhVAknT11VcrMTFR//znPz3brr/+esXGxupf//qXhZ0hFNlsNi1atEiDBg2SZMxyJycn6/7779ef//xnSVJBQYESExOVlZWlm2++2cJuyzHTHcRKSkq0adMmZWRkeLaFhYUpIyNDa9eutbAzwFBQUCBJqlu3rsWdINSNGjVKAwcOrDBeAlZ499131bVrVw0ePFgNGzZUp06d9NJLL1ndFkJYz549lZ2dra1bt0qSvvjiC61evVoDBgywuDNA2rFjh/Ly8ir8/9tutystLS2g8k6E1Q3APIcOHZLT6VRiYmKF7YmJifruu+8s6gowuFwujR07Vr169VLbtm2tbgch7M0339TmzZu1YcMGq1sB9MMPP2j27NkaN26cJk6cqA0bNuiee+5RVFSUhg0bZnV7CEHjx4+Xw+FQq1atFB4eLqfTqSeffFK33nqr1a0BysvLk6RK807Z9wIBoRuAJUaNGqWvvvpKq1evtroVhLA9e/bo3nvv1bJlyxQTE2N1O4BcLpe6du2qKVOmSJI6deqkr776SnPmzCF0wxL//ve/9frrr2v+/Plq06aNcnJyNHbsWCUnJ/OeBKqJw8uDWP369RUeHq78/PwK2/Pz85WUlGRRV4A0evRoLVmyRJ988omaNGlidTsIYZs2bdKBAwfUuXNnRUREKCIiQitXrtSMGTMUEREhp9NpdYsIMY0aNVLr1q0rbLv44ou1e/duizpCqHvggQc0fvx43XzzzWrXrp1uu+023XfffcrMzLS6NcCTaQI97xC6g1hUVJS6dOmi7OxszzaXy6Xs7Gylp6db2BlCldvt1ujRo7Vo0SItX75cqampVreEEHfFFVfoyy+/VE5Ojmfp2rWrbr31VuXk5Cg8PNzqFhFievXqdcqtFLdu3apmzZpZ1BFC3bFjxxQWVjEyhIeHy+VyWdQRUC41NVVJSUkV8o7D4dC6desCKu9weHmQGzdunIYNG6auXbuqe/fumj59uoqKijRixAirW0MIGjVqlObPn6933nlH8fHxnnNt7Ha7YmNjLe4OoSg+Pv6UawrUrl1b9erV41oDsMR9992nnj17asqUKbrxxhu1fv16zZ07V3PnzrW6NYSoa665Rk8++aSaNm2qNm3a6PPPP9ff/vY3/fGPf7S6NYSIo0ePavv27Z7HO3bsUE5OjurWraumTZtq7NixeuKJJ9SyZUulpqbqkUceUXJysucK54GAW4aFgBdeeEHTpk1TXl6eOnbsqBkzZigtLc3qthCCbDZbpdvnzZun4cOH+7cZwIs+ffpwyzBYasmSJZowYYK2bdum1NRUjRs3TiNHjrS6LYSowsJCPfLII1q0aJEOHDig5ORkDRkyRJMmTVJUVJTV7SEErFixQpdddtkp24cNG6asrCy53W5NnjxZc+fO1ZEjR3TJJZdo1qxZuvDCCy3otnKEbgAAAAAATMI53QAAAAAAmITQDQAAAACASQjdAAAAAACYhNANAAAAAIBJCN0AAAAAAJiE0A0AAAAAgEkI3QAAAAAAmITQDQAAAACASQjdAACEoOHDh2vQoEF+f96srCzZbDbZbDaNHTu2WvsMHz7cs8/ixYtN7Q8AAF+LsLoBAADgWzabrcrvT548Wc8995zcbrefOqooISFBubm5ql27drXqn3vuOT311FNq1KiRyZ0BAOB7hG4AAILM/v37PesLFizQpEmTlJub69kWFxenuLg4K1qTZPxRICkpqdr1drtddrvdxI4AADAPh5cDABBkkpKSPIvdbveE3LIlLi7ulMPL+/TpozFjxmjs2LE677zzlJiYqJdeeklFRUUaMWKE4uPjdcEFF+jDDz+s8FxfffWVBgwYoLi4OCUmJuq2227ToUOHzrjnWbNmqWXLloqJiVFiYqJuuOGGc/1nAAAgIBC6AQCAJOmVV15R/fr1tX79eo0ZM0Z33323Bg8erJ49e2rz5s3q27evbrvtNh07dkySdOTIEV1++eXq1KmTNm7cqKVLlyo/P1833njjGT3vxo0bdc899+ixxx5Tbm6uli5dqksvvdSMlwgAgN9xeDkAAJAkdejQQQ8//LAkacKECXrqqadUv359jRw5UpI0adIkzZ49W1u2bFGPHj30wgsvqFOnTpoyZYrnZ7z88stKSUnR1q1bdeGFF1breXfv3q3atWvr6quvVnx8vJo1a6ZOnTr5/gUCAGABZroBAIAkqX379p718PBw1atXT+3atfNsS0xMlCQdOHBAkvTFF1/ok08+8ZwjHhcXp1atWkmSvv/++2o/75VXXqlmzZqpRYsWuu222/T66697ZtMBAKjpCN0AAECSFBkZWeGxzWarsK3squgul0uSdPToUV1zzTXKycmpsGzbtu2MDg+Pj4/X5s2b9cYbb6hRo0aaNGmSOnTooCNHjpz7iwIAwGIcXg4AAM5K586d9dZbb6l58+aKiDi3XykiIiKUkZGhjIwMTZ48WXXq1NHy5ct13XXX+ahbAACswUw3AAA4K6NGjdLhw4c1ZMgQbdiwQd9//70++ugjjRgxQk6ns9o/Z8mSJZoxY4ZycnK0a9cuvfrqq3K5XLroootM7B4AAP8gdAMAgLOSnJysTz/9VE6nU3379lW7du00duxY1alTR2Fh1f8Vo06dOnr77bd1+eWX6+KLL9acOXP0xhtvqE2bNiZ2DwCAf9jcbrfb6iYAAEBoyMrK0tixY8/qfG2bzaZFixZVuL84AACBjpluAADgVwUFBYqLi9NDDz1Urfq77rpLcXFxJncFAIA5mOkGAAB+U1hYqPz8fEnGYeX169c/7T4HDhyQw+GQJDVq1Ei1a9c2tUcAAHyJ0A0AAAAAgEk4vBwAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTRFjdQDBwuVzat2+f4uPjZbPZrG4HAAAAAGAyt9utwsJCJScnKyzM+3w2odsH9u3bp5SUFKvbAAAAAAD42Z49e9SkSROv3yd0+0B8fLwk4x87ISHB4m4AAAAAAGZzOBxKSUnx5EFvCN0+UHZIeUJCAqEbAAAAAELI6U4x5kJqAAAAAACYhNANAAAAAIBJCN0AAAAAAJiE0A0AAAAAgEkI3QAAAAAAmITQDQAAAACASYIydM+cOVPNmzdXTEyM0tLStH79+irrFy5cqFatWikmJkbt2rXTBx984KdOAQAAAADBLOju071gwQKNGzdOc+bMUVpamqZPn65+/fopNzdXDRs2PKV+zZo1GjJkiDIzM3X11Vdr/vz5GjRokDZv3qy2bdue0XOXlJSopKTklO1hYWGKiIioUOeNzWZTZGTkWdWWlpbK7Xb7tVaSoqKizqr2xIkTcrlcPqmNjIz03B/PrFqn0ymn0+mT2oiICIWFhQVMrcvl0okTJ7zWhoeHKzw8PGBq3W63SktLfVJ78ufTrFqp6s8yY0TltYwRjBGMEWdeyxhxdrWMEedWGwife8YIxojf1vpjjKjq3+9kNndVXddAaWlp6tatm1544QVJxgcwJSVFY8aM0fjx40+pv+mmm1RUVKQlS5Z4tvXo0UMdO3bUnDlzKn2O4uJiFRcXex47HA6lpKRo/PjxiomJOaW+ZcuWuuWWWzyPp0yZ4vVD1qxZMw0fPtzzeNq0aTp27FiltcnJyRo5cqTn8fTp01VQUFBpbYMGDfSnP/3J83jWrFk6ePBgpbV2u11jx471PH7ppZe0b9++Smtr1aqlBx54wPM4KytLu3btqrQ2MjJSEydO9DyeP3++tm3bVmmtJE2ePNmzvnDhQn3zzTdeaydMmOD54CxevFhffPGF19o///nPql27tiTp/fff18aNG73WXnHFvQoLq6PiYmnbtv9q//61XmvPO+9u2WwN5XRKv/yyQiUlK73Wut13yO1uLOPT96nCwz/2WltcPEwuV3O53VJExHpFRX3otfaXX4bI6bzw19ocxca+47X22LEbdOJEG0lSRMTXqlXrP1X83GtVWtrx19qtqlXrjSpqB6i0tLskKTx8p2rXfsVr7fHjGSop6SVJCgvbq7i4f3itLS7ureLiPr/WHlBc3OwqatNVXNxXkmSzHVF8/HNea0tKuur48YG/1hYpPv6ZKmo76PjxQWWPlJCQ6bW2tLS1fvllsOdxQsKjVdS21C+/lI8R8fFTZLNVPkacONFMx44N9zyOi5umsLDKxwinM1lFRSNPqp2usLDKxwins4GKisrHiNq1Zyk8vPIxwuWy6+jRsSfVvqTw8MrHCJerlo4eLR8jatXKUkRE5WOE2x2pwsLyMSI2dr4iI72PEQ7H5JNqFyoy0vsY4XBMkGSMETExixUV5X2MKCz8s9zu2r/Wvq+oKO9jRGHhvXK760iSoqP/q+ho72PE0aN3y+Vq+GvtCkVHex8jjh69Qy5XY0lSVNSnionxPkYUFQ2T09lckhQZuV6xsd7HiGPHhujEiQt/rWWMkBgjGCPKahkjJMaI8lrGCClwxog77nhAKSnG40DLGsePH9dTTz2lgoICJSQkeN0/qGa6S0pKtGnTJk2YMMGzLSwsTBkZGVq7tvJBbu3atRo3blyFbf369dPixYu9Pk9mZqYefdT7Gx+Bbfhw6eefpYIC6cILjcWba66Rjhwx1q+8UurVy3vtE09IZX/H6NPHWLx56SWp7O8YPXtKfft6r33jDWnnTmO9Wzdp4EDvtW+/LZWNLR07SoMGea9dskQqG1tat5ZuvNF77UcfSTk5xnrLltKtt3qvXb5c2rDBWG/e3Pj39mbVKmnNGmM9OVm6807vtWvXSitWGOsNGkijRnmv3bhRWrbMWK9TRzrpb0inyMmRys4oqVVLevBB77XffCOVDQ2RkdJf/uK9dutWaeHC8sd//av32h07pPnzyx9PnCid9MfXCn78UcrKKn/8wAPSr39DOkVenvFeKzN2rPHvUZmffpJmzSp//Kc/SZUcHCRJcjik6dPLH48cKTVuXHntL79If/tb+ePhw433RWVKSyvW3nJL1Z/Pk2sHD5batPFe+/zzxs+XjM9Fx47ea2fPlsr+1nnVVVL37t5r//nP6o8Rr7xS/TFi/vzqjxELF1Z/jFi0iDFCYowowxhRjjHCwBhhYIwwBMoYkZ8vT+iuqYJqpnvfvn1q3Lix1qxZo/T0dM/2Bx98UCtXrtS6detO2ScqKkqvvPKKhgwZ4tk2a9YsPfroo8rPz6/0ebzNdB88eLDSv3BwyEfltb465GP/fmndukitXWvTN99IP/54Qvv2uVRUVPnPLS2NlGQcHhIefkJhYRV/bmSkFB1tLOHhkYqOtikmRoqJcSo21qnISCk8XIqIKP8aESHZbJGKiLApLEwKD3cqPNypsDDJZpPCwsoX48iUCNlsYbLZJJvNKcn563r5IlWsNR6X1/5WWW3ZpRrKar0rr5VckioeklXxOcJ/XSqvrcgftW5J3g/JOrPaMJX//dGsWkmq6vCjM6m1SYo86fGZ1JbK6NuftVLZ7NGZ156Q8b7wRW3559682tN95s6k9uTPZyDUBsLnnjGierWMEWdXyxhxbrWB8LlnjKhebc0bI0aPjlKTJr9WBtjh5Q6HQw0aNAitmW5/iY6OVnR09Cnbo6KiKvzH86Y6NWdTe3JQrgm1J/8horq1J05IX35p/FVzzRrp00+lU48wKf+59eoZfxlr2tT42rix8Rc6u91YEhIiTlo3lvDw3/68MicPwKdT02rDVPGXk0CvtdWwWgVIbfU/n4FReyb/iwqE2kD4LDNGGALhc88YYX5tIHzuGSMMgfC5Z4wwv9b6z73ZWeNMa6ub1YIqdNevX1/h4eGnzFDn5+crKSmp0n2SkpLOqB7+53ZLH38szZhhHBZ09GjF74eFSe3bG4dXde4sNWtmhOwmTYzDfAAAAADAKkEVuqOiotSlSxdlZ2dr0K8nobhcLmVnZ2v06NGV7pOenq7s7OwKFw5btmxZhcPTYY3SUmnBAumZZ6STr4uWkCD16GGcF9Wzp3EuVRVHcwAAAACAZYIqdEvSuHHjNGzYMHXt2lXdu3fX9OnTVVRUpBEjRkiShg4dqsaNGysz07ha4L333qvevXvr2Wef1cCBA/Xmm29q48aNmjt3rpUvI6QVFhoXbZg+Xdqzx9hWq5Z0xx3SH/8otW1b1SHgAAAAABA4gi5033TTTTp48KAmTZqkvLw8dezYUUuXLlViYqIkaffu3Z57CkpSz549NX/+fD388MOaOHGiWrZsqcWLF5/xPbpx7vbtMw4hnzPHuLK4JCUmSmPGSHffLdWta21/AAAAAHCmgurq5VZxOByy2+2nvWodvMvKkv7f/5PKLtZ+0UXSn/8s/eEPUiW3PgcAAAAAS1U3BwbdTDdqFrdbmjZNeugh43HPnsb61VcbF0gDAAAAgJqM0A3LuFzSgw9Kzz5rPH7wQempp357f2gAAAAAqLkI3bBEaal0++3Sa68Zj595Rrr/fmt7AgAAAABfI3TD744dkwYPlj74wLgK+csvS0OHWt0VAAAAAPgeoRt+dfiwcb722rVSbKy0cKE0cKDVXQEAAACAOQjd8Jsff5T69ZO++UY67zxpyRLjwmkAAAAAEKwI3fCLbdukjAxp926pcWPpo4+kNm2s7goAAAAAzMVNmWA6l0u69VYjcF90kfTppwRuAAAAAKGBmW6Y7t//ljZskOLipE8+kRo1srojAAAAAPAPZrphquJiaeJEY/2hhwjcAAAAAEILoRummj1b2rHDCNv33Wd1NwAAAADgX4RumObIEenxx431xx6Tate2tB0AAAAA8DtCN0yTmWncl7tNG2n4cKu7AQAAAAD/I3TDFLt3S889Z6xPnSpFcMk+AAAAACGI0A1TPPKIcRG1Pn2kq66yuhsAAAAAsAahGz6XkyO99pqxPm2aZLNZ2g4AAAAAWIbQDZ978EHJ7ZaGDJG6drW6GwAAAACwDqEbPvXf/0rLlklRUdKTT1rdDQAAAABYi9ANn3E6jVluSRo9WkpNtbYfAAAAALAaoRs+869/SV98IdWpI/3lL1Z3AwAAAADWI3TDJ375RXr4YWN94kSpbl1r+wEAAACAQEDohk/MmCH9+KPUtKk0ZozV3QAAAABAYCB045w5ndIzzxjrTzwhxcRY2w8AAAAABApCN87Zpk3SoUOS3W7cJgwAAAAAYCB045z997/G1yuukCIirO0FAAAAAAIJoRvnrCx09+1rbR8AAAAAEGgI3TgnDoe0dq2xTugGAAAAgIoI3TgnK1ZIJ05IF1wgpaZa3Q0AAAAABBZCN84Jh5YDAAAAgHeEbpyTjz4yvvbrZ20fAAAAABCICN04az/8IG3fblyxvE8fq7sBAAAAgMBD6MZZW7bM+JqeLiUkWNsLAAAAAAQiQjfOGudzAwAAAEDVCN04KydOSNnZxjqhGwAAAAAqR+jGWdmwQSookM47T+rSxepuAAAAACAwEbpxVsoOLc/IkMLDre0FAAAAAAIVoRtnpexWYRxaDgAAAADeBVXoPnz4sG699VYlJCSoTp06uv3223X06NEq9+nTp49sNluF5a677vJTxzXTkSPSunXGOqEbAAAAALyLsLoBX7r11lu1f/9+LVu2TKWlpRoxYoTuvPNOzZ8/v8r9Ro4cqccee8zzuFatWma3WqMtXy65XFKrVlLTplZ3AwAAAACBK2hC97fffqulS5dqw4YN6tq1qyTp+eef11VXXaVnnnlGycnJXvetVauWkpKS/NVqjcetwgAAAACgeoLm8PK1a9eqTp06nsAtSRkZGQoLC9O6smOhvXj99ddVv359tW3bVhMmTNCxY8eqrC8uLpbD4aiwhAq3m/O5AQAAAKC6gmamOy8vTw0bNqywLSIiQnXr1lVeXp7X/W655RY1a9ZMycnJ2rJlix566CHl5ubq7bff9rpPZmamHn30UZ/1XpN8/720c6cUGSn17m11NwAAAAAQ2AI+dI8fP15Tp06tsubbb789659/5513etbbtWunRo0a6YorrtD333+v888/v9J9JkyYoHHjxnkeOxwOpaSknHUPNUnZoeW9eklxcdb2AgAAAACBLuBD9/3336/hw4dXWdOiRQslJSXpwIEDFbafOHFChw8fPqPztdPS0iRJ27dv9xq6o6OjFR0dXe2fGUw4nxsAAAAAqi/gQ3eDBg3UoEGD09alp6fryJEj2rRpk7p06SJJWr58uVwulydIV0dOTo4kqVGjRmfVbzArLTWuXC5J/fpZ2wsAAAAA1ARBcyG1iy++WP3799fIkSO1fv16ffrppxo9erRuvvlmz5XL9+7dq1atWmn9+vWSpO+//16PP/64Nm3apJ07d+rdd9/V0KFDdemll6p9+/ZWvpyA9NlnUmGhVL++1LGj1d0AAAAAQOALmtAtGVchb9Wqla644gpdddVVuuSSSzR37lzP90tLS5Wbm+u5OnlUVJQ+/vhj9e3bV61atdL999+v66+/Xu+9955VLyGglR1afuWVUlhQvXMAAAAAwBw2t9vttrqJms7hcMhut6ugoEAJCQlWt2OatDRp/Xpp3jzpNKfZAwAAAEBQq24OZL4S1XL4sLRhg7F+5ZXW9gIAAAAANQWhG9WSnS253VKbNlLjxlZ3AwAAAAA1A6Eb1cKtwgAAAADgzBG6cVput/TRR8Y6oRsAAAAAqo/QjdP6/ntpzx4pKkq69FKruwEAAACAmoPQjdPKzTW+XnyxVKuWtb0AAAAAQE1C6MZpff+98fX8863tAwAAAABqGkI3Tmv7duMroRsAAAAAzgyhG6fFTDcAAAAAnB1CN06L0A0AAAAAZ4fQjSo5ndKOHcY6oRsAAAAAzgyhG1Xau1cqKZEiI6WUFKu7AQAAAICahdCNKpUdWt68uRQRYWkrAAAAAFDjELpRJc7nBgAAAICzR+hGlQjdAAAAAHD2CN2oEqEbAAAAAM4eoRtVInQDAAAAwNkjdMMrt5vQDQAAAADngtANrw4flgoKjPUWLaztBQAAAABqIkI3vCqb5U5OlmJjre0FAAAAAGoiQje84tByAAAAADg3hG54RegGAAAAgHND6IZXhG4AAAAAODeEbni1fbvxldANAAAAAGeH0A2vmOkGAAAAgHND6Ealjh2T9u831gndAAAAAHB2CN2o1A8/GF/tdqluXWt7AQAAAICaitCNSp18aLnNZm0vAAAAAFBTEbpRqbLQfcEF1vYBAAAAADUZoRuV4iJqAAAAAHDuCN2oFKEbAAAAAM4doRuVInQDAAAAwLkjdOMUJ05IO3ca64RuAAAAADh7hG6cYs8eI3hHR0uNG1vdDQAAAADUXIRunKLs0PLUVCmMdwgAAAAAnDUiFU7B+dwAAAAA4BuEbpyC0A0AAAAAvhFUofvJJ59Uz549VatWLdWpU6da+7jdbk2aNEmNGjVSbGysMjIytG3bNnMbDXCEbgAAAADwjaAK3SUlJRo8eLDuvvvuau/z9NNPa8aMGZozZ47WrVun2rVrq1+/fjp+/LiJnQY2QjcAAAAA+EaE1Q340qOPPipJysrKqla92+3W9OnT9fDDD+vaa6+VJL366qtKTEzU4sWLdfPNN5vVasByu6Xt2411QjcAAAAAnJugmuk+Uzt27FBeXp4yMjI82+x2u9LS0rR27Vqv+xUXF8vhcFRYgsWBA1JRkWSzGVcvBwAAAACcvZAO3Xl5eZKkxMTECtsTExM936tMZmam7Ha7Z0lJSTG1T38qO7S8SRPjPt0AAAAAgLMX8KF7/PjxstlsVS7fffedX3uaMGGCCgoKPMuePXv8+vxm4nxuAAAAAPCdgD+n+/7779fw4cOrrGnRosVZ/eykpCRJUn5+vho1auTZnp+fr44dO3rdLzo6WtFBOg1M6AYAAAAA3wn40N2gQQM1aNDAlJ+dmpqqpKQkZWdne0K2w+HQunXrzugK6MGkLHRfcIG1fQAAAABAMAj4w8vPxO7du5WTk6Pdu3fL6XQqJydHOTk5Onr0qKemVatWWrRokSTJZrNp7NixeuKJJ/Tuu+/qyy+/1NChQ5WcnKxBgwZZ9CqsxUw3AAAAAPhOwM90n4lJkybplVde8Tzu1KmTJOmTTz5Rnz59JEm5ubkqKCjw1Dz44IMqKirSnXfeqSNHjuiSSy7R0qVLFRMT49feAwWhGwAAAAB8x+Z2u91WN1HTORwO2e12FRQUKCEhwep2zlphoVTW/pEjkt1uaTsAAAAAELCqmwOD6vBynJsffjC+1qtH4AYAAAAAXyB0w4NDywEAAADAtwjd8CB0AwAAAIBvEbrhQegGAAAAAN8idMOD0A0AAAAAvkXohgehGwAAAAB8i9ANSVJJibRrl7FO6AYAAAAA3yB0Q5IRuF0uKTZWatTI6m4AAAAAIDgQuiGp/NDyFi0km83aXgAAAAAgWBC6IYnzuQEAAADADIRuSCJ0AwAAAIAZCN2QROgGAAAAADOYFrp/+eUX7d2795TtX3/9tVlPiXNA6AYAAAAA3zMldP/nP/9Ry5YtNXDgQLVv317r1q3zfO+2224z4ylxDtxu6YcfjPULLrC2FwAAAAAIJqaE7ieeeEKbNm1STk6O5s2bp9tvv13z58+XJLndbjOeEudg/37pl1+k8HCpWTOruwEAAACA4BFhxg8tLS1VYmKiJKlLly5atWqVfv/732v79u2ycT+qgFN2aHnTplJkpLW9AAAAAEAwMWWmu2HDhtqyZYvncd26dbVs2TJ9++23FbYjMHA+NwAAAACYw6ehu7CwUJL02muvqWHDhhW+FxUVpTfeeEMrV6705VPCB/Lzja+NG1vbBwAAAAAEG5+G7t/97nfKy8tTkyZNlJSUVGlNr169fPmU8IHDh42vdeta2wcAAAAABBufhu5OnTopLS1N3333XYXtOTk5uuqqq3z5VPChn382vp53nrV9AAAAAECw8WnonjdvnoYPH65LLrlEq1ev1tatW3XjjTeqS5cuCg8P9+VTwYeY6QYAAAAAc/j86uWPPvqooqOjdeWVV8rpdOqKK67Q2rVr1b17d18/FXyEmW4AAAAAMIdPZ7rz8/N177336oknnlDr1q0VGRmp4cOHE7gDHDPdAAAAAGAOn4bu1NRUrVq1SgsXLtSmTZv01ltv6c4779S0adN8+TTwsbLQzUw3AAAAAPiWTw8vf/nll3XzzTd7Hvfv31+ffPKJrr76au3cuVMzZ8705dPBR8oOL2emGwAAAAB8y6cz3ScH7jKdO3fWmjVrtHz5cl8+FXyktFT69fbqzHQDAAAAgI/5NHR707x5c61Zs8YfT4UzdORI+XqdOlZ1AQAAAADByS+hW5LOYxo1IJWdz223SxE+v5Y9AAAAAIQ2v4VuBCZuFwYAAAAA5iF0hzhuFwYAAAAA5iF0hzhmugEAAADAPITuEMdMNwAAAACYh9Ad4pjpBgAAAADzELpDHDPdAAAAAGAeQneIY6YbAAAAAMxD6A5xzHQDAAAAgHkI3SGOmW4AAAAAMA+hO8Qx0w0AAAAA5gmq0P3kk0+qZ8+eqlWrlurUqVOtfYYPHy6bzVZh6d+/v7mNBpCy0M1MNwAAAAD4XoTVDfhSSUmJBg8erPT0dP3zn/+s9n79+/fXvHnzPI+jo6PNaC/guN3lh5cz0w0AAAAAvhdUofvRRx+VJGVlZZ3RftHR0UpKSqp2fXFxsYqLiz2PHQ7HGT1foDh2TCopMdaZ6QYAAAAA3wuqw8vP1ooVK9SwYUNddNFFuvvuu/XTTz9VWZ+ZmSm73e5ZUlJS/NSpb5XNckdESHFx1vYCAAAAAMEo5EN3//799eqrryo7O1tTp07VypUrNWDAADmdTq/7TJgwQQUFBZ5lz549fuzYd06+iJrNZm0vAAAAABCMAv7w8vHjx2vq1KlV1nz77bdq1arVWf38m2++2bPerl07tW/fXueff75WrFihK664otJ9oqOjg+K8b24XBgAAAADmCvjQff/992v48OFV1rRo0cJnz9eiRQvVr19f27dv9xq6gwW3CwMAAAAAcwV86G7QoIEaNGjgt+f78ccf9dNPP6lRo0Z+e06rMNMNAAAAAOYKqnO6d+/erZycHO3evVtOp1M5OTnKycnR0aNHPTWtWrXSokWLJElHjx7VAw88oM8++0w7d+5Udna2rr32Wl1wwQXq16+fVS/Db5jpBgAAAABzBfxM95mYNGmSXnnlFc/jTp06SZI++eQT9enTR5KUm5urgoICSVJ4eLi2bNmiV155RUeOHFFycrL69u2rxx9/PCjO2T4dZroBAAAAwFxBFbqzsrJOe49ut9vtWY+NjdVHH31kcleBi5luAAAAADBXUB1ejjPDTDcAAAAAmIvQHcKY6QYAAAAAcxG6Q1hZ6GamGwAAAADMQegOYWWHlzPTDQAAAADmIHSHMGa6AQAAAMBchO4Q5XRKv945jZluAAAAADAJoTtEFRRIZXdPY6YbAAAAAMxB6A5RZedz164tRUVZ2wsAAAAABCtCd4jidmEAAAAAYD5Cd4gqm+nm0HIAAAAAMA+hO0Qx0w0AAAAA5iN0hyhmugEAAADAfITuEMVMNwAAAACYj9AdopjpBgAAAADzEbpDFDPdAAAAAGA+QneIYqYbAAAAAMxH6A5RzHQDAAAAgPkI3SGqLHQz0w0AAAAA5iF0h6iyw8uZ6QYAAAAA8xC6QxSHlwMAAACA+QjdIej4cemXX4x1Di8HAAAAAPMQukNQ2aHlYWFSQoK1vQAAAABAMCN0h6Cy0F2njhG8AQAAAADmIHKFIM7nBgAAAAD/IHSHoLKZbs7nBgAAAABzEbpDEDPdAAAAAOAfhO4QxEw3AAAAAPgHoTsEMdMNAAAAAP5B6A5BzHQDAAAAgH8QukMQM90AAAAA4B+E7hDETDcAAAAA+AehOwQx0w0AAAAA/kHoDkGEbgAAAADwD0J3COLwcgAAAADwD0J3iHG7y0M3M90AAAAAYC5Cd4gpLJScTmOdmW4AAAAAMBehO8SUnc8dEyPFxlrbCwAAAAAEu6AJ3Tt37tTtt9+u1NRUxcbG6vzzz9fkyZNVUlJS5X7Hjx/XqFGjVK9ePcXFxen6669Xfn6+n7r2P87nBgAAAAD/CZrQ/d1338nlcunFF1/U119/rb///e+aM2eOJk6cWOV+9913n9577z0tXLhQK1eu1L59+3Tdddf5qWv/48rlAAAAAOA/Nrfb7ba6CbNMmzZNs2fP1g8//FDp9wsKCtSgQQPNnz9fN9xwgyQjvF988cVau3atevToUel+xcXFKi4u9jx2OBxKSUlRQUGBEhISfP9CfOg//5EGD5YuuUT63/+s7gYAAAAAaiaHwyG73X7aHBg0M92VKSgoUN0qpnQ3bdqk0tJSZWRkeLa1atVKTZs21dq1a73ul5mZKbvd7llSUlJ82reZmOkGAAAAAP8J2tC9fft2Pf/88/p//+//ea3Jy8tTVFSU6tSpU2F7YmKi8vLyvO43YcIEFRQUeJY9e/b4qm3TcU43AAAAAPhPwIfu8ePHy2azVbl89913FfbZu3ev+vfvr8GDB2vkyJE+7yk6OloJCQkVlpqCmW4AAAAA8J8Iqxs4nfvvv1/Dhw+vsqZFixae9X379umyyy5Tz549NXfu3Cr3S0pKUklJiY4cOVJhtjs/P19JSUnn0nbAYqYbAAAAAPwn4EN3gwYN1KBBg2rV7t27V5dddpm6dOmiefPmKSys6on8Ll26KDIyUtnZ2br++uslSbm5udq9e7fS09PPufdAxEw3AAAAAPhPwB9eXl179+5Vnz591LRpUz3zzDM6ePCg8vLyKpybvXfvXrVq1Urr16+XJNntdt1+++0aN26cPvnkE23atEkjRoxQenq61yuX13SEbgAAAADwn4Cf6a6uZcuWafv27dq+fbuaNGlS4Xtld0UrLS1Vbm6ujh075vne3//+d4WFhen6669XcXGx+vXrp1mzZvm1d3/i8HIAAAAA8J+gvk+3v1T3/myBoFkzafduad06qXt3q7sBAAAAgJqJ+3SjUsx0AwAAAID/ELpDSGmpVFhorHNONwAAAACYj9AdQo4cKV8/6Q5pAAAAAACTELpDSNmVy+12KTzc2l4AAAAAIBQQukMI53MDAAAAgH8RukMI9+gGAAAAAP8idIcQZroBAAAAwL8I3SGEmW4AAAAA8C9CdwhhphsAAAAA/IvQHUKY6QYAAAAA/yJ0hxBmugEAAADAvwjdIYSZbgAAAADwL0J3CCF0AwAAAIB/EbpDCIeXAwAAAIB/EbpDCDPdAAAAAOBfhO4Q4XYz0w0AAAAA/kboDhHHjkklJcY6M90AAAAA4B+E7hBRNssdESHVrm1tLwAAAAAQKgjdIeLk87ltNmt7AQAAAIBQQegOEZzPDQAAAAD+R+gOEVy5HAAAAAD8j9AdIpjpBgAAAAD/I3SHCGa6AQAAAMD/CN0hgpluAAAAAPA/QneIYKYbAAAAAPyP0B0iyma6Cd0AAAAA4D+E7hBRNtPN4eUAAAAA4D+E7hDB4eUAAAAA4H+E7hDBhdQAAAAAwP8I3SGCmW4AAAAA8D9CdwhwOqWCAmOdmW4AAAAA8B9CdwgoKJDcbmOd0A0AAAAA/kPoDgFl53PXri1FRVnbCwAAAACEEkJ3COB8bgAAAACwBqE7BHDlcgAAAACwBqE7BDDTDQAAAADWIHSHAGa6AQAAAMAaQRO6d+7cqdtvv12pqamKjY3V+eefr8mTJ6ukpKTK/fr06SObzVZhueuuu/zUtX8w0w0AAAAA1oiwugFf+e677+RyufTiiy/qggsu0FdffaWRI0eqqKhIzzzzTJX7jhw5Uo899pjnca1atcxu16/KZroJ3QAAAADgX0ETuvv376/+/ft7Hrdo0UK5ubmaPXv2aUN3rVq1lJSUVO3nKi4uVnFxseexw+E484b9qGymm8PLAQAAAMC/gubw8soUFBSobjWmd19//XXVr19fbdu21YQJE3Ts2LEq6zMzM2W32z1LSkqKr1o2BYeXAwAAAIA1bG632211E2bYvn27unTpomeeeUYjR470Wjd37lw1a9ZMycnJ2rJlix566CF1795db7/9ttd9KpvpTklJUUFBgRISEnz6Onzhgw+kb7+V+vaV2rWzuhsAAAAAqPkcDofsdvtpc2DAh+7x48dr6tSpVdZ8++23atWqlefx3r171bt3b/Xp00f/+Mc/zuj5li9friuuuELbt2/X+eefX619qvuPDQAAAAAIDkETug8ePKiffvqpypoWLVooKipKkrRv3z716dNHPXr0UFZWlsLCzuwI+qKiIsXFxWnp0qXq169ftfYhdAMAAABAaKluDgz4C6k1aNBADRo0qFbt3r17ddlll6lLly6aN2/eGQduScrJyZEkNWrU6Iz3BQAAAADgZEFzIbW9e/eqT58+atq0qZ555hkdPHhQeXl5ysvLq1DTqlUrrV+/XpL0/fff6/HHH9emTZu0c+dOvfvuuxo6dKguvfRStW/f3qqXAgAAAAAIEgE/011dy5Yt0/bt27V9+3Y1adKkwvfKjqAvLS1Vbm6u5+rkUVFR+vjjjzV9+nQVFRUpJSVF119/vR5++GG/9w8AAAAACD4Bf053TcA53QAAAAAQWqqbA4Pm8HIAAAAAAAINoRsAAAAAAJMQugEAAAAAMEnQXEjNSmWnxTscDos7AQAAAAD4Q1n+O91l0gjdPlBYWChJSklJsbgTAAAAAIA/FRYWym63e/0+Vy/3AZfLpX379ik+Pl42m83qdk7hcDiUkpKiPXv2cHV1BATekwg0vCcRaHhPItDwnkSgCYT3pNvtVmFhoZKTkxUW5v3MbWa6fSAsLOyUe4MHooSEBAZJBBTekwg0vCcRaHhPItDwnkSgsfo9WdUMdxkupAYAAAAAgEkI3QAAAAAAmITQHQKio6M1efJkRUdHW90KIIn3JAIP70kEGt6TCDS8JxFoatJ7kgupAQAAAABgEma6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhOwTMnDlTzZs3V0xMjNLS0rR+/XqrW0KIyszMVLdu3RQfH6+GDRtq0KBBys3NtbotwOOpp56SzWbT2LFjrW4FIWzv3r36wx/+oHr16ik2Nlbt2rXTxo0brW4LIcrpdOqRRx5RamqqYmNjdf755+vxxx8XN0CCv6xatUrXXHONkpOTZbPZtHjx4grfd7vdmjRpkho1aqTY2FhlZGRo27Zt1jTrBaE7yC1YsEDjxo3T5MmTtXnzZnXo0EH9+vXTgQMHrG4NIWjlypUaNWqUPvvsMy1btkylpaXq27evioqKrG4N0IYNG/Tiiy+qffv2VreCEPbzzz+rV69eioyM1IcffqhvvvlGzz77rM477zyrW0OImjp1qmbPnq0XXnhB3377raZOnaqnn35azz//vNWtIUQUFRWpQ4cOmjlzZqXff/rppzVjxgzNmTNH69atU+3atdWvXz8dP37cz516x326g1xaWpq6deumF154QZLkcrmUkpKiMWPGaPz48RZ3h1B38OBBNWzYUCtXrtSll15qdTsIYUePHlXnzp01a9YsPfHEE+rYsaOmT59udVsIQePHj9enn36q//3vf1a3AkiSrr76aiUmJuqf//ynZ9v111+v2NhY/etf/7KwM4Qim82mRYsWadCgQZKMWe7k5GTdf//9+vOf/yxJKigoUGJiorKysnTzzTdb2G05ZrqDWElJiTZt2qSMjAzPtrCwMGVkZGjt2rUWdgYYCgoKJEl169a1uBOEulGjRmngwIEVxkvACu+++666du2qwYMHq2HDhurUqZNeeuklq9tCCOvZs6eys7O1detWSdIXX3yh1atXa8CAARZ3Bkg7duxQXl5ehf9/2+12paWlBVTeibC6AZjn0KFDcjqdSkxMrLA9MTFR3333nUVdAQaXy6WxY8eqV69eatu2rdXtIIS9+eab2rx5szZs2GB1K4B++OEHzZ49W+PGjdPEiRO1YcMG3XPPPYqKitKwYcOsbg8haPz48XI4HGrVqpXCw8PldDr15JNP6tZbb7W6NUB5eXmSVGneKfteICB0A7DEqFGj9NVXX2n16tVWt4IQtmfPHt17771atmyZYmJirG4HkMvlUteuXTVlyhRJUqdOnfTVV19pzpw5hG5Y4t///rdef/11zZ8/X23atFFOTo7Gjh2r5ORk3pNANXF4eRCrX7++wsPDlZ+fX2F7fn6+kpKSLOoKkEaPHq0lS5bok08+UZMmTaxuByFs06ZNOnDggDp37qyIiAhFRERo5cqVmjFjhiIiIuR0Oq1uESGmUaNGat26dYVtF198sXbv3m1RRwh1DzzwgMaPH6+bb75Z7dq102233ab77rtPmZmZVrcGeDJNoOcdQncQi4qKUpcuXZSdne3Z5nK5lJ2drfT0dAs7Q6hyu90aPXq0Fi1apOXLlys1NdXqlhDirrjiCn355ZfKycnxLF27dtWtt96qnJwchYeHW90iQkyvXr1OuZXi1q1b1axZM4s6Qqg7duyYwsIqRobw8HC5XC6LOgLKpaamKikpqULecTgcWrduXUDlHQ4vD3Ljxo3TsGHD1LVrV3Xv3l3Tp09XUVGRRowYYXVrCEGjRo3S/Pnz9c477yg+Pt5zro3dbldsbKzF3SEUxcfHn3JNgdq1a6tevXpcawCWuO+++9SzZ09NmTJFN954o9avX6+5c+dq7ty5VreGEHXNNdfoySefVNOmTdWmTRt9/vnn+tvf/qY//vGPVreGEHH06FFt377d83jHjh3KyclR3bp11bRpU40dO1ZPPPGEWrZsqdTUVD3yyCNKTk72XOE8EHDLsBDwwgsvaNq0acrLy1PHjh01Y8YMpaWlWd0WQpDNZqt0+7x58zR8+HD/NgN40adPH24ZBkstWbJEEyZM0LZt25Samqpx48Zp5MiRVreFEFVYWKhHHnlEixYt0oEDB5ScnKwhQ4Zo0qRJioqKsro9hIAVK1bosssuO2X7sGHDlJWVJbfbrcmTJ2vu3Lk6cuSILrnkEs2aNUsXXnihBd1WjtANAAAAAIBJOKcbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAgBA0fPhwDRo0yO/Pm5WVJZvNJpvNprFjx1Zrn+HDh3v2Wbx4san9AQDgaxFWNwAAAHzLZrNV+f3Jkyfrueeek9vt9lNHFSUkJCg3N1e1a9euVv1zzz2np556So0aNTK5MwAAfI/QDQBAkNm/f79nfcGCBZo0aZJyc3M92+Li4hQXF2dFa5KMPwokJSVVu95ut8tut5vYEQAA5uHwcgAAgkxSUpJnsdvtnpBbtsTFxZ1yeHmfPn00ZswYjR07Vuedd54SExP10ksvqaioSCNGjFB8fLwuuOACffjhhxWe66uvvtKAAQMUFxenxMRE3XbbbTp06NAZ9zxr1iy1bNlSMTExSkxM1A033HCu/wwAAAQEQjcAAJAkvfLKK6pfv77Wr1+vMWPG6O6779bgwYPVs2dPbd68WX379tVtt92mY8eOSZKOHDmiyy+/XJ06ddLGjRu1dOlS5efn68Ybbzyj5924caPuuecePfbYY8rNzdXSpUt16aWXmvESAQDwOw4vBwAAkqQOHTro4YcfliRNmDBBTz31lOrXr6+RI0dKkiZNmqTZs2dry5Yt6tGjh1544QV16tRJU6ZM8fyMl19+WSkpKdq6dasuvPDCaj3v7t27Vbt2bV199dWKj49Xs2bN1KlTJ9+/QAAALMBMNwAAkCS1b9/esx4eHq569eqpXbt2nm2JiYmSpAMHDkiSvvjiC33yySeec8Tj4uLUqlUrSdL3339f7ee98sor1axZM7Vo0UK33XabXn/9dc9sOgAANR2hGwAASJIiIyMrPLbZbBW2lV0V3eVySZKOHj2qa665Rjk5ORWWbdu2ndHh4fHx8dq8ebPeeOMNNWrUSJMmTVKHDh105MiRc39RAABYjMPLAQDAWencubPeeustNW/eXBER5/YrRUREhDIyMpSRkaHJkyerTp06Wr58ua677jofdQsAgDWY6QYAAGdl1KhROnz4sIYMGaINGzbo+++/10cffaQRI0bI6XRW++csWbJEM2bMUE5Ojnbt2qVXX31VLpdLF110kYndAwDgH4RuAABwVpKTk/Xpp5/K6XSqb9++ateuncaOHas6deooLKz6v2LUqVNHb7/9ti6//HJdfPHFmjNnjt544w21adPGxO4BAPAPm9vtdlvdBAAACA1ZWVkaO3bsWZ2vbbPZtGjRogr3FwcAINAx0w0AAPyqoKBAcXFxeuihh6pVf9dddykuLs7krgAAMAcz3QAAwG8KCwuVn58vyTisvH79+qfd58CBA3I4HJKkRo0aqXbt2qb2CACALxG6AQAAAAAwCYeXAwAAAABgEkI3AAAAAAAmIXQDAAAAAGASQjcAAAAAACYhdAMAAAAAYBJCNwAAAAAAJiF0AwAAAABgEkI3AAAAAAAmIXQDAAAAAGASQjcAAAAAACYhdAMAAAAAYBJCNwAAAAAAJomwuoFg4HK5tG/fPsXHx8tms1ndDgAAAADAZG63W4WFhUpOTlZYmPf5bEK3D+zbt08pKSlWtwEAAAAA8LM9e/aoSZMmXr9P6PaB+Ph4ScY/dkJCgsXdAAAAAADM5nA4lJKS4smD3hC6faDskPKEhARCNwAAAACEkNOdYsyF1AAAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADAJoRsAAAAAAJMQugEAAAAAMAmhGwAAAAAAkxC6AQAAAAAwCaEbAAAAAACTELoBAAAAADBJjQvdM2fOVPPmzRUTE6O0tDStX7++yvqFCxeqVatWiomJUbt27fTBBx94rb3rrrtks9k0ffp0H3cNAAAAAAhFNSp0L1iwQOPGjdPkyZO1efNmdejQQf369dOBAwcqrV+zZo2GDBmi22+/XZ9//rkGDRqkQYMG6auvvjqldtGiRfrss8+UnJxs9ssAAAAAAISIGhW6//a3v2nkyJEaMWKEWrdurTlz5qhWrVp6+eWXK61/7rnn1L9/fz3wwAO6+OKL9fjjj6tz58564YUXKtTt3btXY8aM0euvv67IyEh/vBQAAAAAQAioMaG7pKREmzZtUkZGhmdbWFiYMjIytHbt2kr3Wbt2bYV6SerXr1+FepfLpdtuu00PPPCA2rRpU61eiouL5XA4KiwAAAAAAPxWjQndhw4dktPpVGJiYoXtiYmJysvLq3SfvLy809ZPnTpVERERuueee6rdS2Zmpux2u2dJSUk5g1cCAAAAAAgVNSZ0m2HTpk167rnnlJWVJZvNVu39JkyYoIKCAs+yZ88eE7sEAAAAANRUNSZ0169fX+Hh4crPz6+wPT8/X0lJSZXuk5SUVGX9//73Px04cEBNmzZVRESEIiIitGvXLt1///1q3ry5116io6OVkJBQYQEAAAAA4LdqTOiOiopSly5dlJ2d7dnmcrmUnZ2t9PT0SvdJT0+vUC9Jy5Yt89Tfdttt2rJli3JycjxLcnKyHnjgAX300UfmvRgAAAAAQEiIsLqBMzFu3DgNGzZMXbt2Vffu3TV9+nQVFRVpxIgRkqShQ4eqcePGyszMlCTde++96t27t5599lkNHDhQb775pjZu3Ki5c+dKkurVq6d69epVeI7IyEglJSXpoosu8u+LAwAAAAAEnRoVum+66SYdPHhQkyZNUl5enjp27KilS5d6Lpa2e/duhYWVT9737NlT8+fP18MPP6yJEyeqZcuWWrx4sdq2bWvVSwAAAAAAhBCb2+12W91ETedwOGS321VQUMD53QAAAAAQAqqbA2vMOd0AAAAAANQ0hG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExS40L3zJkz1bx5c8XExCgtLU3r16+vsn7hwoVq1aqVYmJi1K5dO33wwQee75WWluqhhx5Su3btVLt2bSUnJ2vo0KHat2+f2S8DAAAAABACalToXrBggcaNG6fJkydr8+bN6tChg/r166cDBw5UWr9mzRoNGTJEt99+uz7//HMNGjRIgwYN0ldffSVJOnbsmDZv3qxHHnlEmzdv1ttvv63c3Fz93//9nz9fFgAAAAAgSNncbrfb6iaqKy0tTd26ddMLL7wgSXK5XEpJSdGYMWM0fvz4U+pvuukmFRUVacmSJZ5tPXr0UMeOHTVnzpxKn2PDhg3q3r27du3apaZNm1ZaU1xcrOLiYs9jh8OhlJQUFRQUKCEh4VxeIgAAAACgBnA4HLLb7afNgTVmprukpESbNm1SRkaGZ1tYWJgyMjK0du3aSvdZu3ZthXpJ6tevn9d6SSooKJDNZlOdOnW81mRmZsput3uWlJSUM3sxAAAAAICQUGNC96FDh+R0OpWYmFhhe2JiovLy8irdJy8v74zqjx8/roceekhDhgyp8i8VEyZMUEFBgWfZs2fPGb4aAAAAAEAoiLC6gUBRWlqqG2+8UW63W7Nnz66yNjo6WtHR0X7qDAAAAABQU9WY0F2/fn2Fh4crPz+/wvb8/HwlJSVVuk9SUlK16ssC965du7R8+XLOywYAAAAA+ESNObw8KipKXbp0UXZ2tmeby+VSdna20tPTK90nPT29Qr0kLVu2rEJ9WeDetm2bPv74Y9WrV8+cFwAAAAAACDk1ZqZbksaNG6dhw4apa9eu6t69u6ZPn66ioiKNGDFCkjR06FA1btxYmZmZkqR7771XvXv31rPPPquBAwfqzTff1MaNGzV37lxJRuC+4YYbtHnzZi1ZskROp9NzvnfdunUVFRVlzQsFAAAAAASFGhW6b7rpJh08eFCTJk1SXl6eOnbsqKVLl3oulrZ7926FhZVP3vfs2VPz58/Xww8/rIkTJ6ply5ZavHix2rZtK0nau3ev3n33XUlSx44dKzzXJ598oj59+vjldQEAAAAAglONuk93oKru/dkAAAAAAMEh6O7TDQAAAABATUPoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAAAAAExC6AYAAAAAwCSEbgAAAAAATELoBgAAAADAJIRuAAAAAABMQugGAAAAAMAkhG4AAID/3969B0V1Hm4cfxaQSwSWeGEXIiqmWvGGFxTRTJIKFU104ohJdIhV4+g0gySIaQuJ4jQ/FaOTRqkXaqatcZTEaqptmMTWwVRzoYoYbLyhTW01WkBLAMWKBPb3h+N2iKgQfT3ofj8zZyLvvu/uc5wzxsdz9hwAAAyhdAMAAAAAYAilGwAAAAAAQyjdAAAAAAAYQukGAAAAAMAQSjcAAAAAAIZQugEAAAAAMITSDQAAAACAIZRuAAAAAAAMaXXprqysNJGjxVavXq3u3bvL399fsbGx2rdv303nb9myRb1795a/v7/69++vDz74oMnrLpdLWVlZCgsLU0BAgBISEnTixAmTuwAAAAAA8BA+rV3QqVMnPfTQQ4qOjm6y9erVSzabzURGt82bNys9PV25ubmKjY3VihUrlJiYqNLSUoWGhl43/7PPPtOUKVOUnZ2tcePGKS8vTxMmTNCBAwfUr18/SdKyZcuUk5Ojt99+W5GRkVqwYIESExN15MgR+fv7G92fu8Hlcqn+Ur3VMQAAAACg1do90M54zzTN5nK5XK1ZcPjwYZWUlOjgwYMqKSnR559/rsrKSvn7+6tfv37au3evqayKjY3V0KFDtWrVKklSY2OjIiIilJqaqoyMjOvmP/vss6qtrVV+fr57bPjw4Ro4cKByc3PlcrkUHh6uefPm6eWXX5YkVVdXy+FwaP369Zo8eXKzOerq6lRXV+f+uaamRhEREaqurlZwcPCd3OXbdqX2irIDs62OAQAAAACtlnkxU77tfa2O0ayamhrZ7fZb9sBWX17et29fJScna9myZfrzn/+siooK5efnKywsTPHx8bcV+mauXLmi4uJiJSQkuMe8vLyUkJCgwsLCZtcUFhY2mS9JiYmJ7vknT55UWVlZkzl2u12xsbE3fE9Jys7Olt1ud28RERG3s2sAAAAAgPtUqy8v/zabzaaxY8dq48aNWrdu3Z3I1Kzz58+roaFBDoejybjD4dCxY8eaXVNWVtbs/LKyMvfr18ZuNKc5mZmZSk9Pd/987Ux3W9TugXbKvJhpdQwAAAAAaLV2D7SzOsJtu+3Sfc3w4cM1ZcqUO/V2bZqfn5/8/PysjtEiNputzV6OAQAAAAD3u1aX7sDAQPXv31/R0dEaMGCAoqOj1bt3bxUVFenChQsmMkq6egM3b29vlZeXNxkvLy+X0+lsdo3T6bzp/Gv/LS8vV1hYWJM5AwcOvIPpAQAAAACeqNXf6d66daueeuopVVVVaeXKlXrssccUGhqqcePGad68eSYySpJ8fX01ZMgQFRQUuMcaGxtVUFCguLi4ZtfExcU1mS9JO3fudM+PjIyU0+lsMqempkZ79+694XsCAAAAANBSrT7TPWbMGI0ZM8b986VLl3Ty5El17Njxhmec75T09HRNmzZNMTExGjZsmFasWKHa2lrNmDFDkvSjH/1IDz30kLKzr96t+6WXXtJjjz2mN954Q08++aTeffdd7d+/3/3dc5vNprS0NC1atEg9e/Z0PzIsPDxcEyZMMLovAAAAAID7321/p/uBBx5Q375970SWW3r22Wd17tw5ZWVlqaysTAMHDtSOHTvcN0I7deqUvLz+d/J+xIgRysvL0/z58/XKK6+oZ8+e2r59u/sZ3ZL005/+VLW1tZo9e7aqqqr0yCOPaMeOHffFM7oBAAAAANZq9XO6cb2WPp8NAAAAAHB/MPacbgAAAAAA0DKUbgAAAAAADKF0AwAAAABgCKUbAAAAAABDKN0AAAAAABhC6QYAAAAAwBBKNwAAAAAAhlC6AQAAAAAwhNINAAAAAIAhlG4AAAAAAAyhdAMAAAAAYAilGwAAAAAAQyjdAAAAAAAYQukGAAAAAMAQSjcAAAAAAIZQugEAAAAAMITSDQAAAACAIZRuAAAAAAAMoXQDAAAAAGAIpRsAAAAAAEMo3QAAAAAAGELpBgAAAADAEEo3AAAAAACGULoBAAAAADCE0g0AAAAAgCGUbgAAAAAADKF0AwAAAABgCKUbAAAAAABDKN0AAAAAABhC6QYAAAAAwBBKNwAAAAAAhlC6AQAAAAAwhNINAAAAAIAhlG4AAAAAAAyhdAMAAAAAYAilGwAAAAAAQyjdAAAAAAAYcs+U7srKSiUnJys4OFghISGaOXOmLl68eNM1ly9fVkpKijp27KjAwEAlJSWpvLzc/frBgwc1ZcoURUREKCAgQFFRUVq5cqXpXQEAAAAAeIh7pnQnJyfr8OHD2rlzp/Lz87Vnzx7Nnj37pmvmzp2r999/X1u2bNHu3bt19uxZTZw40f16cXGxQkNDtXHjRh0+fFivvvqqMjMztWrVKtO7AwAAAADwADaXy+WyOsStHD16VH369FFRUZFiYmIkSTt27NATTzyhr776SuHh4detqa6uVufOnZWXl6dJkyZJko4dO6aoqCgVFhZq+PDhzX5WSkqKjh49ql27dt0wT11dnerq6tw/19TUKCIiQtXV1QoODr6dXQUAAAAA3ANqampkt9tv2QPviTPdhYWFCgkJcRduSUpISJCXl5f27t3b7Jri4mLV19crISHBPda7d2917dpVhYWFN/ys6upqdejQ4aZ5srOzZbfb3VtEREQr9wgAAAAA4AnuidJdVlam0NDQJmM+Pj7q0KGDysrKbrjG19dXISEhTcYdDscN13z22WfavHnzLS9bz8zMVHV1tXs7ffp0y3cGAAAAAOAxLC3dGRkZstlsN92OHTt2V7IcOnRITz31lBYuXKjRo0ffdK6fn5+Cg4ObbAAAAAAAfJuPlR8+b948TZ8+/aZzevToIafTqYqKiibj33zzjSorK+V0Optd53Q6deXKFVVVVTU5211eXn7dmiNHjig+Pl6zZ8/W/Pnzv9O+AAAAAADwbZaW7s6dO6tz5863nBcXF6eqqioVFxdryJAhkqRdu3apsbFRsbGxza4ZMmSI2rVrp4KCAiUlJUmSSktLderUKcXFxbnnHT58WKNGjdK0adO0ePHiO7BXAAAAAABcdU/cvVySxo4dq/LycuXm5qq+vl4zZsxQTEyM8vLyJElnzpxRfHy8NmzYoGHDhkmSXnjhBX3wwQdav369goODlZqaKunqd7elq5eUjxo1SomJiVq+fLn7s7y9vVv0jwHXtPSudQAAAACA+0NLe6ClZ7pbY9OmTZozZ47i4+Pl5eWlpKQk5eTkuF+vr69XaWmpLl265B5788033XPr6uqUmJioNWvWuF/funWrzp07p40bN2rjxo3u8W7duumf//znXdkvAAAAAMD96545092WcaYbAAAAADzLffWcbgAAAAAA7kWUbgAAAAAADKF0AwAAAABgCKUbAAAAAABDKN0AAAAAABhC6QYAAAAAwBBKNwAAAAAAhlC6AQAAAAAwhNINAAAAAIAhlG4AAAAAAAyhdAMAAAAAYAilGwAAAAAAQyjdAAAAAAAYQukGAAAAAMAQSjcAAAAAAIZQugEAAAAAMITSDQAAAACAIZRuAAAAAAAMoXQDAAAAAGAIpRsAAAAAAEMo3QAAAAAAGELpBgAAAADAEEo3AAAAAACGULoBAAAAADCE0g0AAAAAgCGUbgAAAAAADKF0AwAAAABgCKUbAAAAAABDKN0AAAAAABhC6QYAAAAAwBBKNwAAAAAAhlC6AQAAAAAwhNINAAAAAIAhlG4AAAAAAAyhdAMAAAAAYAilGwAAAAAAQyjdAAAAAAAYcs+U7srKSiUnJys4OFghISGaOXOmLl68eNM1ly9fVkpKijp27KjAwEAlJSWpvLy82bn/+c9/1KVLF9lsNlVVVRnYAwAAAACAp7lnSndycrIOHz6snTt3Kj8/X3v27NHs2bNvumbu3Ll6//33tWXLFu3evVtnz57VxIkTm507c+ZMDRgwwER0AAAAAICHsrlcLpfVIW7l6NGj6tOnj4qKihQTEyNJ2rFjh5544gl99dVXCg8Pv25NdXW1OnfurLy8PE2aNEmSdOzYMUVFRamwsFDDhw93z127dq02b96srKwsxcfH6+uvv1ZISEiL89XU1Mhut6u6ulrBwcG3t7MAAAAAgDavpT3wnjjTXVhYqJCQEHfhlqSEhAR5eXlp7969za4pLi5WfX29EhIS3GO9e/dW165dVVhY6B47cuSIXnvtNW3YsEFeXi377airq1NNTU2TDQAAAACAb7snSndZWZlCQ0ObjPn4+KhDhw4qKyu74RpfX9/rzlg7HA73mrq6Ok2ZMkXLly9X165dW5wnOztbdrvdvUVERLRuhwAAAAAAHsHS0p2RkSGbzXbT7dixY8Y+PzMzU1FRUXruuedava66utq9nT592lBCAAAAAMC9zMfKD583b56mT59+0zk9evSQ0+lURUVFk/FvvvlGlZWVcjqdza5zOp26cuWKqqqqmpztLi8vd6/ZtWuXvvjiC23dulWSdO3r7Z06ddKrr76qn//8582+t5+fn/z8/FqyiwAAAAAAD2Zp6e7cubM6d+58y3lxcXGqqqpScXGxhgwZIulqYW5sbFRsbGyza4YMGaJ27dqpoKBASUlJkqTS0lKdOnVKcXFxkqT33ntP//3vf91rioqK9Pzzz+vjjz/Www8/fLu7BwAAAADwcJaW7paKiorSmDFjNGvWLOXm5qq+vl5z5szR5MmT3XcuP3PmjOLj47VhwwYNGzZMdrtdM2fOVHp6ujp06KDg4GClpqYqLi7Ofefybxfr8+fPuz+vNXcvBwAAAACgOfdE6ZakTZs2ac6cOYqPj5eXl5eSkpKUk5Pjfr2+vl6lpaW6dOmSe+zNN990z62rq1NiYqLWrFljRXwAAAAAgAe6J57T3dbxnG4AAAAA8Cz31XO6AQAAAAC4F1G6AQAAAAAwhNINAAAAAIAhlG4AAAAAAAyhdAMAAAAAYAilGwAAAAAAQyjdAAAAAAAYQukGAAAAAMAQSjcAAAAAAIZQugEAAAAAMITSDQAAAACAIZRuAAAAAAAMoXQDAAAAAGAIpRsAAAAAAEMo3QAAAAAAGELpBgAAAADAEEo3AAAAAACGULoBAAAAADCE0g0AAAAAgCGUbgAAAAAADKF0AwAAAABgCKUbAAAAAABDKN0AAAAAABhC6QYAAAAAwBBKNwAAAAAAhlC6AQAAAAAwhNINAAAAAIAhlG4AAAAAAAzxsTrA/cDlckmSampqLE4CAAAAALgbrvW/a33wRijdd8CFCxckSRERERYnAQAAAADcTRcuXJDdbr/h6zbXrWo5bqmxsVFnz55VUFCQbDab1XGuU1NTo4iICJ0+fVrBwcFWxwE4JtHmcEyireGYRFvDMYm2pi0cky6XSxcuXFB4eLi8vG78zW3OdN8BXl5e6tKli9Uxbik4OJg/JNGmcEyireGYRFvDMYm2hmMSbY3Vx+TNznBfw43UAAAAAAAwhNINAAAAAIAhlG4P4Ofnp4ULF8rPz8/qKIAkjkm0PRyTaGs4JtHWcEyirbmXjklupAYAAAAAgCGc6QYAAAAAwBBKNwAAAAAAhlC6AQAAAAAwhNINAAAAAIAhlG4PsHr1anXv3l3+/v6KjY3Vvn37rI4ED5Wdna2hQ4cqKChIoaGhmjBhgkpLS62OBbgtXbpUNptNaWlpVkeBBztz5oyee+45dezYUQEBAerfv7/2799vdSx4qIaGBi1YsECRkZEKCAjQww8/rP/7v/8T92LG3bJnzx6NHz9e4eHhstls2r59e5PXXS6XsrKyFBYWpoCAACUkJOjEiRPWhL0BSvd9bvPmzUpPT9fChQt14MABRUdHKzExURUVFVZHgwfavXu3UlJS9Ne//lU7d+5UfX29Ro8erdraWqujASoqKtKvfvUrDRgwwOoo8GBff/21Ro4cqXbt2unDDz/UkSNH9MYbb+jBBx+0Oho81Ouvv661a9dq1apVOnr0qF5//XUtW7ZMv/zlL62OBg9RW1ur6OhorV69utnXly1bppycHOXm5mrv3r1q3769EhMTdfny5buc9MZ4ZNh9LjY2VkOHDtWqVaskSY2NjYqIiFBqaqoyMjIsTgdPd+7cOYWGhmr37t169NFHrY4DD3bx4kUNHjxYa9as0aJFizRw4ECtWLHC6ljwQBkZGfr000/18ccfWx0FkCSNGzdODodDv/71r91jSUlJCggI0MaNGy1MBk9ks9m0bds2TZgwQdLVs9zh4eGaN2+eXn75ZUlSdXW1HA6H1q9fr8mTJ1uY9n84030fu3LlioqLi5WQkOAe8/LyUkJCggoLCy1MBlxVXV0tSerQoYPFSeDpUlJS9OSTTzb58xKwwh//+EfFxMTo6aefVmhoqAYNGqS33nrL6ljwYCNGjFBBQYGOHz8uSTp48KA++eQTjR071uJkgHTy5EmVlZU1+f+33W5XbGxsm+o7PlYHgDnnz59XQ0ODHA5Hk3GHw6Fjx45ZlAq4qrGxUWlpaRo5cqT69etndRx4sHfffVcHDhxQUVGR1VEA/eMf/9DatWuVnp6uV155RUVFRXrxxRfl6+uradOmWR0PHigjI0M1NTXq3bu3vL291dDQoMWLFys5OdnqaIDKysokqdm+c+21toDSDcASKSkpOnTokD755BOro8CDnT59Wi+99JJ27twpf39/q+MAamxsVExMjJYsWSJJGjRokA4dOqTc3FxKNyzxu9/9Tps2bVJeXp769u2rkpISpaWlKTw8nGMSaCEuL7+PderUSd7e3iovL28yXl5eLqfTaVEqQJozZ47y8/P10UcfqUuXLlbHgQcrLi5WRUWFBg8eLB8fH/n4+Gj37t3KycmRj4+PGhoarI4IDxMWFqY+ffo0GYuKitKpU6csSgRP95Of/EQZGRmaPHmy+vfvr6lTp2ru3LnKzs62Ohrg7jRtve9Quu9jvr6+GjJkiAoKCtxjjY2NKigoUFxcnIXJ4KlcLpfmzJmjbdu2adeuXYqMjLQ6EjxcfHy8vvjiC5WUlLi3mJgYJScnq6SkRN7e3lZHhIcZOXLkdY9SPH78uLp162ZRIni6S5cuycuraWXw9vZWY2OjRYmA/4mMjJTT6WzSd2pqarR379421Xe4vPw+l56ermnTpikmJkbDhg3TihUrVFtbqxkzZlgdDR4oJSVFeXl5+sMf/qCgoCD3d23sdrsCAgIsTgdPFBQUdN09Bdq3b6+OHTtyrwFYYu7cuRoxYoSWLFmiZ555Rvv27dO6deu0bt06q6PBQ40fP16LFy9W165d1bdvX33++ef6xS9+oeeff97qaPAQFy9e1N///nf3zydPnlRJSYk6dOigrl27Ki0tTYsWLVLPnj0VGRmpBQsWKDw83H2H87aAR4Z5gFWrVmn58uUqKyvTwIEDlZOTo9jYWKtjwQPZbLZmx3/7299q+vTpdzcMcAOPP/44jwyDpfLz85WZmakTJ04oMjJS6enpmjVrltWx4KEuXLigBQsWaNu2baqoqFB4eLimTJmirKws+fr6Wh0PHuAvf/mLfvCDH1w3Pm3aNK1fv14ul0sLFy7UunXrVFVVpUceeURr1qxRr169LEjbPEo3AAAAAACG8J1uAAAAAAAMoXQDAAAAAGAIpRsAAAAAAEMo3QAAAAAAGELpBgAAAADAEEo3AAAAAACGULoBAAAAADCE0g0AAAAAgCGUbgAAPND06dM1YcKEu/6569evl81mk81mU1paWovWTJ8+3b1m+/btRvMBAHCn+VgdAAAA3Fk2m+2mry9cuFArV66Uy+W6S4maCg4OVmlpqdq3b9+i+StXrtTSpUsVFhZmOBkAAHcepRsAgPvMv//9b/evN2/erKysLJWWlrrHAgMDFRgYaEU0SVf/UcDpdLZ4vt1ul91uN5gIAABzuLwcAID7jNPpdG92u91dcq9tgYGB111e/vjjjys1NVVpaWl68MEH5XA49NZbb6m2tlYzZsxQUFCQvve97+nDDz9s8lmHDh3S2LFjFRgYKIfDoalTp+r8+fOtzrxmzRr17NlT/v7+cjgcmjRp0u3+NgAA0CZQugEAgCTp7bffVqdOnbRv3z6lpqbqhRde0NNPP60RI0bowIEDGj16tKZOnapLly5JkqqqqjRq1CgNGjRI+/fv144dO1ReXq5nnnmmVZ+7f/9+vfjii3rttddUWlqqHTt26NFHHzWxiwAA3HVcXg4AACRJ0dHRmj9/viQpMzNTS5cuVadOnTRr1ixJUlZWltauXau//e1vGj58uFatWqVBgwZpyZIl7vf4zW9+o4iICB0/fly9evVq0eeeOnVK7du317hx4xQUFKRu3bpp0KBBd34HAQCwAGe6AQCAJGnAgAHuX3t7e6tjx47q37+/e8zhcEiSKioqJEkHDx7URx995P6OeGBgoHr37i1J+vLLL1v8uT/84Q/VrVs39ejRQ1OnTtWmTZvcZ9MBALjXUboBAIAkqV27dk1+ttlsTcau3RW9sbFRknTx4kWNHz9eJSUlTbYTJ0606vLwoKAgHThwQO+8847CwsKUlZWl6OhoVVVV3f5OAQBgMS4vBwAA38ngwYP13nvvqXv37vLxub2/Uvj4+CghIUEJCQlauHChQkJCtGvXLk2cOPEOpQUAwBqc6QYAAN9JSkqKKisrNWXKFBUVFenLL7/Un/70J82YMUMNDQ0tfp/8/Hzl5OSopKRE//rXv7RhwwY1Njbq+9//vsH0AADcHZRuAADwnYSHh+vTTz9VQ0ODRo8erf79+ystLU0hISHy8mr5XzFCQkL0+9//XqNGjVJUVJRyc3P1zjvvqG/fvgbTAwBwd9hcLpfL6hAAAMAzrF+/Xmlpad/p+9o2m03btm1r8nxxAADaOs50AwCAu6q6ulqBgYH62c9+1qL5P/7xjxUYGGg4FQAAZnCmGwAA3DUXLlxQeXm5pKuXlXfq1OmWayoqKlRTUyNJCgsLU/v27Y1mBADgTqJ0AwAAAABgCJeXAwAAAABgCKUbAAAAAABDKN0AAAAAABhC6QYAAAAAwBBKNwAAAAAAhlC6AQAAAAAwhNINAAAAAIAhlG4AAAAAAAz5f+tXcUrFYyOeAAAAAElFTkSuQmCC", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "# import libraries\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "\n", "# define linear system\n", "n = dim_x = 2 # state dimension\n", "m = dim_u = 1 # input dimension\n", "\n", "A = np.array([[2, 0],\n", " [0, -5]])\n", "\n", "B = np.array([[1],\n", " [-2]])\n", "\n", "# set simulation parameters\n", "dt = 0.1 # time step\n", "sim_steps = 100 # number of simulation steps\n", "\n", "# set initial state\n", "t = 0.0 # [s]\n", "x = np.array([[3.0],\n", " [-5.0]])\n", "\n", "# prepare storage\n", "x_log = np.zeros((dim_x, sim_steps))\n", "u_log = np.zeros((dim_u, sim_steps))\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # announce current state\n", " print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\n", "\n", " ##### calculate control input #####\n", " u = np.array([[0.0]]) # zero input\n", " ###################################\n", "\n", " # update state\n", " x_dot = A @ x + B @ u\n", " x = x + x_dot * dt\n", "\n", " # update time\n", " t = t + dt\n", "\n", " # store data\n", " x_log[:, i] = x.flatten()\n", " u_log[:, i] = u.flatten()\n", "\n", "# plot results\n", "fig, ax = plt.subplots(3, 1, figsize=(10, 10))\n", "time = np.arange(0, sim_steps * dt, dt)\n", "\n", "ax[0].plot(time, x_log[0, :], label='x1', color=\"blue\")\n", "ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[0].set_xlabel('Time [s]')\n", "ax[0].set_ylabel(r'$x_1$')\n", "\n", "ax[1].plot(time, x_log[1, :], label='x2', color=\"blue\")\n", "ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[1].set_xlabel('Time [s]')\n", "ax[1].set_ylabel(r'$x_2$')\n", "\n", "ax[2].plot(time, u_log[0, :], label='u', color=\"purple\")\n", "ax[2].set_xlabel('Time [s]')\n", "ax[2].set_ylabel(r'$u$')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Case.2 : Linear Quadratic Regulator\n", "\n", "$$\n", "\\begin{align}\n", "{u}(t) &= - f \\cdot x(t) \\nonumber\\\\ \n", " &= - \\begin{bmatrix} 4.43 & -0.08\\end{bmatrix} x(t) \\nonumber\n", "\\end{align}\n", "$$" ] }, { "cell_type": "code", "execution_count": 32, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "solutions : \n", "### sol. 0\n", "0.790090716185432\n", "1.41489373711061\n", "-0.333070844337325\n", "### sol. 1\n", "-0.238962893661010\n", "-0.0144238134144138\n", "0.0957588124024186\n", "### sol. 2\n", "4.67613366903330\n", "0.118577328113591\n", "0.0993577861808820\n", "### sol. 3\n", "-0.247669654823031\n", "-0.172108476299586\n", "-2.76000493791945\n", "#########\n" ] } ], "source": [ "from sympy import symbols, Eq, solve\n", "\n", "# define symbols\n", "p1, p2, p3 = symbols('p1 p2 p3')\n", "\n", "# define equations\n", "eq1 = Eq(-p1**2 + 4*p1 + 4*p1*p2 - 4*p2**2 + 1, 0)\n", "eq2 = Eq(2*p2**2 - 3*p2 - p1*p2 + 2*p1*p3 - 4*p2*p3, 0)\n", "eq3 = Eq(-p2**2 + 4*p2*p3 - 10*p3 - 4*p3**2 + 1, 0)\n", "\n", "# solve equations\n", "solutions = solve([eq1, eq2, eq3], [p1, p2, p3])\n", "\n", "# output solutions\n", "print(\"solutions : \")\n", "for idx, s in enumerate(solutions):\n", " print(f\"### sol. {idx}\")\n", " for j in range(len(s)):\n", " print(s[j].evalf())\n", "print(\"###\"*3)\n" ] }, { "cell_type": "code", "execution_count": 28, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "t = 0.00 [s], x = [3.00, -5.00]\n", "t = 0.10 [s], x = [2.23, 0.24]\n", "t = 0.20 [s], x = [1.69, 2.09]\n", "t = 0.30 [s], x = [1.30, 2.51]\n", "t = 0.40 [s], x = [1.00, 2.36]\n", "t = 0.50 [s], x = [0.78, 2.03]\n", "t = 0.60 [s], x = [0.60, 1.67]\n", "t = 0.70 [s], x = [0.47, 1.34]\n", "t = 0.80 [s], x = [0.37, 1.07]\n", "t = 0.90 [s], x = [0.29, 0.84]\n", "t = 1.00 [s], x = [0.22, 0.66]\n", "t = 1.10 [s], x = [0.17, 0.52]\n", "t = 1.20 [s], x = [0.14, 0.41]\n", "t = 1.30 [s], x = [0.11, 0.32]\n", "t = 1.40 [s], x = [0.08, 0.25]\n", "t = 1.50 [s], x = [0.06, 0.19]\n", "t = 1.60 [s], x = [0.05, 0.15]\n", "t = 1.70 [s], x = [0.04, 0.12]\n", "t = 1.80 [s], x = [0.03, 0.09]\n", "t = 1.90 [s], x = [0.02, 0.07]\n", "t = 2.00 [s], x = [0.02, 0.06]\n", "t = 2.10 [s], x = [0.01, 0.04]\n", "t = 2.20 [s], x = [0.01, 0.03]\n", "t = 2.30 [s], x = [0.01, 0.03]\n", "t = 2.40 [s], x = [0.01, 0.02]\n", "t = 2.50 [s], x = [0.01, 0.02]\n", "t = 2.60 [s], x = [0.00, 0.01]\n", "t = 2.70 [s], x = [0.00, 0.01]\n", "t = 2.80 [s], x = [0.00, 0.01]\n", "t = 2.90 [s], x = [0.00, 0.01]\n", "t = 3.00 [s], x = [0.00, 0.00]\n", "t = 3.10 [s], x = [0.00, 0.00]\n", "t = 3.20 [s], x = [0.00, 0.00]\n", "t = 3.30 [s], x = [0.00, 0.00]\n", "t = 3.40 [s], x = [0.00, 0.00]\n", "t = 3.50 [s], x = [0.00, 0.00]\n", "t = 3.60 [s], x = [0.00, 0.00]\n", "t = 3.70 [s], x = [0.00, 0.00]\n", "t = 3.80 [s], x = [0.00, 0.00]\n", "t = 3.90 [s], x = [0.00, 0.00]\n", "t = 4.00 [s], x = [0.00, 0.00]\n", "t = 4.10 [s], x = [0.00, 0.00]\n", "t = 4.20 [s], x = [0.00, 0.00]\n", "t = 4.30 [s], x = [0.00, 0.00]\n", "t = 4.40 [s], x = [0.00, 0.00]\n", "t = 4.50 [s], x = [0.00, 0.00]\n", "t = 4.60 [s], x = [0.00, 0.00]\n", "t = 4.70 [s], x = [0.00, 0.00]\n", "t = 4.80 [s], x = [0.00, 0.00]\n", "t = 4.90 [s], x = [0.00, 0.00]\n", "t = 5.00 [s], x = [0.00, 0.00]\n", "t = 5.10 [s], x = [0.00, 0.00]\n", "t = 5.20 [s], x = [0.00, 0.00]\n", "t = 5.30 [s], x = [0.00, 0.00]\n", "t = 5.40 [s], x = [0.00, 0.00]\n", "t = 5.50 [s], x = [0.00, 0.00]\n", "t = 5.60 [s], x = [0.00, 0.00]\n", "t = 5.70 [s], x = [0.00, 0.00]\n", "t = 5.80 [s], x = [0.00, 0.00]\n", "t = 5.90 [s], x = [0.00, 0.00]\n", "t = 6.00 [s], x = [0.00, 0.00]\n", "t = 6.10 [s], x = [0.00, 0.00]\n", "t = 6.20 [s], x = [0.00, 0.00]\n", "t = 6.30 [s], x = [0.00, 0.00]\n", "t = 6.40 [s], x = [0.00, 0.00]\n", "t = 6.50 [s], x = [0.00, 0.00]\n", "t = 6.60 [s], x = [0.00, 0.00]\n", "t = 6.70 [s], x = [0.00, 0.00]\n", "t = 6.80 [s], x = [0.00, 0.00]\n", "t = 6.90 [s], x = [0.00, 0.00]\n", "t = 7.00 [s], x = [0.00, 0.00]\n", "t = 7.10 [s], x = [0.00, 0.00]\n", "t = 7.20 [s], x = [0.00, 0.00]\n", "t = 7.30 [s], x = [0.00, 0.00]\n", "t = 7.40 [s], x = [0.00, 0.00]\n", "t = 7.50 [s], x = [0.00, 0.00]\n", "t = 7.60 [s], x = [0.00, 0.00]\n", "t = 7.70 [s], x = [0.00, 0.00]\n", "t = 7.80 [s], x = [0.00, 0.00]\n", "t = 7.90 [s], x = [0.00, 0.00]\n", "t = 8.00 [s], x = [0.00, 0.00]\n", "t = 8.10 [s], x = [0.00, 0.00]\n", "t = 8.20 [s], x = [0.00, 0.00]\n", "t = 8.30 [s], x = [0.00, 0.00]\n", "t = 8.40 [s], x = [0.00, 0.00]\n", "t = 8.50 [s], x = [0.00, 0.00]\n", "t = 8.60 [s], x = [0.00, 0.00]\n", "t = 8.70 [s], x = [0.00, 0.00]\n", "t = 8.80 [s], x = [0.00, 0.00]\n", "t = 8.90 [s], x = [0.00, 0.00]\n", "t = 9.00 [s], x = [0.00, 0.00]\n", "t = 9.10 [s], x = [0.00, 0.00]\n", "t = 9.20 [s], x = [0.00, 0.00]\n", "t = 9.30 [s], x = [0.00, 0.00]\n", "t = 9.40 [s], x = [0.00, 0.00]\n", "t = 9.50 [s], x = [0.00, 0.00]\n", "t = 9.60 [s], x = [0.00, 0.00]\n", "t = 9.70 [s], x = [0.00, 0.00]\n", "t = 9.80 [s], x = [0.00, 0.00]\n", "t = 9.90 [s], x = [0.00, 0.00]\n" ] }, { "data": { "image/png": "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", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "# import libraries\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "\n", "# define linear system\n", "n = dim_x = 2 # state dimension\n", "m = dim_u = 1 # input dimension\n", "\n", "A = np.array([[2, 0],\n", " [0, -5]])\n", "\n", "B = np.array([[1],\n", " [-2]])\n", "\n", "# set simulation parameters\n", "dt = 0.1 # time step\n", "sim_steps = 100 # number of simulation steps\n", "\n", "# set initial state\n", "t = 0.0 # [s]\n", "x = np.array([[3.0],\n", " [-5.0]])\n", "\n", "# prepare storage\n", "x_log = np.zeros((dim_x, sim_steps))\n", "u_log = np.zeros((dim_u, sim_steps))\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # announce current state\n", " print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\n", "\n", " ##### calculate control input #####\n", " f = np.array([[4.43, -0.08]]) # feedback gain martrix\n", " u = - f @ x # state feedback control input\n", " ###################################\n", "\n", " # update state\n", " x_dot = A @ x + B @ u\n", " x = x + x_dot * dt\n", "\n", " # update time\n", " t = t + dt\n", "\n", " # store data\n", " x_log[:, i] = x.flatten()\n", " u_log[:, i] = u.flatten()\n", "\n", "# plot results\n", "fig, ax = plt.subplots(3, 1, figsize=(10, 10))\n", "time = np.arange(0, sim_steps * dt, dt)\n", "\n", "ax[0].plot(time, x_log[0, :], label='x1', color=\"blue\")\n", "ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[0].set_xlabel('Time [s]')\n", "ax[0].set_ylabel(r'$x_1$')\n", "\n", "ax[1].plot(time, x_log[1, :], label='x2', color=\"blue\")\n", "ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[1].set_xlabel('Time [s]')\n", "ax[1].set_ylabel(r'$x_2$')\n", "\n", "ax[2].plot(time, u_log[0, :], label='u', color=\"purple\")\n", "ax[2].set_xlabel('Time [s]')\n", "ax[2].set_ylabel(r'$u$')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] } ], "metadata": { "kernelspec": { "display_name": ".venv", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 2 } ================================================ FILE: notebooks/preliminary/state_feedback_control_tutorial.ipynb ================================================ { "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Stabilize A Linear System With State Feedback Control\n", "\n", "\n", "## Control Target System\n", "$$\n", "\\dot{x}(t) = \\begin{bmatrix} 2 & 0 \\\\ 0 & -5 \\end{bmatrix} x(t) + \\begin{bmatrix} 1 \\\\ -2 \\end{bmatrix} u(t)\n", "$$" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Case.1 : zero input\n", "\n", "$$\n", "\\begin{align}\n", "{u}(t) = \\begin{bmatrix} 0 \\end{bmatrix} \\nonumber\n", "\\end{align}\n", "$$" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "t = 0.00 [s], x = [3.00, -5.00]\n", "t = 0.10 [s], x = [3.60, -2.50]\n", "t = 0.20 [s], x = [4.32, -1.25]\n", "t = 0.30 [s], x = [5.18, -0.62]\n", "t = 0.40 [s], x = [6.22, -0.31]\n", "t = 0.50 [s], x = [7.46, -0.16]\n", "t = 0.60 [s], x = [8.96, -0.08]\n", "t = 0.70 [s], x = [10.75, -0.04]\n", "t = 0.80 [s], x = [12.90, -0.02]\n", "t = 0.90 [s], x = [15.48, -0.01]\n", "t = 1.00 [s], x = [18.58, -0.00]\n", "t = 1.10 [s], x = [22.29, -0.00]\n", "t = 1.20 [s], x = [26.75, -0.00]\n", "t = 1.30 [s], x = [32.10, -0.00]\n", "t = 1.40 [s], x = [38.52, -0.00]\n", "t = 1.50 [s], x = [46.22, -0.00]\n", "t = 1.60 [s], x = [55.47, -0.00]\n", "t = 1.70 [s], x = [66.56, -0.00]\n", "t = 1.80 [s], x = [79.87, -0.00]\n", "t = 1.90 [s], x = [95.84, -0.00]\n", "t = 2.00 [s], x = [115.01, -0.00]\n", "t = 2.10 [s], x = [138.02, -0.00]\n", "t = 2.20 [s], x = [165.62, -0.00]\n", "t = 2.30 [s], x = [198.74, -0.00]\n", "t = 2.40 [s], x = [238.49, -0.00]\n", "t = 2.50 [s], x = [286.19, -0.00]\n", "t = 2.60 [s], x = [343.43, -0.00]\n", "t = 2.70 [s], x = [412.11, -0.00]\n", "t = 2.80 [s], x = [494.53, -0.00]\n", "t = 2.90 [s], x = [593.44, -0.00]\n", "t = 3.00 [s], x = [712.13, -0.00]\n", "t = 3.10 [s], x = [854.55, -0.00]\n", "t = 3.20 [s], x = [1025.47, -0.00]\n", "t = 3.30 [s], x = [1230.56, -0.00]\n", "t = 3.40 [s], x = [1476.67, -0.00]\n", "t = 3.50 [s], x = [1772.00, -0.00]\n", "t = 3.60 [s], x = [2126.41, -0.00]\n", "t = 3.70 [s], x = [2551.69, -0.00]\n", "t = 3.80 [s], x = [3062.02, -0.00]\n", "t = 3.90 [s], x = [3674.43, -0.00]\n", "t = 4.00 [s], x = [4409.31, -0.00]\n", "t = 4.10 [s], x = [5291.18, -0.00]\n", "t = 4.20 [s], x = [6349.41, -0.00]\n", "t = 4.30 [s], x = [7619.30, -0.00]\n", "t = 4.40 [s], x = [9143.15, -0.00]\n", "t = 4.50 [s], x = [10971.79, -0.00]\n", "t = 4.60 [s], x = [13166.14, -0.00]\n", "t = 4.70 [s], x = [15799.37, -0.00]\n", "t = 4.80 [s], x = [18959.25, -0.00]\n", "t = 4.90 [s], x = [22751.10, -0.00]\n", "t = 5.00 [s], x = [27301.31, -0.00]\n", "t = 5.10 [s], x = [32761.58, -0.00]\n", "t = 5.20 [s], x = [39313.89, -0.00]\n", "t = 5.30 [s], x = [47176.67, -0.00]\n", "t = 5.40 [s], x = [56612.01, -0.00]\n", "t = 5.50 [s], x = [67934.41, -0.00]\n", "t = 5.60 [s], x = [81521.29, -0.00]\n", "t = 5.70 [s], x = [97825.55, -0.00]\n", "t = 5.80 [s], x = [117390.65, -0.00]\n", "t = 5.90 [s], x = [140868.79, -0.00]\n", "t = 6.00 [s], x = [169042.54, -0.00]\n", "t = 6.10 [s], x = [202851.05, -0.00]\n", "t = 6.20 [s], x = [243421.26, -0.00]\n", "t = 6.30 [s], x = [292105.51, -0.00]\n", "t = 6.40 [s], x = [350526.62, -0.00]\n", "t = 6.50 [s], x = [420631.94, -0.00]\n", "t = 6.60 [s], x = [504758.33, -0.00]\n", "t = 6.70 [s], x = [605709.99, -0.00]\n", "t = 6.80 [s], x = [726851.99, -0.00]\n", "t = 6.90 [s], x = [872222.39, -0.00]\n", "t = 7.00 [s], x = [1046666.87, -0.00]\n", "t = 7.10 [s], x = [1256000.24, -0.00]\n", "t = 7.20 [s], x = [1507200.29, -0.00]\n", "t = 7.30 [s], x = [1808640.35, -0.00]\n", "t = 7.40 [s], x = [2170368.42, -0.00]\n", "t = 7.50 [s], x = [2604442.11, -0.00]\n", "t = 7.60 [s], x = [3125330.53, -0.00]\n", "t = 7.70 [s], x = [3750396.64, -0.00]\n", "t = 7.80 [s], x = [4500475.96, -0.00]\n", "t = 7.90 [s], x = [5400571.16, -0.00]\n", "t = 8.00 [s], x = [6480685.39, -0.00]\n", "t = 8.10 [s], x = [7776822.46, -0.00]\n", "t = 8.20 [s], x = [9332186.96, -0.00]\n", "t = 8.30 [s], x = [11198624.35, -0.00]\n", "t = 8.40 [s], x = [13438349.22, -0.00]\n", "t = 8.50 [s], x = [16126019.06, -0.00]\n", "t = 8.60 [s], x = [19351222.87, -0.00]\n", "t = 8.70 [s], x = [23221467.45, -0.00]\n", "t = 8.80 [s], x = [27865760.94, -0.00]\n", "t = 8.90 [s], x = [33438913.12, -0.00]\n", "t = 9.00 [s], x = [40126695.75, -0.00]\n", "t = 9.10 [s], x = [48152034.90, -0.00]\n", "t = 9.20 [s], x = [57782441.88, -0.00]\n", "t = 9.30 [s], x = [69338930.25, -0.00]\n", "t = 9.40 [s], x = [83206716.30, -0.00]\n", "t = 9.50 [s], x = [99848059.56, -0.00]\n", "t = 9.60 [s], x = [119817671.47, -0.00]\n", "t = 9.70 [s], x = [143781205.77, -0.00]\n", "t = 9.80 [s], x = [172537446.92, -0.00]\n", "t = 9.90 [s], x = [207044936.31, -0.00]\n" ] }, { "data": { "image/png": "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", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "# import libraries\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "\n", "# define linear system\n", "n = dim_x = 2 # state dimension\n", "m = dim_u = 1 # input dimension\n", "\n", "A = np.array([[2, 0],\n", " [0, -5]])\n", "\n", "B = np.array([[1],\n", " [-2]])\n", "\n", "# set simulation parameters\n", "dt = 0.1 # time step\n", "sim_steps = 100 # number of simulation steps\n", "\n", "# set initial state\n", "t = 0.0 # [s]\n", "x = np.array([[3.0],\n", " [-5.0]])\n", "\n", "# prepare storage\n", "x_log = np.zeros((dim_x, sim_steps))\n", "u_log = np.zeros((dim_u, sim_steps))\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # announce current state\n", " print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\n", "\n", " ##### calculate control input #####\n", " u = np.array([[0.0]]) # zero input\n", " ###################################\n", "\n", " # update state\n", " x_dot = A @ x + B @ u\n", " x = x + x_dot * dt\n", "\n", " # update time\n", " t = t + dt\n", "\n", " # store data\n", " x_log[:, i] = x.flatten()\n", " u_log[:, i] = u.flatten()\n", "\n", "# plot results\n", "fig, ax = plt.subplots(3, 1, figsize=(10, 10))\n", "time = np.arange(0, sim_steps * dt, dt)\n", "\n", "ax[0].plot(time, x_log[0, :], label='x1', color=\"blue\")\n", "ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[0].set_xlabel('Time [s]')\n", "ax[0].set_ylabel(r'$x_1$')\n", "\n", "ax[1].plot(time, x_log[1, :], label='x2', color=\"blue\")\n", "ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[1].set_xlabel('Time [s]')\n", "ax[1].set_ylabel(r'$x_2$')\n", "\n", "ax[2].plot(time, u_log[0, :], label='u', color=\"purple\")\n", "ax[2].set_xlabel('Time [s]')\n", "ax[2].set_ylabel(r'$u$')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Case.2 : state feedback control\n", "\n", "$$\n", "\\begin{align}\n", "{u}(t) &= - f \\cdot x(t) \\nonumber\\\\ \n", " &= - \\begin{bmatrix} 12/7 & 6/7\\end{bmatrix} x(t) \\nonumber\n", "\\end{align}\n", "$$" ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "t = 0.00 [s], x = [3.00, -5.00]\n", "t = 0.10 [s], x = [3.51, -2.33]\n", "t = 0.20 [s], x = [3.81, -0.36]\n", "t = 0.30 [s], x = [3.95, 1.07]\n", "t = 0.40 [s], x = [3.98, 2.07]\n", "t = 0.50 [s], x = [3.91, 2.75]\n", "t = 0.60 [s], x = [3.79, 3.19]\n", "t = 0.70 [s], x = [3.62, 3.44]\n", "t = 0.80 [s], x = [3.43, 3.55]\n", "t = 0.90 [s], x = [3.22, 3.56]\n", "t = 1.00 [s], x = [3.01, 3.50]\n", "t = 1.10 [s], x = [2.80, 3.38]\n", "t = 1.20 [s], x = [2.59, 3.23]\n", "t = 1.30 [s], x = [2.38, 3.05]\n", "t = 1.40 [s], x = [2.19, 2.87]\n", "t = 1.50 [s], x = [2.01, 2.68]\n", "t = 1.60 [s], x = [1.84, 2.49]\n", "t = 1.70 [s], x = [1.67, 2.30]\n", "t = 1.80 [s], x = [1.53, 2.12]\n", "t = 1.90 [s], x = [1.39, 1.94]\n", "t = 2.00 [s], x = [1.26, 1.78]\n", "t = 2.10 [s], x = [1.14, 1.63]\n", "t = 2.20 [s], x = [1.04, 1.49]\n", "t = 2.30 [s], x = [0.94, 1.35]\n", "t = 2.40 [s], x = [0.85, 1.23]\n", "t = 2.50 [s], x = [0.77, 1.12]\n", "t = 2.60 [s], x = [0.70, 1.01]\n", "t = 2.70 [s], x = [0.63, 0.92]\n", "t = 2.80 [s], x = [0.57, 0.83]\n", "t = 2.90 [s], x = [0.51, 0.75]\n", "t = 3.00 [s], x = [0.46, 0.68]\n", "t = 3.10 [s], x = [0.42, 0.62]\n", "t = 3.20 [s], x = [0.38, 0.56]\n", "t = 3.30 [s], x = [0.34, 0.50]\n", "t = 3.40 [s], x = [0.31, 0.45]\n", "t = 3.50 [s], x = [0.28, 0.41]\n", "t = 3.60 [s], x = [0.25, 0.37]\n", "t = 3.70 [s], x = [0.22, 0.33]\n", "t = 3.80 [s], x = [0.20, 0.30]\n", "t = 3.90 [s], x = [0.18, 0.27]\n", "t = 4.00 [s], x = [0.16, 0.24]\n", "t = 4.10 [s], x = [0.15, 0.22]\n", "t = 4.20 [s], x = [0.13, 0.20]\n", "t = 4.30 [s], x = [0.12, 0.18]\n", "t = 4.40 [s], x = [0.11, 0.16]\n", "t = 4.50 [s], x = [0.10, 0.14]\n", "t = 4.60 [s], x = [0.09, 0.13]\n", "t = 4.70 [s], x = [0.08, 0.12]\n", "t = 4.80 [s], x = [0.07, 0.11]\n", "t = 4.90 [s], x = [0.06, 0.10]\n", "t = 5.00 [s], x = [0.06, 0.09]\n", "t = 5.10 [s], x = [0.05, 0.08]\n", "t = 5.20 [s], x = [0.05, 0.07]\n", "t = 5.30 [s], x = [0.04, 0.06]\n", "t = 5.40 [s], x = [0.04, 0.06]\n", "t = 5.50 [s], x = [0.03, 0.05]\n", "t = 5.60 [s], x = [0.03, 0.05]\n", "t = 5.70 [s], x = [0.03, 0.04]\n", "t = 5.80 [s], x = [0.02, 0.04]\n", "t = 5.90 [s], x = [0.02, 0.03]\n", "t = 6.00 [s], x = [0.02, 0.03]\n", "t = 6.10 [s], x = [0.02, 0.03]\n", "t = 6.20 [s], x = [0.02, 0.02]\n", "t = 6.30 [s], x = [0.01, 0.02]\n", "t = 6.40 [s], x = [0.01, 0.02]\n", "t = 6.50 [s], x = [0.01, 0.02]\n", "t = 6.60 [s], x = [0.01, 0.02]\n", "t = 6.70 [s], x = [0.01, 0.01]\n", "t = 6.80 [s], x = [0.01, 0.01]\n", "t = 6.90 [s], x = [0.01, 0.01]\n", "t = 7.00 [s], x = [0.01, 0.01]\n", "t = 7.10 [s], x = [0.01, 0.01]\n", "t = 7.20 [s], x = [0.01, 0.01]\n", "t = 7.30 [s], x = [0.01, 0.01]\n", "t = 7.40 [s], x = [0.00, 0.01]\n", "t = 7.50 [s], x = [0.00, 0.01]\n", "t = 7.60 [s], x = [0.00, 0.01]\n", "t = 7.70 [s], x = [0.00, 0.01]\n", "t = 7.80 [s], x = [0.00, 0.00]\n", "t = 7.90 [s], x = [0.00, 0.00]\n", "t = 8.00 [s], x = [0.00, 0.00]\n", "t = 8.10 [s], x = [0.00, 0.00]\n", "t = 8.20 [s], x = [0.00, 0.00]\n", "t = 8.30 [s], x = [0.00, 0.00]\n", "t = 8.40 [s], x = [0.00, 0.00]\n", "t = 8.50 [s], x = [0.00, 0.00]\n", "t = 8.60 [s], x = [0.00, 0.00]\n", "t = 8.70 [s], x = [0.00, 0.00]\n", "t = 8.80 [s], x = [0.00, 0.00]\n", "t = 8.90 [s], x = [0.00, 0.00]\n", "t = 9.00 [s], x = [0.00, 0.00]\n", "t = 9.10 [s], x = [0.00, 0.00]\n", "t = 9.20 [s], x = [0.00, 0.00]\n", "t = 9.30 [s], x = [0.00, 0.00]\n", "t = 9.40 [s], x = [0.00, 0.00]\n", "t = 9.50 [s], x = [0.00, 0.00]\n", "t = 9.60 [s], x = [0.00, 0.00]\n", "t = 9.70 [s], x = [0.00, 0.00]\n", "t = 9.80 [s], x = [0.00, 0.00]\n", "t = 9.90 [s], x = [0.00, 0.00]\n" ] }, { "data": { "image/png": "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", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "# import libraries\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "\n", "# define linear system\n", "n = dim_x = 2 # state dimension\n", "m = dim_u = 1 # input dimension\n", "\n", "A = np.array([[2, 0],\n", " [0, -5]])\n", "\n", "B = np.array([[1],\n", " [-2]])\n", "\n", "# set simulation parameters\n", "dt = 0.1 # time step\n", "sim_steps = 100 # number of simulation steps\n", "\n", "# set initial state\n", "t = 0.0 # [s]\n", "x = np.array([[3.0],\n", " [-5.0]])\n", "\n", "# prepare storage\n", "x_log = np.zeros((dim_x, sim_steps))\n", "u_log = np.zeros((dim_u, sim_steps))\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # announce current state\n", " print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\n", "\n", " ##### calculate control input #####\n", " f = np.array([[12.0/7.0, 6.0/7.0]]) # feedback gain martrix\n", " u = - f @ x # state feedback control input\n", " ###################################\n", "\n", " # update state\n", " x_dot = A @ x + B @ u\n", " x = x + x_dot * dt\n", "\n", " # update time\n", " t = t + dt\n", "\n", " # store data\n", " x_log[:, i] = x.flatten()\n", " u_log[:, i] = u.flatten()\n", "\n", "# plot results\n", "fig, ax = plt.subplots(3, 1, figsize=(10, 10))\n", "time = np.arange(0, sim_steps * dt, dt)\n", "\n", "ax[0].plot(time, x_log[0, :], label='x1', color=\"blue\")\n", "ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[0].set_xlabel('Time [s]')\n", "ax[0].set_ylabel(r'$x_1$')\n", "\n", "ax[1].plot(time, x_log[1, :], label='x2', color=\"blue\")\n", "ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\n", "ax[1].set_xlabel('Time [s]')\n", "ax[1].set_ylabel(r'$x_2$')\n", "\n", "ax[2].plot(time, u_log[0, :], label='u', color=\"purple\")\n", "ax[2].set_xlabel('Time [s]')\n", "ax[2].set_ylabel(r'$u$')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] } ], "metadata": { "kernelspec": { "display_name": ".venv", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 2 } ================================================ FILE: notebooks/purepursuit.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Pure Pursuit Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle \n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " look_ahead_point: list = [0.0, 0.0], # look ahead point\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj, look_ahead_point)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, look_ahead_point: list) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the look ahead point\n", " frame = self.main_ax.plot(look_ahead_point[0]-x, look_ahead_point[1]-y, marker='X', color='blue', markersize=13, zorder=5)\n", "\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame += self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Longitudinal Controller : PID Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class PIDLongitudinalController():\n", " def __init__(\n", " self,\n", " p_gain: float = +1.2, # P Gain\n", " i_gain: float = +0.4, # I Gain\n", " d_gain: float = +0.1, # D Gain\n", " target_velocity: float = 3.0, # [m/s]\n", " ) -> None:\n", " \"\"\"initialize pid controller for keeping constant velocity\"\"\"\n", " # pid control parameters\n", " self.K_p = p_gain\n", " self.K_i = i_gain\n", " self.K_d = d_gain\n", " self.target_vel = target_velocity\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # calculate tracking error, its integral and derivative\n", " r = self.target_vel\n", " y = observed_vel\n", " e = r - y # tracking error to the traget velocity\n", " ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\n", " de = (e - self.pre_e) / delta_t # derivative of the tracking error\n", "\n", " # calculate control input\n", " acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\n", "\n", " # update previous tracking error\n", " self.pre_e = e\n", "\n", " return acc_cmd\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Lateral Controller : Pure Pursuit Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class PurePursuitLateralController():\n", " def __init__(\n", " self,\n", " look_ahead_dist: float = 5.0, # [m]\n", " wheel_base: float = 2.5, # [m]\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " ) -> None:\n", " \"\"\"initialize pure pursuit controller for path-tracking\"\"\"\n", " # pure pursuit control parameters\n", " self.look_ahead_dist = look_ahead_dist\n", " self.wheel_base = wheel_base\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " # ref_path info\n", " self.ref_path = ref_path\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # set vehicle state variables from observation\n", " x = observed_x[0]\n", " y = observed_x[1]\n", " yaw = observed_x[2]\n", "\n", " # get the waypoint closest to current vehicle position \n", " _, ref_x, ref_y, ref_yaw, _ = self._get_lookahead_waypoint(x, y, self.look_ahead_dist, update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # calculate control input\n", " alpha = np.arctan2(ref_y - y, ref_x - x) - yaw # vehicle heading angle error\n", " steer_cmd = np.arctan2(2.0 * self.wheel_base * np.sin(alpha) / self.look_ahead_dist, 1.0) # given from geometric relationship\n", "\n", " return steer_cmd, [ref_x, ref_y]\n", "\n", " def _get_lookahead_waypoint(self, x: float, y: float, look_ahead_dist: float, update_prev_idx: bool = False):\n", " \"\"\"search the target waypoint to the vehicle on the reference path\"\"\"\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [np.abs(-look_ahead_dist**2 + idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " target_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the look-ahead waypoint\n", " ref_x = self.ref_path[target_idx,0]\n", " ref_y = self.ref_path[target_idx,1]\n", " ref_yaw = self.ref_path[target_idx,2]\n", " ref_v = self.ref_path[target_idx,3]\n", "\n", " # update target waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = target_idx\n", "\n", " return target_idx, ref_x, ref_y, ref_yaw, ref_v" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : PID Controller\n", "- Lateral Control : Pure Pursuit Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize pid controller for acceleration control\n", "pid_lon_controller = PIDLongitudinalController(\n", " p_gain = 1.2,\n", " i_gain = 0.4,\n", " d_gain = 0.1,\n", " target_velocity = +5.0 # [m/s]\n", ")\n", "\n", "# initialize pure pursuit controller for steering control\n", "purepursuit_lat_controller = PurePursuitLateralController(\n", " look_ahead_dist = 5.0, # [m]\n", " ref_path = ref_path, # ndarray, size is \n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate control inputs\n", " current_velocity = current_state[3]\n", " accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\n", " steer_input, look_ahead_point = purepursuit_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\n", "\n", " except IndexError as ex:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory, look_ahead_point=look_ahead_point) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"purepursuit_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/stanley.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Stanley Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle \n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Longitudinal Controller : PID Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class PIDLongitudinalController():\n", " def __init__(\n", " self,\n", " p_gain: float = +1.2, # P Gain\n", " i_gain: float = +0.4, # I Gain\n", " d_gain: float = +0.1, # D Gain\n", " target_velocity: float = 3.0, # [m/s]\n", " ) -> None:\n", " \"\"\"initialize pid controller for keeping constant velocity\"\"\"\n", " # pid control parameters\n", " self.K_p = p_gain\n", " self.K_i = i_gain\n", " self.K_d = d_gain\n", " self.target_vel = target_velocity\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # calculate tracking error, its integral and derivative\n", " r = self.target_vel\n", " y = observed_vel\n", " e = r - y # tracking error to the traget velocity\n", " ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\n", " de = (e - self.pre_e) / delta_t # derivative of the tracking error\n", "\n", " # calculate control input\n", " acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\n", "\n", " # update previous tracking error\n", " self.pre_e = e\n", "\n", " return acc_cmd\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Lateral Controller : Stanley Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class StanleyLateralController():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m] distance between the center of gravity and the front axle\n", " cross_track_error_gain: float = 0.5, # gain for the lateral error\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " ) -> None:\n", " \"\"\"initialize stanley controller for path-tracking\"\"\"\n", " # stanley control parameters\n", " self.k = cross_track_error_gain # gain for the lateral error\n", " self.l_f = l_f # [m] distance between the center of gravity and the front axle\n", "\n", " # ref_path info\n", " self.ref_path = ref_path\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # set vehicle state variables from observation\n", " x = observed_x[0]\n", " y = observed_x[1]\n", " yaw = observed_x[2]\n", " v = observed_x[3]\n", "\n", " # get the waypoint closest to current vehicle position\n", " front_x, front_y = x + self.l_f * np.cos(yaw), y + self.l_f * np.sin(yaw)\n", " _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(front_x, front_y, update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # which side of the reference path is the car on, the right or the left?\n", " ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n", " x1, y1 = ref_x, ref_y\n", " x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0*np.sin(ref_yaw)\n", " vx, vy = x2 - x1, y2 - y1\n", " wx, wy = front_x - x1, front_y - y1\n", " s = vx * wy - vy * wx # s>0 : vehicle is on the left of the path, s<0 : vehicle is on the left of the path,\n", "\n", " # get tracking error\n", " e = -np.sign(s) * np.sqrt((ref_x-front_x)**2 + (ref_y-front_y)**2) # lateral error \n", " alpha = ref_yaw - yaw # heading error\n", " alpha = np.arctan2(np.sin(alpha), np.cos(alpha)) # normalize heading error to [-pi, pi]\n", "\n", " # calculate control input\n", " epsilon = 0.1 # to avoid zero division when v is too small\n", " steer_cmd = alpha + np.arctan2(self.k * e, v + epsilon)\n", "\n", " return steer_cmd\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : PID Controller\n", "- Lateral Control : Stanley Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize pid controller for acceleration control\n", "pid_lon_controller = PIDLongitudinalController(\n", " p_gain = 1.2,\n", " i_gain = 0.4,\n", " d_gain = 0.1,\n", " target_velocity = +5.0 # [m/s]\n", ")\n", "\n", "# initialize stanley controller for steering control\n", "stanley_lat_controller = StanleyLateralController(\n", " cross_track_error_gain = 1.5, # gain for the lateral error\n", " ref_path = ref_path, # ndarray, size is \n", ")\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate control inputs\n", " current_velocity = current_state[3]\n", " accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\n", " steer_input = stanley_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\n", "\n", " except IndexError as ex:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"stanley_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/state_feedback.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# State Feedback Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Control Target : Vehicle \n", "- Longitudinal dynamics : Point Mass Model\n", "- Lateral dynamics : Kinematic Bicycle Model" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " l_f: float = 1.5, # [m]\n", " l_r: float = 1.0, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.l_f = l_f # [m]\n", " self.l_r = l_r # [m]\n", " self.wheel_base = l_f + l_r # [m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " l_r = self.l_r\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " beta = np.arctan(l_r / l * np.tan(steer))\n", " new_x = x + v * np.cos(yaw + beta) * dt\n", " new_y = y + v * np.sin(yaw + beta) * dt\n", " new_yaw = yaw + v / l * np.sin(beta) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Longitudinal Controller : PID Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class PIDLongitudinalController():\n", " def __init__(\n", " self,\n", " p_gain: float = +1.2, # P Gain\n", " i_gain: float = +0.4, # I Gain\n", " d_gain: float = +0.1, # D Gain\n", " target_velocity: float = 3.0, # [m/s]\n", " ) -> None:\n", " \"\"\"initialize pid controller for keeping constant velocity\"\"\"\n", " # pid control parameters\n", " self.K_p = p_gain\n", " self.K_i = i_gain\n", " self.K_d = d_gain\n", " self.target_vel = target_velocity\n", " self.pre_e = 0.0 # previous tracking error\n", " self.integrated_e = 0.0 # integrated tracking error\n", "\n", " def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # calculate tracking error, its integral and derivative\n", " r = self.target_vel\n", " y = observed_vel\n", " e = r - y # tracking error to the traget velocity\n", " ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\n", " de = (e - self.pre_e) / delta_t # derivative of the tracking error\n", "\n", " # calculate control input\n", " acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\n", "\n", " # update previous tracking error\n", " self.pre_e = e\n", "\n", " return acc_cmd\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Lateral Controller : State Feedback Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class StateFeedbackLateralController():\n", " def __init__(\n", " self,\n", " f : np.ndarray = np.array([1.0, 1.0]), # feedback gain matrix\n", " ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n", " ) -> None:\n", " \"\"\"initialize state feedback controller for path-tracking\"\"\"\n", " # feedback gain matrix\n", " self.f = f\n", "\n", " # ref_path info\n", " self.ref_path = ref_path\n", " self.prev_waypoints_idx = 0\n", "\n", " def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\n", " \"\"\"calculate control input\"\"\"\n", "\n", " # set vehicle state variables from observation\n", " x = observed_x[0]\n", " y = observed_x[1]\n", " yaw = observed_x[2]\n", " v = observed_x[3]\n", "\n", " # get the waypoint closest to current vehicle position\n", " _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\n", " if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n", " print(\"[ERROR] Reached the end of the reference path.\")\n", " raise IndexError\n", "\n", " # which side of the reference path is the car on, the right or the left?\n", " ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n", " x1, y1 = ref_x, ref_y\n", " x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0 * np.sin(ref_yaw)\n", " vx, vy = x2 - x1, y2 - y1\n", " wx, wy = x - x1, y - y1\n", " s = vx * wy - vy * wx # s>0 : vehicle is on the left of the path, s<0 : vehicle is on the left of the path,\n", "\n", " # get tracking error\n", " y_e = np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # lateral error \n", " theta_e = yaw - ref_yaw # heading error\n", " theta_e = np.arctan2(np.sin(theta_e), np.cos(theta_e)) # normalize heading error to [-pi, pi]\n", "\n", " # calculate control input\n", " steer_cmd = -self.f @ np.array([y_e, theta_e])\n", "\n", " return steer_cmd[0]\n", "\n", " def solve_are(self, A, B, Q, R):\n", " \"\"\"solve algebraic riccati equation with the Arimoto-Potter algorithm\n", " Ref: https://qiita.com/trgkpc/items/8210927d5b035912a153\n", " \"\"\"\n", " # define hamiltonian matrix\n", " H = np.block([[A, -B @ np.linalg.inv(R) @ B.T],\n", " [-Q , -A.T]])\n", "\n", " # solve eigenvalue problem\n", " eigenvalue, w = np.linalg.eig(H)\n", "\n", " # define Y and Z, which are used to calculate P\n", " Y_, Z_ = [], []\n", " n = len(w[0])//2\n", "\n", " # sort eigenvalues\n", " index_array = sorted([i for i in range(2*n)],\n", " key = lambda x:eigenvalue[x].real)\n", "\n", " # choose n eigenvalues which have smaller real part\n", " for i in index_array[:n]:\n", " Y_.append(w.T[i][:n])\n", " Z_.append(w.T[i][n:])\n", " Y = np.array(Y_).T\n", " Z = np.array(Z_).T\n", "\n", " # calculate P\n", " if np.linalg.det(Y) != 0:\n", " return Z @ np.linalg.inv(Y)\n", " else:\n", " print(\"Warning: Y is not regular matrix. Result may be wrong!\") # TODO : need to consider mathmatical meaning of this case.\n", " return Z @ np.linalg.pinv(Y)\n", "\n", " def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n", " \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n", " SEARCH_IDX_LEN = 200 # [points] forward search range\n", " prev_idx = self.prev_waypoints_idx\n", " dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n", " dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n", " d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n", " min_d = min(d)\n", " nearest_idx = d.index(min_d) + prev_idx\n", "\n", " # get reference values of the nearest waypoint\n", " ref_x = self.ref_path[nearest_idx,0]\n", " ref_y = self.ref_path[nearest_idx,1]\n", " ref_yaw = self.ref_path[nearest_idx,2]\n", " ref_v = self.ref_path[nearest_idx,3]\n", "\n", " # update nearest waypoint index if necessary\n", " if update_prev_idx:\n", " self.prev_waypoints_idx = nearest_idx \n", "\n", " return nearest_idx, ref_x, ref_y, ref_yaw, ref_v" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Simulation of Path-Tracking\n", "- Longitudinal Control : PID Controller\n", "- Lateral Control : State Feedback Controller" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "delta_t = 0.05 # [sec]\n", "sim_steps = 1000 # [steps]\n", "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n", "\n", "# load and visualize reference path\n", "ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n", "plt.title(\"Reference Path\")\n", "plt.plot(ref_path[:,0], ref_path[:,1])\n", "plt.show()\n", "\n", "# initialize a vehicle as a control target\n", "vehicle = Vehicle(\n", " max_steer_abs=0.523, # [rad]\n", " max_accel_abs=2.000, # [m/s^2]\n", " ref_path = ref_path[:, 0:2], # ndarray, size is \n", ")\n", "vehicle.reset(\n", " init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n", ")\n", "vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n", "\n", "# initialize pid controller for acceleration control\n", "pid_lon_controller = PIDLongitudinalController(\n", " p_gain = 1.2,\n", " i_gain = 0.4,\n", " d_gain = 0.1,\n", " target_velocity = +5.0 # [m/s]\n", ")\n", "\n", "# initialize state feedback controller for steering control\n", "state_feedback_lat_controller = StateFeedbackLateralController(\n", " f = np.array([[2.0, 2.0]]), # feedback gain matrix\n", " ref_path = ref_path, # ndarray, size is \n", ")\n", "\"\"\"Note : \n", "The feedback gain matrix f can be calculated with pole placement method, for example.\n", "\"\"\"\n", "\n", "# simulation loop\n", "for i in range(sim_steps):\n", "\n", " # get current state of vehicle\n", " current_state = vehicle.get_state()\n", "\n", " try:\n", " # calculate control inputs\n", " current_velocity = current_state[3]\n", " accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\n", " steer_input = state_feedback_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\n", "\n", " except IndexError as ex:\n", " # the vehicle has reached the end of the reference path\n", " print(\"[ERROR] IndexError detected. Terminate simulation.\")\n", " break\n", "\n", " # print current state and input force\n", " print(steer_input)\n", " print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n", "\n", " # update states of vehicle\n", " print(\"steer_input\", steer_input)\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation\n", "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"state_feedback_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.5" } }, "nbformat": 4, "nbformat_minor": 4 } ================================================ FILE: notebooks/unicycle_model.ipynb ================================================ { "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# Unicycle Model\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Definition of Coordinate Systems\n", "\n", "\"definition_of_frames\"\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## State Equation\n", "\n", "\"UM\"\n", "\n", "$$\n", "\\begin{align}\n", "\\frac{\\mathrm{d}}{\\mathrm{d}t}\n", "\\begin{bmatrix}\n", "p^G_x \\\\\n", "p^G_y \\\\\n", "\\phi \\\\\n", "V\n", "\\end{bmatrix}\n", "=\n", "\\begin{bmatrix}\n", "V \\cos\\phi \\\\\n", "V \\sin\\phi \\\\\n", "( V / l ) \\tan{\\delta} \\\\\n", "{a}\n", "\\end{bmatrix}.\n", "\\end{align}\n", "$$\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from typing import Tuple\n", "from matplotlib import patches\n", "from matplotlib.animation import ArtistAnimation\n", "from IPython import display" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Vehicle():\n", " def __init__(\n", " self,\n", " wheel_base: float = 2.5, # [m]\n", " max_steer_abs: float = 0.523, # [rad]\n", " max_accel_abs: float = 2.000, # [m/s^2]\n", " ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n", " delta_t: float = 0.05, # [s]\n", " visualize: bool = True,\n", " ) -> None:\n", " \"\"\"initialize vehicle environment\n", " state variables:\n", " x: x-axis position in the global frame [m]\n", " y: y-axis position in the global frame [m]\n", " yaw: orientation in the global frame [rad]\n", " v: longitudinal velocity [m/s]\n", " control input:\n", " steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n", " accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n", " Note: dynamics of the vehicle is the Unicycle Model. \n", " \"\"\"\n", " # vehicle parameters\n", " self.wheel_base = wheel_base#[m]\n", " self.max_steer_abs = max_steer_abs # [rad]\n", " self.max_accel_abs = max_accel_abs # [m/s^2]\n", " self.delta_t = delta_t #[s]\n", " self.ref_path = ref_path\n", "\n", " # visualization settings\n", " self.vehicle_w = 3.00\n", " self.vehicle_l = 4.00\n", " self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n", " self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n", "\n", " # reset environment\n", " self.visualize_flag = visualize\n", " self.reset()\n", "\n", " def reset(\n", " self, \n", " init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n", " ) -> None:\n", " \"\"\"reset environment to initial state\"\"\"\n", "\n", " # reset state variables\n", " self.state = init_state\n", "\n", " # clear animation frames\n", " self.frames = []\n", "\n", " if self.visualize_flag:\n", " # prepare figure\n", " self.fig = plt.figure(figsize=(9,9))\n", " self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n", " self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n", " self.steer_ax = plt.subplot2grid((3,4), (1,3))\n", " self.accel_ax = plt.subplot2grid((3,4), (2,3))\n", "\n", " # graph layout settings\n", " ## main view\n", " self.main_ax.set_aspect('equal')\n", " self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n", " self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n", " self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n", " self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n", " ## mini map\n", " self.minimap_ax.set_aspect('equal')\n", " self.minimap_ax.axis('off')\n", " ## steering angle view\n", " self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n", " self.steer_ax.axis('off')\n", " ## acceleration view\n", " self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n", " self.accel_ax.axis('off')\n", " \n", " # apply tight layout\n", " self.fig.tight_layout()\n", "\n", " def update(\n", " self, \n", " u: np.ndarray, \n", " delta_t: float = 0.0, \n", " append_frame: bool = True, \n", " vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n", " ) -> None:\n", " \"\"\"update state variables\"\"\"\n", " # keep previous states\n", " x, y, yaw, v = self.state\n", "\n", " # prepare params\n", " l = self.wheel_base\n", " dt = self.delta_t if delta_t == 0.0 else delta_t\n", "\n", " # limit control inputs\n", " steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n", " accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n", "\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", " # update state variables\n", " new_x = x + v * np.cos(yaw) * dt\n", " new_y = y + v * np.sin(yaw) * dt\n", " new_yaw = yaw + v / l * np.tan(steer) * dt\n", " new_v = v + accel * dt\n", " self.state = np.array([new_x, new_y, new_yaw, new_v])\n", " \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n", "\n", " # record frame\n", " if append_frame:\n", " self.append_frame(steer, accel, vehicle_traj)\n", "\n", " def get_state(self) -> np.ndarray:\n", " \"\"\"return state variables\"\"\"\n", " return self.state.copy()\n", "\n", " def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n", " \"\"\"draw a frame of the animation.\"\"\"\n", " # get current states\n", " x, y, yaw, v = self.state\n", "\n", " ### main view ###\n", " # draw the vehicle shape\n", " vw, vl = self.vehicle_w, self.vehicle_l\n", " vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n", " vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n", " rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n", " frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n", "\n", " # draw wheels\n", " ww, wl = 0.4, 0.7 #[m]\n", " wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n", " wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n", "\n", " ## rear-left wheel\n", " wheel_shape_rl_x, wheel_shape_rl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n", " wheel_rl_x, wheel_rl_y = \\\n", " self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n", "\n", " ## rear-right wheel\n", " wheel_shape_rr_x, wheel_shape_rr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n", " wheel_rr_x, wheel_rr_y = \\\n", " self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n", "\n", " ## front-left wheel\n", " wheel_shape_fl_x, wheel_shape_fl_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n", " wheel_fl_x, wheel_fl_y = \\\n", " self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n", "\n", " ## front-right wheel\n", " wheel_shape_fr_x, wheel_shape_fr_y = \\\n", " self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n", " wheel_fr_x, wheel_fr_y = \\\n", " self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n", " frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n", "\n", " # draw the vehicle center circle\n", " vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n", " frame += [self.main_ax.add_artist(vehicle_center)]\n", "\n", " # draw the reference path\n", " ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n", " ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n", " frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n", "\n", " # draw the information text\n", " text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n", " frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n", "\n", " # draw vehicle trajectory\n", " if vehicle_traj.any():\n", " vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n", " vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n", " frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n", "\n", " ### mini map view ###\n", " frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n", " rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n", " self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n", " frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n", " frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n", " if vehicle_traj.any():\n", " frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n", "\n", " ### control input view ###\n", " # steering angle\n", " MAX_STEER = self.max_steer_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " s_abs = np.abs(steer)\n", " if steer < 0.0: # when turning right\n", " steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else: # when turning left\n", " steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += steer_pie_obj\n", " frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # acceleration\n", " MAX_ACCEL = self.max_accel_abs\n", " PIE_RATE = 3.0/4.0\n", " PIE_STARTANGLE = 225 # [deg]\n", " a_abs = np.abs(accel)\n", " if accel > 0.0:\n", " accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " else:\n", " accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n", " frame += accel_pie_obj\n", " frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n", "\n", " # append frame\n", " self.frames.append(frame)\n", "\n", " # rotate shape and return location on the x-y plane.\n", " def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n", " transformed_x = []\n", " transformed_y = []\n", " if len(xlist) != len(ylist):\n", " print(\"[ERROR] xlist and ylist must have the same size.\")\n", " raise AttributeError\n", "\n", " for i, xval in enumerate(xlist):\n", " transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n", " transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n", " transformed_x.append(transformed_x[0])\n", " transformed_y.append(transformed_y[0])\n", " return transformed_x, transformed_y\n", "\n", " def show_animation(self, interval_ms: int) -> None:\n", " \"\"\"show animation of the recorded frames\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n", " html = display.HTML(ani.to_jshtml())\n", " display.display(html)\n", " plt.close()\n", "\n", " def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n", " \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n", " ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n", " ani.save(filename, writer=movie_writer)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Run Simulation" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Zig Zag Run" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 100 # [step]\n", "delta_t = 0.1 # [s]\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-3.0, 50.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, v]\n", "vehicle_trajectory = np.array(vehicle.get_state()[:2])\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", " steer_input = 0.6 * np.sin(i/5.0) # steering command [rad]\n", " accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"ucm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Steady Input" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# simulation settings\n", "sim_step = 200 # [step]\n", "delta_t = 0.1 # [s]\n", "\n", "# initialize vehicle simulator\n", "ref_path_x = np.linspace(-25.0, 25.0, 10)\n", "ref_path_y = np.zeros(10)\n", "vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n", "vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 5.5])) # set initial state of the vehicle, [x, y, yaw, v]\n", "vehicle_trajectory = np.array(vehicle.get_state()[:2])\n", "\n", "# simulation loop\n", "for i in range(sim_step):\n", " steer_input = 0.3 # steering command [rad]\n", " accel_input = 2.0 # acceleration command [m/s^2]\n", " vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n", " vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n", "\n", "# show animation on jupyter notebook\n", "vehicle.show_animation(interval_ms=delta_t*1000)\n", "\n", "# save animation as a mp4 file if necessary\n", "# vehicle.save_animation(\"ucm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file" ] } ], "metadata": { "kernelspec": { "display_name": ".venv", "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.10.5" }, "orig_nbformat": 4 }, "nbformat": 4, "nbformat_minor": 2 } ================================================ FILE: pyproject.toml ================================================ [tool.poetry] name = "path-tracking" version = "0.1.0" description = "" authors = ["MizuhoAOKI "] readme = "README.md" packages = [{include = "path_tracking"}] [tool.poetry.dependencies] python = ">=3.10,<3.13" matplotlib = "^3.8.0" numpy = "^1.26.0" notebook = "^7.0.4" sympy = "^1.12" [build-system] requires = ["poetry-core"] build-backend = "poetry.core.masonry.api"