[
  {
    "path": ".gitignore",
    "content": "*.mp4\n.ipynb_checkpoints\n__pycache__/\n*.pyc\n\n# Packages\n*.egg\n!/tests/**/*.egg\n/*.egg-info\n/dist/*\nbuild\n_build\n.cache\n*.so\n\n# Installer logs\npip-log.txt\n\n# Unit test / coverage reports\n.coverage\n.pytest_cache\n\n.DS_Store\n.idea/*\n# .python-version\n.vscode/*\n\n/test.py\n/test_*.*\n\n/setup.cfg\nMANIFEST.in\n/setup.py\n/docs/site/*\n/tests/fixtures/simple_project/setup.py\n/tests/fixtures/project_with_extras/setup.py\n.mypy_cache\n\n.venv\n/releases/*\npip-wheel-metadata\n/poetry.toml\n\npoetry/core/*"
  },
  {
    "path": ".python-version",
    "content": "3.10.5\n"
  },
  {
    "path": "LICENSE.txt",
    "content": "MIT License\n\nCopyright (c) 2024 MizuhoAOKI\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n"
  },
  {
    "path": "README.md",
    "content": "[![License: MIT](https://img.shields.io/badge/License-MIT-blue.svg)](https://opensource.org/licenses/MIT)\n[![Poetry](https://img.shields.io/endpoint?url=https://python-poetry.org/badge/v0.json)](https://python-poetry.org/)\n![](https://geps.dev/progress/36)\n\n# Path Tracking Catalog\n25 path-tracking algorithms are (goint to be) implemented with python.\n\nhttps://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/1b2b39f6-87fc-4f3d-9b18-4ae32f71b5fd\n\n- [Vehicle Models for Simulation](#vehicle-models-for-simulation)\n  - [x] [Dynamic Bicycle Model](#dynamic-bicycle-model)\n  - [x] [Kinematic Bicycle Model](#kinematic-bicycle-model)\n  - [x] [Unicycle Model](#unicycle-model)\n- [Control Algorithms](#control-algorithms)\n  - [x] [Bang-Bang Control](#bang-bang-control)\n  - [x] [PID Control](#pid-control)\n  - [x] [Pure Pursuit Control](#pure-pursuit-control)\n  - [x] [Stanley Control](#stanley-control)\n  - [x] [Fuzzy Logic Control](#fuzzy-logic-control)\n  - [ ] Genetic Algorithm\n  - [x] [Dynamic Window Approach](#dynamic-window-approach)\n  - [ ] State Lattice Planner\n  - [x] [State Feedback Control](#state-feedback-control)\n  - [x] [Linear Quadratic Regulator (infinite horizon)](#linear-quadratic-regulator)\n  - [ ] Linear Quadratic Regulator (finite horizon)\n  - [ ] Differential Dynamic Programming (infinite horizon)\n  - [ ] Differential Dynamic Programming (finite horizon)\n  - [ ] Iterative LQR (infinite horizon)\n  - [ ] Iterative LQR (finite horizon)\n  - [ ] Linear Model Predictive Control (formulated as Quadratic Programming)\n  - [ ] Nonlinear Model Predictive Control (solved by C/GMRES method)\n  - [x] [Model Predictive Path-Integral Control](#model-predictive-path-integral-control)\n  - [ ] Sliding Mode Control\n  - [ ] Q-Learning\n  - [ ] Multi Layer Perceptron\n  - [ ] Linear Quadratic Gaussian\n  - [ ] H∞ Control (LMI)\n  - [ ] Lyapunov\n  - [ ] Adaptive Control\n\n## Setup\n```sh\ngit clone https://github.com/MizuhoAOKI/path_tracking_catalog.git\ncd path_tracking_catalog\npoetry install\n```\n\n## Vehicle Models for Simulation\n\n### Definition of Coordinate Systems\n<img src=\"./media/def_of_frames.svg\" width=\"500px\" alt=\"definition_of_frames\">\n\n### Dynamic Bicycle Model\n\n```math\n\\begin{align}\n&\\frac{\\mathrm{d}}{\\mathrm{d}t}\n\\begin{bmatrix}\np^G_x \\\\\np^G_y \\\\\n\\phi \\\\\nv^B_x \\\\\nv^B_y \\\\\n\\omega \\\\\n\\end{bmatrix}\n=\n\\begin{bmatrix}\nv^B_x \\cos\\phi - v^B_y \\sin\\phi \\\\\nv^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<p align=\"center\">\n<img src=\"./media/DBM.svg\" width=\"500px\" alt=\"DBM\" />\n</p>\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/d51c3821-9b35-4e91-8235-e63b18f33f03\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/dynamic_bicycle_model.ipynb\n```\n\n### Kinematic Bicycle Model\n\n```math\n\\begin{align}\n& \\frac{\\mathrm{d}}{\\mathrm{d}t}\n\\begin{bmatrix}\np^G_x \\\\\np^G_y \\\\\n\\phi \\\\\nV\n\\end{bmatrix}\n=\n\\begin{bmatrix}\nV \\cos(\\phi + \\beta) \\\\\nV \\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\n<p align=\"center\">\n<img src=\"./media/KBM.svg\" width=\"500px\" alt=\"KBM\" />\n</p>\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/b85fe31c-3e4a-47a9-bc54-694cde225bd5\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/kinematic_bicycle_model.ipynb\n```\n\n### Unicycle Model\n\n```math\n\\begin{align}\n\\frac{\\mathrm{d}}{\\mathrm{d}t}\n\\begin{bmatrix}\np^G_x \\\\\np^G_y \\\\\n\\phi \\\\\nV\n\\end{bmatrix}\n=\n\\begin{bmatrix}\nV \\cos\\phi \\\\\nV \\sin\\phi \\\\\n( V / l ) \\tan{\\delta} \\\\\n{a}\n\\end{bmatrix}.\n\\end{align}\n```\n\n<p align=\"center\">\n<img src=\"./media/UM.svg\" width=\"500px\" alt=\"UM\" />\n</p>\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/8cba0010-6a21-4830-8974-b4b57c166bcf\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/unicycle_model.ipynb\n```\n\n## Control Algorithms\n### Bang-Bang Control\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/cc88214e-3914-4126-ac57-3f63d8397094\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/bangbang.ipynb\n```\n\n### PID Control\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/83e813d9-b611-49da-abe2-45333bfb80d2\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/pid.ipynb\n```\n\n### Pure-Pursuit Control\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/a23f8437-d695-4848-83fb-a8424f311683\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/purepursuit.ipynb\n```\n\n### Stanley Control\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/43f3ce4f-8181-45ad-bc08-ac96f3a91e2b\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/stanley.ipynb\n```\n\n### Fuzzy Logic Control\n\nhttps://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/d8f9f256-4b2c-4b9e-8b57-e33f05e59e6f\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/fuzzy.ipynb\n```\n\n### Dynamic Window Approach\n\n#### Simple Path Tracking\n\nhttps://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/23078e50-46a5-48eb-b89b-4a2111b320f0\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/dwa_pathtracking.ipynb\n```\n\n#### Path Tracking with Obstacle Avoidance\n\nhttps://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/7b2a80b7-8d3e-4d37-89f4-23337efcb937\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/dwa_obstacle_avoidance.ipynb\n```\n\n### State Lattice Planner\n\n\n### State Feedback Control\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/8e48380b-f840-49cc-91e0-07dad356b5be\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/state_feedback.ipynb\n```\n\n### Linear Quadratic Regulator\n\nhttps://github.com/MizuhoAOKI/path_tracking/assets/63337525/13d6abbc-2904-422d-acba-17ede074a181\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/lqr.ipynb\n```\n\n### Model Predictive Control\n\n\n### Model Predictive Path-Integral Control\n\n#### Simple Path Tracking\n\nhttps://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/acfefe3a-a22a-4cbc-a5c8-a83086943684\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/mppi_pathtracking.ipynb\n```\n\n#### Path Tracking with Obstacle Avoidance\n\nhttps://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/fe84eaf6-8795-490d-9ef1-efecc427052e\n\n```sh\ncd path_tracking_catalog\npoetry run jupyter notebook notebooks/mppi_obstacle_avoidance.ipynb\n```\n\n### Sliding Mode Control\n\n\n### Q-Learning\n\n"
  },
  {
    "path": "media/.gitkeep",
    "content": ""
  },
  {
    "path": "notebooks/bangbang.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Bang-Bang Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle \\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Controller : Bang-Bang Controller\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Longitudinal Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class BangBangLongitudinalController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            acceleration_cmd: float = +1.0, # [m/s^2]\\n\",\n    \"            deceleration_cmd: float = -1.0, # [m/s-2]\\n\",\n    \"            target_velocity: float = 3.0, # [m/s]\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize bang-bang controller for keeping constant velocity\\\"\\\"\\\"\\n\",\n    \"        self.acc_cmd = acceleration_cmd\\n\",\n    \"        self.dec_cmd = deceleration_cmd\\n\",\n    \"        self.target_vel = target_velocity\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_vel: float) -> None:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"        if observed_vel < self.target_vel:\\n\",\n    \"            return self.acc_cmd\\n\",\n    \"        else:\\n\",\n    \"            return self.dec_cmd\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Keeping Speed Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 100 # [step]\\n\",\n    \"delta_t = 0.1 # [s]\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-3.0, 50.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize speed controller\\n\",\n    \"speed_controller = BangBangLongitudinalController(\\n\",\n    \"    acceleration_cmd = +1.0, # [m/s^2]\\n\",\n    \"    deceleration_cmd = -1.0, # [m/s]\\n\",\n    \"    target_velocity = 3.0, # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"    current_velocity = current_state[3]\\n\",\n    \"\\n\",\n    \"    # calculate control inputs\\n\",\n    \"    steer_input = 0.0  # steering command [rad] # set zero because this is just a test run of speed controller\\n\",\n    \"    accel_input = speed_controller.calc_control_input(observed_vel=current_velocity) # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"bangbang_speed_control_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Lateral Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class BangBangLateralController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            left_steering_cmd: float = 0.5, # [rad]\\n\",\n    \"            right_steering_cmd: float = 0.5, # [rad]\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize bang-bang controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # bang-bang control parameters\\n\",\n    \"        self.left_steering_cmd = left_steering_cmd\\n\",\n    \"        self.right_steering_cmd = right_steering_cmd\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # set vehicle state variables from observation\\n\",\n    \"        x = observed_x[0]\\n\",\n    \"        y = observed_x[1]\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position \\n\",\n    \"        _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # which side of the reference path is the car on, the right or the left?\\n\",\n    \"        ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\\n\",\n    \"        x1, y1 = ref_x, ref_y\\n\",\n    \"        x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0*np.sin(ref_yaw)\\n\",\n    \"        vx, vy = x2 - x1, y2 - y1\\n\",\n    \"        wx, wy =  x - x1,  y - y1\\n\",\n    \"        s = vx * wy - vy * wx\\n\",\n    \"\\n\",\n    \"        if s > 0:\\n\",\n    \"            # vehicle is on the left\\n\",\n    \"            return self.right_steering_cmd\\n\",\n    \"        else:\\n\",\n    \"            # vehicle is on the right\\n\",\n    \"            return self.left_steering_cmd\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : Bang-Bang Controller\\n\",\n    \"- Lateral Control : Bang-Bang Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize bang-bang controllers for the vehicle\\n\",\n    \"bangbang_lon_controller = BangBangLongitudinalController(\\n\",\n    \"    acceleration_cmd = +1.0, # [m/s^2]\\n\",\n    \"    deceleration_cmd = -1.0, # [m/s^2]\\n\",\n    \"    target_velocity = +5.0 # [m/s]\\n\",\n    \")\\n\",\n    \"bangbang_lat_controller = BangBangLateralController(\\n\",\n    \"    left_steering_cmd = +0.5, # [rad]\\n\",\n    \"    right_steering_cmd = -0.5, # [rad]\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate control inputs\\n\",\n    \"        current_velocity = current_state[3]\\n\",\n    \"        accel_input = bangbang_lon_controller.calc_control_input(observed_vel=current_velocity)\\n\",\n    \"        steer_input = bangbang_lat_controller.calc_control_input(observed_x=current_state)\\n\",\n    \"\\n\",\n    \"    except IndexError as ex:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"bangbang_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/dwa_obstacle_avoidance.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Dynamic Window Approach\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle\\n\",\n    \"\\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            vehicle_width = 3.0, # [m]\\n\",\n    \"            vehicle_length = 4.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\\n\",\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\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.wheel_base = wheel_base#[m]\\n\",\n    \"        self.vehicle_w = vehicle_width\\n\",\n    \"        self.vehicle_l = vehicle_length\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # obstacle parameters\\n\",\n    \"        self.obstacle_circles = obstacle_circles\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"        self.minimap_view_x_lim_min, self.minimap_view_x_lim_max = -40.0, 40.0\\n\",\n    \"        self.minimap_view_y_lim_min, self.minimap_view_y_lim_max = -10.0, 40.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            self.minimap_ax.set_xlim(self.minimap_view_x_lim_min, self.minimap_view_x_lim_max)\\n\",\n    \"            self.minimap_ax.set_ylim(self.minimap_view_y_lim_min, self.minimap_view_y_lim_max)\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"            optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\\n\",\n    \"            sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\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\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw the predicted optimal trajectory from mppi\\n\",\n    \"        if optimal_traj.any():\\n\",\n    \"            optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\\n\",\n    \"            optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        # draw the sampled trajectories from mppi\\n\",\n    \"        if sampled_traj_list.any():\\n\",\n    \"            min_alpha_value = 0.25\\n\",\n    \"            max_alpha_value = 0.35\\n\",\n    \"            for idx, sampled_traj in enumerate(sampled_traj_list):\\n\",\n    \"                # draw darker for better samples\\n\",\n    \"                alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\\n\",\n    \"                sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\\n\",\n    \"                sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        # draw the circular obstacles in the main view\\n\",\n    \"        for obs in self.obstacle_circles:\\n\",\n    \"            obs_x, obs_y, obs_r = obs\\n\",\n    \"            obs_circle = patches.Circle([obs_x-x, obs_y-y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\\n\",\n    \"            frame += [self.main_ax.add_artist(obs_circle)]\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        # draw the circular obstacles in the mini map view\\n\",\n    \"        for obs in self.obstacle_circles:\\n\",\n    \"            obs_x, obs_y, obs_r = obs\\n\",\n    \"            obs_circle = patches.Circle([obs_x, obs_y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\\n\",\n    \"            frame += [self.minimap_ax.add_artist(obs_circle)]\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Controller : Dynamic Window Approach\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class DWAControllerForPathTracking():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            delta_t: float = 0.05,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            vehicle_width: float = 3.0, # [m]\\n\",\n    \"            vehicle_length: float = 4.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"            horizon_step_T: int = 30,\\n\",\n    \"            param_steer_resolution: float = 0.01, # [rad]\\n\",\n    \"            param_accel_resolution: float = 0.025, # [m/s^2]\\n\",\n    \"            param_steer_window_size: float = 0.2, # [rad]\\n\",\n    \"            param_accel_window_size: float = 0.2, # [m/s^2]\\n\",\n    \"            stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            visualize_optimal_traj = True,  # if True, optimal trajectory is visualized\\n\",\n    \"            visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\\n\",\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\",\n    \"            collision_safety_margin_rate: float = 1.2, # safety margin for collision check\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize dwa controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # dwa parameters\\n\",\n    \"        self.dim_x = 4 # dimension of system state vector\\n\",\n    \"        self.dim_u = 2 # dimension of control input vector\\n\",\n    \"        self.T = horizon_step_T # prediction horizon\\n\",\n    \"        self.param_steer_resolution = param_steer_resolution # [rad]\\n\",\n    \"        self.param_accel_resolution = param_accel_resolution # [m/s^2]\\n\",\n    \"        self.param_steer_window_size = param_steer_window_size # [rad]\\n\",\n    \"        self.param_accel_window_size = param_accel_window_size # [m/s^2]\\n\",\n    \"        self.stage_cost_weight = stage_cost_weight\\n\",\n    \"        self.terminal_cost_weight = terminal_cost_weight\\n\",\n    \"        self.visualize_optimal_traj = visualize_optimal_traj\\n\",\n    \"        self.visualze_sampled_trajs = visualze_sampled_trajs\\n\",\n    \"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.wheel_base = wheel_base #[m]\\n\",\n    \"        self.vehicle_w = vehicle_width #[m]\\n\",\n    \"        self.vehicle_l = vehicle_length #[m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # obstacle parameters\\n\",\n    \"        self.obstacle_circles = obstacle_circles\\n\",\n    \"        self.collision_safety_margin_rate = collision_safety_margin_rate\\n\",\n    \"\\n\",\n    \"        # dwa variables\\n\",\n    \"        self.u_prev = np.zeros((self.T, self.dim_u))\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\\n\",\n    \"        \\\"\\\"\\\"calculate optimal control input\\\"\\\"\\\"\\n\",\n    \"        # load privious control input sequence\\n\",\n    \"        u = self.u_prev\\n\",\n    \"\\n\",\n    \"        # set initial x value from observation\\n\",\n    \"        x0 = observed_x\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position \\n\",\n    \"        self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # clarify exploration range of control input space\\n\",\n    \"        steer_prev = u[0, 0]\\n\",\n    \"        accel_prev = u[0, 1]\\n\",\n    \"        steer_min = np.clip(steer_prev - self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        steer_max = np.clip(steer_prev + self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel_min = np.clip(accel_prev - self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"        accel_max = np.clip(accel_prev + self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        # sample control input sequence\\n\",\n    \"        dynamic_window_steer = np.linspace(steer_min, steer_max, int((steer_max-steer_min)/self.param_steer_resolution)+1)\\n\",\n    \"        dynamic_window_accel = np.linspace(accel_min, accel_max, int((accel_max-accel_min)/self.param_accel_resolution)+1)\\n\",\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\",\n    \"        sample_num = dynamic_window.shape[0] * dynamic_window.shape[1]\\n\",\n    \"\\n\",\n    \"        # prepare buffer\\n\",\n    \"        ## sampled control input sequence\\n\",\n    \"        v = np.zeros((sample_num, self.T, self.dim_u)) # control input sequence with noise\\n\",\n    \"        ## cost of each sample\\n\",\n    \"        S = np.zeros((sample_num))\\n\",\n    \"        ## counter of samples\\n\",\n    \"        k = 0\\n\",\n    \"\\n\",\n    \"        # loop for 0 ~ K-1 samples\\n\",\n    \"        for i, j in np.ndindex(dynamic_window.shape[0], dynamic_window.shape[1]):\\n\",\n    \"\\n\",\n    \"            # set initial(t=0) state x i.e. observed state of the vehicle\\n\",\n    \"            x = x0\\n\",\n    \"\\n\",\n    \"            # loop for time step t = 1 ~ T\\n\",\n    \"            for t in range(1, self.T+1):\\n\",\n    \"\\n\",\n    \"                # get control input, which is constant during the prediction horizon\\n\",\n    \"                v[k, t-1] = dynamic_window[i, j]\\n\",\n    \"\\n\",\n    \"                # update x\\n\",\n    \"                x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"\\n\",\n    \"                # add stage cost\\n\",\n    \"                S[k] += self._c(x)\\n\",\n    \"\\n\",\n    \"            # add terminal cost\\n\",\n    \"            S[k] += self._phi(x)\\n\",\n    \"\\n\",\n    \"            # update counter of samples\\n\",\n    \"            k += 1\\n\",\n    \"\\n\",\n    \"        # get optimal control input\\n\",\n    \"        k_opt = np.argmin(S)\\n\",\n    \"        u = v[k_opt]\\n\",\n    \"\\n\",\n    \"        # calculate optimal trajectory\\n\",\n    \"        optimal_traj = np.zeros((self.T, self.dim_x))\\n\",\n    \"        if self.visualize_optimal_traj:\\n\",\n    \"            x = x0\\n\",\n    \"            for t in range(self.T):\\n\",\n    \"                x = self._F(x, self._g(u[t-1]))\\n\",\n    \"                optimal_traj[t] = x\\n\",\n    \"\\n\",\n    \"        # calculate sampled trajectories\\n\",\n    \"        sampled_traj_list = np.zeros((k, self.T, self.dim_x))\\n\",\n    \"        sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\\n\",\n    \"        if self.visualze_sampled_trajs:\\n\",\n    \"            for k in sorted_idx:\\n\",\n    \"                x = x0\\n\",\n    \"                for t in range(self.T):\\n\",\n    \"                    x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"                    sampled_traj_list[k, t] = x\\n\",\n    \"\\n\",\n    \"        # update privious control input sequence (shift 1 step to the left)\\n\",\n    \"        self.u_prev[:-1] = u[1:]\\n\",\n    \"        self.u_prev[-1] = u[-1]\\n\",\n    \"\\n\",\n    \"        # return optimal control input and input sequence\\n\",\n    \"        return u[0], u, optimal_traj, sampled_traj_list\\n\",\n    \"\\n\",\n    \"    def _g(self, v: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"clamp input\\\"\\\"\\\"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\\n\",\n    \"        v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\\n\",\n    \"        return v\\n\",\n    \"\\n\",\n    \"    def _c(self, x_t: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate stage cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_t\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate stage cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                     self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"\\n\",\n    \"        # add penalty for collision with obstacles\\n\",\n    \"        stage_cost += self._is_collided(x_t) * 1.0e10\\n\",\n    \"\\n\",\n    \"        return stage_cost\\n\",\n    \"\\n\",\n    \"    def _phi(self, x_T: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate terminal cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_T\\n\",\n    \"        x, y, yaw, v = x_T\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate terminal cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                        self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"\\n\",\n    \"        # add penalty for collision with obstacles\\n\",\n    \"        terminal_cost += self._is_collided(x_T) * 1.0e10\\n\",\n    \"\\n\",\n    \"        return terminal_cost\\n\",\n    \"\\n\",\n    \"    def _is_collided(self,  x_t: np.ndarray) -> bool:\\n\",\n    \"        \\\"\\\"\\\"check if the vehicle is collided with obstacles\\\"\\\"\\\"\\n\",\n    \"        # vehicle shape parameters\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        safety_margin_rate = self.collision_safety_margin_rate\\n\",\n    \"        vw, vl = vw*safety_margin_rate, vl*safety_margin_rate\\n\",\n    \"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, _ = x_t\\n\",\n    \"\\n\",\n    \"        # key points for collision check\\n\",\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\",\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\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\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    \"\\n\",\n    \"        # check if the key points are inside the obstacles\\n\",\n    \"        for obs in self.obstacle_circles: # for each circular obstacles\\n\",\n    \"            obs_x, obs_y, obs_r = obs # [m] x, y, radius\\n\",\n    \"            for p in range(len(rotated_vehicle_shape_x)):\\n\",\n    \"                if (rotated_vehicle_shape_x[p]-obs_x)**2 + (rotated_vehicle_shape_y[p]-obs_y)**2 < obs_r**2:\\n\",\n    \"                    return 1.0 # collided\\n\",\n    \"\\n\",\n    \"        return 0.0 # not collided\\n\",\n    \"\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        \\\"\\\"\\\"rotate shape and return location on the x-y plane.\\\"\\\"\\\"\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\\n\",\n    \"\\n\",\n    \"    def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"calculate next state of the vehicle\\\"\\\"\\\"\\n\",\n    \"        # get previous state variables\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        steer, accel = v_t\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"\\n\",\n    \"        # return updated state\\n\",\n    \"        x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        return x_t_plus_1\\n\",\n    \"\\n\",\n    \"    def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"apply moving average filter for smoothing input sequence\\n\",\n    \"        Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        b = np.ones(window_size)/window_size\\n\",\n    \"        dim = xx.shape[1]\\n\",\n    \"        xx_mean = np.zeros(xx.shape)\\n\",\n    \"\\n\",\n    \"        for d in range(dim):\\n\",\n    \"            xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\\\"same\\\")\\n\",\n    \"            n_conv = math.ceil(window_size/2)\\n\",\n    \"            xx_mean[0,d] *= window_size/n_conv\\n\",\n    \"            for i in range(1, n_conv):\\n\",\n    \"                xx_mean[i,d] *= window_size/(i+n_conv)\\n\",\n    \"                xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \\n\",\n    \"        return xx_mean\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Obstacle Avoidance\\n\",\n    \"- Longitudinal Control : Dynamic Window Approach\\n\",\n    \"- Lateral Control : Dynamic Window Approach\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 150 # [steps]\\n\",\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    \"\\n\",\n    \"# obstacle params\\n\",\n    \"OBSTACLE_CIRCLES = np.array([\\n\",\n    \"    [+ 8.0, +5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\\n\",\n    \"    [+18.0, -5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\\n\",\n    \"])\\n\",\n    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    wheel_base=2.5,\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \"    obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 0.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize a dwa controller for the vehicle\\n\",\n    \"dwa = DWAControllerForPathTracking(\\n\",\n    \"    delta_t = delta_t*2.0, # [s]\\n\",\n    \"    wheel_base = 2.5, # [m]\\n\",\n    \"    max_steer_abs = 0.523, # [rad]\\n\",\n    \"    max_accel_abs = 2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \"    horizon_step_T = 20, # [steps]\\n\",\n    \"    param_steer_resolution = 0.02, # [rad]\\n\",\n    \"    param_accel_resolution = 0.025, # [m/s^2]\\n\",\n    \"    param_steer_window_size = 0.5, # [rad]\\n\",\n    \"    param_accel_window_size = 0.5, # [m/s^2]\\n\",\n    \"    stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    visualze_sampled_trajs = True,\\n\",\n    \"    obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\\n\",\n    \"    collision_safety_margin_rate = 1.2, # safety margin for collision check\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate input force with DWA\\n\",\n    \"        optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = dwa.calc_control_input(\\n\",\n    \"            observed_x = current_state\\n\",\n    \"        )\\n\",\n    \"    except IndexError as e:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\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\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"# save animation\\n\",\n    \"# vehicle.save_animation(\\\"dwa_obstacle_avoidance_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/dwa_pathtracking.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Dynamic Window Approach\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle\\n\",\n    \"\\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.wheel_base = wheel_base#[m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"            optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from dwa\\n\",\n    \"            sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from dwa\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\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\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw the predicted optimal trajectory from dwa\\n\",\n    \"        if optimal_traj.any():\\n\",\n    \"            optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\\n\",\n    \"            optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        # draw the sampled trajectories from dwa\\n\",\n    \"        if sampled_traj_list.any():\\n\",\n    \"            min_alpha_value = 0.25\\n\",\n    \"            max_alpha_value = 0.35\\n\",\n    \"            for idx, sampled_traj in enumerate(sampled_traj_list):\\n\",\n    \"                # draw darker for better samples\\n\",\n    \"                alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\\n\",\n    \"                sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\\n\",\n    \"                sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Controller : Dynamic Window Approach\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class DWAControllerForPathTracking():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            delta_t: float = 0.05,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"            horizon_step_T: int = 30,\\n\",\n    \"            param_steer_resolution: float = 0.01, # [rad]\\n\",\n    \"            param_accel_resolution: float = 0.025, # [m/s^2]\\n\",\n    \"            param_steer_window_size: float = 0.2, # [rad]\\n\",\n    \"            param_accel_window_size: float = 0.2, # [m/s^2]\\n\",\n    \"            stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            visualize_optimal_traj = True,  # if True, optimal trajectory is visualized\\n\",\n    \"            visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize dwa controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # dwa parameters\\n\",\n    \"        self.dim_x = 4 # dimension of system state vector\\n\",\n    \"        self.dim_u = 2 # dimension of control input vector\\n\",\n    \"        self.T = horizon_step_T # prediction horizon\\n\",\n    \"        self.param_steer_resolution = param_steer_resolution # [rad]\\n\",\n    \"        self.param_accel_resolution = param_accel_resolution # [m/s^2]\\n\",\n    \"        self.param_steer_window_size = param_steer_window_size # [rad]\\n\",\n    \"        self.param_accel_window_size = param_accel_window_size # [m/s^2]\\n\",\n    \"        self.stage_cost_weight = stage_cost_weight\\n\",\n    \"        self.terminal_cost_weight = terminal_cost_weight\\n\",\n    \"        self.visualize_optimal_traj = visualize_optimal_traj\\n\",\n    \"        self.visualze_sampled_trajs = visualze_sampled_trajs\\n\",\n    \"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.wheel_base = wheel_base#[m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # dwa variables\\n\",\n    \"        self.u_prev = np.zeros((self.T, self.dim_u))\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\\n\",\n    \"        \\\"\\\"\\\"calculate optimal control input\\\"\\\"\\\"\\n\",\n    \"        # load privious control input sequence\\n\",\n    \"        u = self.u_prev\\n\",\n    \"\\n\",\n    \"        # set initial x value from observation\\n\",\n    \"        x0 = observed_x\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position \\n\",\n    \"        self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # clarify exploration range of control input space\\n\",\n    \"        steer_prev = u[0, 0]\\n\",\n    \"        accel_prev = u[0, 1]\\n\",\n    \"        steer_min = np.clip(steer_prev - self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        steer_max = np.clip(steer_prev + self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel_min = np.clip(accel_prev - self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"        accel_max = np.clip(accel_prev + self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        # sample control input sequence\\n\",\n    \"        dynamic_window_steer = np.linspace(steer_min, steer_max, int((steer_max-steer_min)/self.param_steer_resolution)+1)\\n\",\n    \"        dynamic_window_accel = np.linspace(accel_min, accel_max, int((accel_max-accel_min)/self.param_accel_resolution)+1)\\n\",\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\",\n    \"        sample_num = dynamic_window.shape[0] * dynamic_window.shape[1]\\n\",\n    \"\\n\",\n    \"        # prepare buffer\\n\",\n    \"        ## sampled control input sequence\\n\",\n    \"        v = np.zeros((sample_num, self.T, self.dim_u)) # control input sequence with noise\\n\",\n    \"        ## cost of each sample\\n\",\n    \"        S = np.zeros((sample_num))\\n\",\n    \"        ## counter of samples\\n\",\n    \"        k = 0\\n\",\n    \"\\n\",\n    \"        # loop for 0 ~ K-1 samples\\n\",\n    \"        for i, j in np.ndindex(dynamic_window.shape[0], dynamic_window.shape[1]):\\n\",\n    \"\\n\",\n    \"            # set initial(t=0) state x i.e. observed state of the vehicle\\n\",\n    \"            x = x0\\n\",\n    \"\\n\",\n    \"            # loop for time step t = 1 ~ T\\n\",\n    \"            for t in range(1, self.T+1):\\n\",\n    \"\\n\",\n    \"                # get control input, which is constant during the prediction horizon\\n\",\n    \"                v[k, t-1] = dynamic_window[i, j]\\n\",\n    \"\\n\",\n    \"                # update x\\n\",\n    \"                x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"\\n\",\n    \"                # add stage cost\\n\",\n    \"                S[k] += self._c(x)\\n\",\n    \"\\n\",\n    \"            # add terminal cost\\n\",\n    \"            S[k] += self._phi(x)\\n\",\n    \"\\n\",\n    \"            # update counter of samples\\n\",\n    \"            k += 1\\n\",\n    \"\\n\",\n    \"        # get optimal control input\\n\",\n    \"        k_opt = np.argmin(S)\\n\",\n    \"        u = v[k_opt]\\n\",\n    \"\\n\",\n    \"        # calculate optimal trajectory\\n\",\n    \"        optimal_traj = np.zeros((self.T, self.dim_x))\\n\",\n    \"        if self.visualize_optimal_traj:\\n\",\n    \"            x = x0\\n\",\n    \"            for t in range(self.T):\\n\",\n    \"                x = self._F(x, self._g(u[t-1]))\\n\",\n    \"                optimal_traj[t] = x\\n\",\n    \"\\n\",\n    \"        # calculate sampled trajectories\\n\",\n    \"        sampled_traj_list = np.zeros((k, self.T, self.dim_x))\\n\",\n    \"        sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\\n\",\n    \"        if self.visualze_sampled_trajs:\\n\",\n    \"            for k in sorted_idx:\\n\",\n    \"                x = x0\\n\",\n    \"                for t in range(self.T):\\n\",\n    \"                    x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"                    sampled_traj_list[k, t] = x\\n\",\n    \"\\n\",\n    \"        # update privious control input sequence (shift 1 step to the left)\\n\",\n    \"        self.u_prev[:-1] = u[1:]\\n\",\n    \"        self.u_prev[-1] = u[-1]\\n\",\n    \"\\n\",\n    \"        # return optimal control input and input sequence\\n\",\n    \"        return u[0], u, optimal_traj, sampled_traj_list\\n\",\n    \"\\n\",\n    \"    def _g(self, v: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"clamp input\\\"\\\"\\\"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\\n\",\n    \"        v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\\n\",\n    \"        return v\\n\",\n    \"\\n\",\n    \"    def _c(self, x_t: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate stage cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_t\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate stage cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                     self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"        return stage_cost\\n\",\n    \"\\n\",\n    \"    def _phi(self, x_T: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate terminal cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_T\\n\",\n    \"        x, y, yaw, v = x_T\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate terminal cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                        self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"        return terminal_cost\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\\n\",\n    \"\\n\",\n    \"    def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"calculate next state of the vehicle\\\"\\\"\\\"\\n\",\n    \"        # get previous state variables\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        steer, accel = v_t\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"\\n\",\n    \"        # return updated state\\n\",\n    \"        x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        return x_t_plus_1\\n\",\n    \"\\n\",\n    \"    def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"apply moving average filter for smoothing input sequence\\n\",\n    \"        Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        b = np.ones(window_size)/window_size\\n\",\n    \"        dim = xx.shape[1]\\n\",\n    \"        xx_mean = np.zeros(xx.shape)\\n\",\n    \"\\n\",\n    \"        for d in range(dim):\\n\",\n    \"            xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\\\"same\\\")\\n\",\n    \"            n_conv = math.ceil(window_size/2)\\n\",\n    \"            xx_mean[0,d] *= window_size/n_conv\\n\",\n    \"            for i in range(1, n_conv):\\n\",\n    \"                xx_mean[i,d] *= window_size/(i+n_conv)\\n\",\n    \"                xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \\n\",\n    \"        return xx_mean\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : Dynamic Window Approach\\n\",\n    \"- Lateral Control : Dynamic Window Approach\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    wheel_base=2.5,\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize a dwa controller for the vehicle\\n\",\n    \"dwa = DWAControllerForPathTracking(\\n\",\n    \"    delta_t = delta_t*2.0, # [s]\\n\",\n    \"    wheel_base = 2.5, # [m]\\n\",\n    \"    max_steer_abs = 0.523, # [rad]\\n\",\n    \"    max_accel_abs = 2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \"    horizon_step_T = 20, # [steps]\\n\",\n    \"    param_steer_resolution = 0.01, # [rad]\\n\",\n    \"    param_accel_resolution = 0.025, # [m/s^2]\\n\",\n    \"    param_steer_window_size = 0.2, # [rad]\\n\",\n    \"    param_accel_window_size = 0.2, # [m/s^2]\\n\",\n    \"    stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    visualze_sampled_trajs = True\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate input force with DWA\\n\",\n    \"        optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = dwa.calc_control_input(\\n\",\n    \"            observed_x = current_state\\n\",\n    \"        )\\n\",\n    \"    except IndexError as e:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\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\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"# save animation\\n\",\n    \"# vehicle.save_animation(\\\"dwa_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/dynamic_bicycle_model.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Dynamic Bicycle Model\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Definition of Coordinate Systems\\n\",\n    \"\\n\",\n    \"<img src=\\\"../media/def_of_frames.svg\\\" width=\\\"500px\\\" alt=\\\"definition_of_frames\\\">\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## State Equation\\n\",\n    \"\\n\",\n    \"<img src=\\\"../media/DBM.svg\\\" width=\\\"500px\\\" alt=\\\"DBM\\\">\\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"&\\\\frac{\\\\mathrm{d}}{\\\\mathrm{d}t}\\n\",\n    \"\\\\begin{bmatrix}\\n\",\n    \"p^G_x \\\\\\\\\\n\",\n    \"p^G_y \\\\\\\\\\n\",\n    \"\\\\phi \\\\\\\\\\n\",\n    \"v^B_x \\\\\\\\\\n\",\n    \"v^B_y \\\\\\\\\\n\",\n    \"\\\\omega \\\\\\\\\\n\",\n    \"\\\\end{bmatrix}\\n\",\n    \"=\\n\",\n    \"\\\\begin{bmatrix}\\n\",\n    \"v^B_x \\\\cos\\\\phi - v^B_y \\\\sin\\\\phi \\\\\\\\\\n\",\n    \"v^B_x \\\\sin\\\\phi + v^B_y \\\\cos\\\\phi \\\\\\\\\\n\",\n    \"\\\\omega \\\\\\\\\\n\",\n    \"{a}\\\\cos\\\\beta - (F_{f}^{\\\\rm{lat}}\\\\sin {{\\\\delta}})/m + v^B_y \\\\omega \\\\\\\\\\n\",\n    \"{a}\\\\sin\\\\beta + F_{r}^{\\\\rm{lat}}/m + F_{f}^{\\\\rm{lat}} \\\\cos{\\\\delta}/m - v^B_x \\\\omega \\\\\\\\\\n\",\n    \"(F_{f}^{\\\\rm{lat}}l_f\\\\cos{\\\\delta} - F_{r}^{\\\\rm{lat}}l_r)/I_z\\\\\\n\",\n    \"\\\\end{bmatrix}, \\\\\\\\ \\\\\\\\\\n\",\n    \"& F_{f}^{\\\\rm{lat}} = - C_f \\\\left( \\\\frac{v^B_y + l_f \\\\omega}{v^B_x} - {\\\\delta} \\\\right), \\\\\\\\ \\\\\\\\\\n\",\n    \"& F_{r}^{\\\\rm{lat}} = - C_r \\\\left( \\\\frac{v^B_y - l_r \\\\omega}{v^B_x} \\\\right), \\\\\\\\ \\\\\\\\\\n\",\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\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.1, # [m]\\n\",\n    \"            l_r: float = 1.4, # [m]\\n\",\n    \"            mass: float = 1000.0, # [kg]\\n\",\n    \"            I_z: float = 1300.0, # [kg*m^2]\\n\",\n    \"            C_f: float = 5000.0 * 2.0, # [N/rad]\\n\",\n    \"            C_r: float = 6000.0 * 2.0, # [N/rad]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            vx: x-axis velocity [m/s]\\n\",\n    \"            vy: y-axis velocity [m/s]\\n\",\n    \"            omega: angular velocity [rad/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Dynamic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.mass = mass # [kg]\\n\",\n    \"        self.I_z = I_z # [kg*m^2]\\n\",\n    \"        self.C_f = C_f # [N/rad]\\n\",\n    \"        self.C_r = C_r # [N/rad]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\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\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, vx, vy, omega = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l_f = self.l_f\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        m = self.mass\\n\",\n    \"        C_f = self.C_f\\n\",\n    \"        C_r = self.C_r\\n\",\n    \"        I_z = self.I_z\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # calculate tire forces\\n\",\n    \"        F_fy = - C_f * ((vy + l_f * omega) / vx - steer)\\n\",\n    \"        F_ry = - C_r * ((vy - l_r * omega) / vx)\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = vy / vx\\n\",\n    \"        new_x = x + (vx * np.cos(yaw) - vy * np.sin(yaw)) * dt\\n\",\n    \"        new_y = y + (vx * np.sin(yaw) + vy * np.cos(yaw)) * dt\\n\",\n    \"        new_yaw = yaw + omega * dt\\n\",\n    \"        new_vx = vx + (accel * np.cos(beta) - (F_fy * np.sin(steer) / m) + vy * omega) * dt\\n\",\n    \"        new_vy = vy + (accel * np.sin(beta) + F_ry / m + (F_fy * np.cos(steer) / m) - vx * omega) * dt\\n\",\n    \"        new_omega = omega + ((F_fy * l_f * np.cos(steer) - F_ry * l_r) / I_z) * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_vx, new_vy, new_omega])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, vx, vy, omega = self.state\\n\",\n    \"        v = np.sqrt(vx**2 + vy**2) # vehicle velocity\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Run Simulation\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"### Zig Zag Run\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 200 # [step]\\n\",\n    \"delta_t = 0.05 # [s] # Note : shorter delta_t is recommended for dynamic bicycle model\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-3.0, 50.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array(vehicle.get_state()[:2])\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"    steer_input = 0.6 * np.sin(i/5.0)  # steering command [rad]\\n\",\n    \"    accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"dbm.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Steady Input\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 400 # [step]\\n\",\n    \"delta_t = 0.05 # [s] # Note : shorter delta_t is recommended for dynamic bicycle model\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-25.0, 25.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array(vehicle.get_state()[:2])\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"    steer_input = 0.3  # steering command [rad]\\n\",\n    \"    accel_input = 2.0 # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"dbm.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \".venv\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.12.0\"\n  },\n  \"orig_nbformat\": 4\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 2\n}\n"
  },
  {
    "path": "notebooks/fuzzy.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Fuzzy Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle \\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Longitudinal Controller : Fuzzy Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class FuzzyLongitudinalController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            slow_thresh_rate: float = +0.6, # [%]\\n\",\n    \"            fast_thresh_rate: float = +1.4, # [%]\\n\",\n    \"            target_velocity: float = 3.0, # [m/s]\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize fuzzy controller for keeping constant velocity\\\"\\\"\\\"\\n\",\n    \"        # fuzzy control parameters\\n\",\n    \"        self.target_vel = target_velocity\\n\",\n    \"        self.slow_thresh_vel = slow_thresh_rate * target_velocity\\n\",\n    \"        self.fast_thresh_vel = fast_thresh_rate * target_velocity\\n\",\n    \"        self.acc_when_vel_is_slow = +2.0 # [m/s^2]\\n\",\n    \"        self.acc_when_vel_is_mid = 0.0 # [m/s^2]\\n\",\n    \"        self.acc_when_vel_is_fast = -2.0 # [m/s^2]\\n\",\n    \"\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"        # calculate tracking error, its integral and derivative\\n\",\n    \"        v = observed_vel\\n\",\n    \"\\n\",\n    \"        # calculate control input with fuzzy control\\n\",\n    \"        acc_cmd = self.defuzzify(v)\\n\",\n    \"\\n\",\n    \"        return acc_cmd\\n\",\n    \"\\n\",\n    \"    def defuzzify(self, vel: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"defuzzigy control input\\\"\\\"\\\"\\n\",\n    \"        w_slow = 1.0\\n\",\n    \"        w_mid  = 1.0\\n\",\n    \"        w_fast = 1.0\\n\",\n    \"        acc_cmd = self.acc_when_vel_is_slow * w_slow * self.speed_slow(vel) \\\\\\n\",\n    \"                + self.acc_when_vel_is_mid  * w_mid  * self.speed_mid(vel) \\\\\\n\",\n    \"                + self.acc_when_vel_is_fast * w_fast * self.speed_fast(vel)\\n\",\n    \"\\n\",\n    \"        return acc_cmd\\n\",\n    \"\\n\",\n    \"    def speed_slow(self, vel: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for slow speed\\\"\\\"\\\"\\n\",\n    \"        if vel < self.slow_thresh_vel:\\n\",\n    \"            return 1.0\\n\",\n    \"        elif vel < self.fast_thresh_vel:\\n\",\n    \"            return (self.fast_thresh_vel - vel) / (self.fast_thresh_vel - self.slow_thresh_vel)\\n\",\n    \"        else:\\n\",\n    \"            return 0.0\\n\",\n    \"\\n\",\n    \"    def speed_mid(self, vel: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for middle speed\\\"\\\"\\\"\\n\",\n    \"        if vel < self.slow_thresh_vel:\\n\",\n    \"            return 0.0\\n\",\n    \"        elif vel < self.target_vel:\\n\",\n    \"            return (vel - self.slow_thresh_vel) / (self.target_vel - self.slow_thresh_vel)\\n\",\n    \"        elif vel < self.fast_thresh_vel:\\n\",\n    \"            return (self.fast_thresh_vel - vel) / (self.fast_thresh_vel - self.target_vel)\\n\",\n    \"        else:\\n\",\n    \"            return 0.0\\n\",\n    \"\\n\",\n    \"    def speed_fast(self, vel: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for fast speed\\\"\\\"\\\"\\n\",\n    \"        if vel < self.slow_thresh_vel:\\n\",\n    \"            return 0.0\\n\",\n    \"        elif vel < self.fast_thresh_vel:\\n\",\n    \"            return (vel - self.slow_thresh_vel) / (self.fast_thresh_vel - self.slow_thresh_vel)\\n\",\n    \"        else:\\n\",\n    \"            return 1.0\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Lateral Controller : Fuzzy Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class FuzzyLateralController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize fuzzy controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # fuzzy control parameters\\n\",\n    \"        self.y_e_n_thresh = -1.0 # [m]\\n\",\n    \"        self.y_e_z_thresh = 0.0 # [m]\\n\",\n    \"        self.y_e_p_thresh = +1.0 # [m]\\n\",\n    \"        self.steer_when_y_e_is_n = +0.523 # [rad]\\n\",\n    \"        self.steer_when_y_e_is_z = 0.0 # [rad]\\n\",\n    \"        self.steer_when_y_e_is_p = -0.523 # [rad]\\n\",\n    \"\\n\",\n    \"        self.theta_e_n_thresh = -0.523/7.0 # [rad]\\n\",\n    \"        self.theta_e_z_thresh = 0.0 # [rad]\\n\",\n    \"        self.theta_e_p_thresh = +0.523/7.0 # [rad]\\n\",\n    \"        self.steer_when_theta_e_is_n = +0.523 # [rad]\\n\",\n    \"        self.steer_when_theta_e_is_z = 0.0 # [rad]\\n\",\n    \"        self.steer_when_theta_e_is_p = -0.523 # [rad]\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # set vehicle state variables from observation\\n\",\n    \"        x = observed_x[0]\\n\",\n    \"        y = observed_x[1]\\n\",\n    \"        yaw = observed_x[2]\\n\",\n    \"        v = observed_x[3]\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # which side of the reference path is the car on, the right or the left?\\n\",\n    \"        ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\\n\",\n    \"        x1, y1 = ref_x, ref_y\\n\",\n    \"        x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0 * np.sin(ref_yaw)\\n\",\n    \"        vx, vy = x2 - x1, y2 - y1\\n\",\n    \"        wx, wy =  x - x1,  y - y1\\n\",\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    \"\\n\",\n    \"        # get tracking error\\n\",\n    \"        y_e = np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # lateral error \\n\",\n    \"        theta_e = yaw - ref_yaw # heading error\\n\",\n    \"        theta_e = np.arctan2(np.sin(theta_e), np.cos(theta_e)) # normalize heading error to [-pi, pi]\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        steer_cmd = self.defuzzify(y_e, theta_e)\\n\",\n    \"        return steer_cmd\\n\",\n    \"\\n\",\n    \"    def defuzzify(self, y_e: float, theta_e: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"defuzzigy control input\\\"\\\"\\\"\\n\",\n    \"        w_p = 0.6\\n\",\n    \"        w_z = 0.6\\n\",\n    \"        w_n = 0.6\\n\",\n    \"\\n\",\n    \"        steer_cmd_y_e = self.steer_when_y_e_is_p * w_p * self.y_e_p(y_e) \\\\\\n\",\n    \"                      + self.steer_when_y_e_is_z * w_z * self.y_e_z(y_e) \\\\\\n\",\n    \"                      + self.steer_when_y_e_is_n * w_n * self.y_e_n(y_e)\\n\",\n    \"\\n\",\n    \"        steer_cmd_theta_e = self.steer_when_theta_e_is_p * (1 - w_p) * self.theta_e_p(theta_e) \\\\\\n\",\n    \"                          + self.steer_when_theta_e_is_z * (1 - w_z) * self.theta_e_z(theta_e) \\\\\\n\",\n    \"                          + self.steer_when_theta_e_is_n * (1 - w_n) * self.theta_e_n(theta_e)\\n\",\n    \"\\n\",\n    \"        steer_cmd = steer_cmd_y_e + steer_cmd_theta_e\\n\",\n    \"        return steer_cmd\\n\",\n    \"\\n\",\n    \"    def y_e_n(self, y_e: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for y_e negative\\\"\\\"\\\"\\n\",\n    \"        if y_e < self.y_e_n_thresh:\\n\",\n    \"            return 1.0\\n\",\n    \"        elif y_e < self.y_e_z_thresh:\\n\",\n    \"            return (self.y_e_z_thresh - y_e) / (self.y_e_z_thresh - self.y_e_n_thresh)\\n\",\n    \"        else:\\n\",\n    \"            return 0.0\\n\",\n    \"\\n\",\n    \"    def y_e_z(self, y_e: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for y_e zero\\\"\\\"\\\"\\n\",\n    \"        if y_e < self.y_e_n_thresh:\\n\",\n    \"            return 0.0\\n\",\n    \"        elif y_e < self.y_e_z_thresh:\\n\",\n    \"            return (y_e - self.y_e_n_thresh) / (self.y_e_z_thresh - self.y_e_n_thresh)\\n\",\n    \"        elif y_e < self.y_e_p_thresh:\\n\",\n    \"            return (self.y_e_p_thresh - y_e) / (self.y_e_p_thresh - self.y_e_z_thresh)\\n\",\n    \"        else:\\n\",\n    \"            return 0.0\\n\",\n    \"\\n\",\n    \"    def y_e_p(self, y_e: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for y_e positive\\\"\\\"\\\"\\n\",\n    \"        if y_e < self.y_e_z_thresh:\\n\",\n    \"            return 0.0\\n\",\n    \"        elif y_e < self.y_e_p_thresh:\\n\",\n    \"            return (y_e - self.y_e_z_thresh) / (self.y_e_p_thresh - self.y_e_z_thresh)\\n\",\n    \"        else:\\n\",\n    \"            return 1.0\\n\",\n    \"\\n\",\n    \"    def theta_e_n(self, theta_e: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for theta_e negative\\\"\\\"\\\"\\n\",\n    \"        if theta_e < self.theta_e_n_thresh:\\n\",\n    \"            return 1.0\\n\",\n    \"        elif theta_e < self.theta_e_z_thresh:\\n\",\n    \"            return (self.theta_e_z_thresh - theta_e) / (self.theta_e_z_thresh - self.theta_e_n_thresh)\\n\",\n    \"        else:\\n\",\n    \"            return 0.0\\n\",\n    \"\\n\",\n    \"    def theta_e_z(self, theta_e: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for theta_e zero\\\"\\\"\\\"\\n\",\n    \"        if theta_e < self.theta_e_n_thresh:\\n\",\n    \"            return 0.0\\n\",\n    \"        elif theta_e < self.theta_e_z_thresh:\\n\",\n    \"            return (theta_e - self.theta_e_n_thresh) / (self.theta_e_z_thresh - self.theta_e_n_thresh)\\n\",\n    \"        elif theta_e < self.theta_e_p_thresh:\\n\",\n    \"            return (self.theta_e_p_thresh - theta_e) / (self.theta_e_p_thresh - self.theta_e_z_thresh)\\n\",\n    \"        else:\\n\",\n    \"            return 0.0\\n\",\n    \"\\n\",\n    \"    def theta_e_p(self, theta_e: float):\\n\",\n    \"        \\\"\\\"\\\"fuzzy membership function for theta_e positive\\\"\\\"\\\"\\n\",\n    \"        if theta_e < self.theta_e_z_thresh:\\n\",\n    \"            return 0.0\\n\",\n    \"        elif theta_e < self.theta_e_p_thresh:\\n\",\n    \"            return (theta_e - self.theta_e_z_thresh) / (self.theta_e_p_thresh - self.theta_e_z_thresh)\\n\",\n    \"        else:\\n\",\n    \"            return 1.0\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : Fuzzy Controller\\n\",\n    \"- Lateral Control : Fuzzy Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize fuzzy controller for acceleration control\\n\",\n    \"fuzzy_lon_controller = FuzzyLongitudinalController(\\n\",\n    \"    target_velocity = +5.0 # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# initialize fuzzy controller for steering control\\n\",\n    \"fuzzy_lat_controller = FuzzyLateralController(\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate control inputs\\n\",\n    \"        current_velocity = current_state[3]\\n\",\n    \"        accel_input = fuzzy_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\\n\",\n    \"        steer_input = fuzzy_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\\n\",\n    \"\\n\",\n    \"    except IndexError as ex:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"fuzzy_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/kinematic_bicycle_model.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Definition of Coordinate Systems\\n\",\n    \"\\n\",\n    \"<img src=\\\"../media/def_of_frames.svg\\\" width=\\\"500px\\\" alt=\\\"definition_of_frames\\\">\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## State Equation\\n\",\n    \"\\n\",\n    \"<img src=\\\"../media/KBM.svg\\\" width=\\\"500px\\\" alt=\\\"KBM\\\">\\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"& \\\\frac{\\\\mathrm{d}}{\\\\mathrm{d}t}\\n\",\n    \"\\\\begin{bmatrix}\\n\",\n    \"p^G_x \\\\\\\\\\n\",\n    \"p^G_y \\\\\\\\\\n\",\n    \"\\\\phi \\\\\\\\\\n\",\n    \"V\\n\",\n    \"\\\\end{bmatrix}\\n\",\n    \"=\\n\",\n    \"\\\\begin{bmatrix}\\n\",\n    \"V \\\\cos(\\\\phi + \\\\beta) \\\\\\\\\\n\",\n    \"V \\\\sin(\\\\phi + \\\\beta) \\\\\\\\\\n\",\n    \"(V/l_r)  \\\\sin\\\\beta \\\\\\\\\\n\",\n    \"{a}\\n\",\n    \"\\\\end{bmatrix},\\\\\\\\ \\\\\\\\\\n\",\n    \"& \\\\beta = \\\\tan^{-1} \\\\left( \\\\frac{l_r}{l_f + l_r} \\\\tan({\\\\delta}) \\\\right).\\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Run Simulation\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"### Zig Zag Run\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 100 # [step]\\n\",\n    \"delta_t = 0.1 # [s]\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-3.0, 50.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array(vehicle.get_state()[:2])\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"    steer_input = 0.6 * np.sin(i/5.0)  # steering command [rad]\\n\",\n    \"    accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"kbm.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Steady Input\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 200 # [step]\\n\",\n    \"delta_t = 0.1 # [s]\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-25.0, 25.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array(vehicle.get_state()[:2])\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"    steer_input = 0.3  # steering command [rad]\\n\",\n    \"    accel_input = 2.0 # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"kbm.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \".venv\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  },\n  \"orig_nbformat\": 4\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 2\n}\n"
  },
  {
    "path": "notebooks/lqr.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Linear Quadratic Regulator (LQR)\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle \\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Longitudinal Controller : PID Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class PIDLongitudinalController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            p_gain: float = +1.2, # P Gain\\n\",\n    \"            i_gain: float = +0.4, # I Gain\\n\",\n    \"            d_gain: float = +0.1, # D Gain\\n\",\n    \"            target_velocity: float = 3.0, # [m/s]\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize pid controller for keeping constant velocity\\\"\\\"\\\"\\n\",\n    \"        # pid control parameters\\n\",\n    \"        self.K_p = p_gain\\n\",\n    \"        self.K_i = i_gain\\n\",\n    \"        self.K_d = d_gain\\n\",\n    \"        self.target_vel = target_velocity\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # calculate tracking error, its integral and derivative\\n\",\n    \"        r = self.target_vel\\n\",\n    \"        y = observed_vel\\n\",\n    \"        e = r - y # tracking error to the traget velocity\\n\",\n    \"        ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\\n\",\n    \"        de = (e - self.pre_e) / delta_t # derivative of the tracking error\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\\n\",\n    \"\\n\",\n    \"        # update previous tracking error\\n\",\n    \"        self.pre_e = e\\n\",\n    \"\\n\",\n    \"        return acc_cmd\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Lateral Controller : Linear Quadratic Regulator (LQR)\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class LQRLateralController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            wheel_base: float = 2.5, # [m] wheel_base\\n\",\n    \"            Q: np.ndarray = np.diag([1.0, 1.0]), # weight matrix for state variables\\n\",\n    \"            R: np.ndarray = np.diag([1.0]), # weight matrix for control inputs\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize lqr controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l = wheel_base # [m] wheel base\\n\",\n    \"\\n\",\n    \"        # weight matrices for LQR\\n\",\n    \"        self.Q = Q # weight matrix for state variables\\n\",\n    \"        self.R = R # weight matrix for control inputs\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # set vehicle state variables from observation\\n\",\n    \"        x = observed_x[0]\\n\",\n    \"        y = observed_x[1]\\n\",\n    \"        yaw = observed_x[2]\\n\",\n    \"        v = observed_x[3]\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # which side of the reference path is the car on, the right or the left?\\n\",\n    \"        ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\\n\",\n    \"        x1, y1 = ref_x, ref_y\\n\",\n    \"        x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0 * np.sin(ref_yaw)\\n\",\n    \"        vx, vy = x2 - x1, y2 - y1\\n\",\n    \"        wx, wy =  x - x1,  y - y1\\n\",\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    \"\\n\",\n    \"        # get tracking error\\n\",\n    \"        y_e = np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # lateral error \\n\",\n    \"        theta_e = yaw - ref_yaw # heading error\\n\",\n    \"        theta_e = np.arctan2(np.sin(theta_e), np.cos(theta_e)) # normalize heading error to [-pi, pi]\\n\",\n    \"\\n\",\n    \"        # define A, B matrices and solve algebraic riccati equation to get feedback gain matrix f for LQR\\n\",\n    \"        delta_ref = 0.0 # [rad] reference steering angle (assuming the reference path is straight line)\\n\",\n    \"        A = np.array([\\n\",\n    \"            [0, v],\\n\",\n    \"            [0, 0],\\n\",\n    \"        ])\\n\",\n    \"        B = np.array([\\n\",\n    \"            [0],\\n\",\n    \"            [v / (self.l * (np.cos(delta_ref))**2)],\\n\",\n    \"        ])\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        P = self.solve_are(A, B, self.Q, self.R)\\n\",\n    \"        f = np.linalg.inv(self.R) @ B.T @ P\\n\",\n    \"        steer_cmd = -f @ np.array([y_e, theta_e])\\n\",\n    \"\\n\",\n    \"        return steer_cmd[0].real # TODO : why does steer_cmd have imaginary part?\\n\",\n    \"\\n\",\n    \"    def solve_are(self, A, B, Q, R):\\n\",\n    \"        \\\"\\\"\\\"solve algebraic riccati equation with the Arimoto-Potter algorithm\\n\",\n    \"        Ref: https://qiita.com/trgkpc/items/8210927d5b035912a153\\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # define hamiltonian matrix\\n\",\n    \"        H = np.block([[A, -B @ np.linalg.inv(R) @ B.T],\\n\",\n    \"                    [-Q , -A.T]])\\n\",\n    \"\\n\",\n    \"        # solve eigenvalue problem\\n\",\n    \"        eigenvalue, w = np.linalg.eig(H)\\n\",\n    \"\\n\",\n    \"        # define Y and Z, which are used to calculate P\\n\",\n    \"        Y_, Z_ = [], []\\n\",\n    \"        n = len(w[0])//2\\n\",\n    \"\\n\",\n    \"        # sort eigenvalues\\n\",\n    \"        index_array = sorted([i for i in range(2*n)],\\n\",\n    \"            key = lambda x:eigenvalue[x].real)\\n\",\n    \"\\n\",\n    \"        # choose n eigenvalues which have smaller real part\\n\",\n    \"        for i in index_array[:n]:\\n\",\n    \"            Y_.append(w.T[i][:n])\\n\",\n    \"            Z_.append(w.T[i][n:])\\n\",\n    \"        Y = np.array(Y_).T\\n\",\n    \"        Z = np.array(Z_).T\\n\",\n    \"\\n\",\n    \"        # calculate P\\n\",\n    \"        if np.linalg.det(Y) != 0:\\n\",\n    \"            return Z @ np.linalg.inv(Y)\\n\",\n    \"        else:\\n\",\n    \"            print(\\\"Warning: Y is not regular matrix. Result may be wrong!\\\") # TODO : need to consider mathmatical meaning of this case.\\n\",\n    \"            return Z @ np.linalg.pinv(Y)\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : PID Controller\\n\",\n    \"- Lateral Control : LQR Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize pid controller for acceleration control\\n\",\n    \"pid_lon_controller = PIDLongitudinalController(\\n\",\n    \"    p_gain = 1.2,\\n\",\n    \"    i_gain = 0.4,\\n\",\n    \"    d_gain = 0.1,\\n\",\n    \"    target_velocity = +5.0 # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# initialize lqr controller for steering control\\n\",\n    \"lqr_lat_controller = LQRLateralController(\\n\",\n    \"    Q = np.diag([10.0, 30.0]), # weight matrix for state variables\\n\",\n    \"    R = np.diag([6.0]), # weight matrix for control inputs\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate control inputs\\n\",\n    \"        current_velocity = current_state[3]\\n\",\n    \"        accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\\n\",\n    \"        steer_input = lqr_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\\n\",\n    \"\\n\",\n    \"    except IndexError as ex:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\n    \"    print(steer_input)\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\n    \"    print(\\\"steer_input\\\", steer_input)\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"lqr_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/mppi_obstacle_avoidance.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# MPPI (Model Predictive Path-Integral) Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle\\n\",\n    \"\\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            vehicle_width = 3.0, # [m]\\n\",\n    \"            vehicle_length = 4.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\\n\",\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\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.wheel_base = wheel_base#[m]\\n\",\n    \"        self.vehicle_w = vehicle_width\\n\",\n    \"        self.vehicle_l = vehicle_length\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # obstacle parameters\\n\",\n    \"        self.obstacle_circles = obstacle_circles\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"        self.minimap_view_x_lim_min, self.minimap_view_x_lim_max = -40.0, 40.0\\n\",\n    \"        self.minimap_view_y_lim_min, self.minimap_view_y_lim_max = -10.0, 40.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            self.minimap_ax.set_xlim(self.minimap_view_x_lim_min, self.minimap_view_x_lim_max)\\n\",\n    \"            self.minimap_ax.set_ylim(self.minimap_view_y_lim_min, self.minimap_view_y_lim_max)\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"            optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\\n\",\n    \"            sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\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\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw the predicted optimal trajectory from mppi\\n\",\n    \"        if optimal_traj.any():\\n\",\n    \"            optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\\n\",\n    \"            optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        # draw the sampled trajectories from mppi\\n\",\n    \"        if sampled_traj_list.any():\\n\",\n    \"            min_alpha_value = 0.25\\n\",\n    \"            max_alpha_value = 0.35\\n\",\n    \"            for idx, sampled_traj in enumerate(sampled_traj_list):\\n\",\n    \"                # draw darker for better samples\\n\",\n    \"                alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\\n\",\n    \"                sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\\n\",\n    \"                sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        # draw the circular obstacles in the main view\\n\",\n    \"        for obs in self.obstacle_circles:\\n\",\n    \"            obs_x, obs_y, obs_r = obs\\n\",\n    \"            obs_circle = patches.Circle([obs_x-x, obs_y-y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\\n\",\n    \"            frame += [self.main_ax.add_artist(obs_circle)]\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        # draw the circular obstacles in the mini map view\\n\",\n    \"        for obs in self.obstacle_circles:\\n\",\n    \"            obs_x, obs_y, obs_r = obs\\n\",\n    \"            obs_circle = patches.Circle([obs_x, obs_y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\\n\",\n    \"            frame += [self.minimap_ax.add_artist(obs_circle)]\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Controller : MPPI Controller\\n\",\n    \"\\n\",\n    \"### Note\\n\",\n    \"The following MPPI implementation follows Algorithms 1 and 2 of the reference paper. \\n\",\n    \"\\n\",\n    \"### Reference\\n\",\n    \"1. G. Williams et al. \\\"Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving\\\" \\n\",\n    \"    - URL : https://ieeexplore.ieee.org/document/8558663\\n\",\n    \"    - PDF : https://arxiv.org/pdf/1707.02342.pdf\\n\",\n    \"\\n\",\n    \"### Brief overview of MPPI algorithm\\n\",\n    \"Here is a general process flow to calculate optimal input with mppi algorithm.\\n\",\n    \"\\n\",\n    \"**[Step 1]** ramdomly sample input sequence\\n\",\n    \"\\n\",\n    \"Mean input sequence $U$ and ramdomly sampled input sequence $V$ are defined as follows.  \\n\",\n    \"Usually, optimal input sequence on the previous step is used as $U$. \\n\",\n    \"$$\\n\",\n    \"    \\\\begin{align}\\n\",\n    \"        & (\\\\mathbf{u}_0, \\\\mathbf{u}_1, ... \\\\mathbf{u}_{T-1}) = U \\\\in \\\\mathbb{R}^{m \\\\times T}, \\\\nonumber \\\\\\\\\\n\",\n    \"        & (\\\\mathbf{v}_0, \\\\mathbf{v}_1, ... \\\\mathbf{v}_{T-1}) = V \\\\in \\\\mathbb{R}^{m \\\\times T}, \\\\nonumber \\\\\\\\\\n\",\n    \"        & \\\\mathbf{v}_t = \\\\mathbf{u}_t + \\\\epsilon_t, \\\\nonumber \\\\\\\\\\n\",\n    \"        & \\\\epsilon_t \\\\sim \\\\mathcal{N}(0, \\\\Sigma).\\\\nonumber \\n\",\n    \"    \\\\end{align}\\n\",\n    \"$$\\n\",\n    \"\\n\",\n    \"\\n\",\n    \"**[Step 2]** predict future states and evaluate cost for each sample\\n\",\n    \"\\n\",\n    \"We assume a discrete time, continuous state-action dynamical system as a control target.  \\n\",\n    \"$\\\\mathbf{x}$ is a system state, and $\\\\mathbf{v}$ is a sampled control input.\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"\\\\mathbf{x}_t  &\\\\in \\\\mathbb{R}^{n}, \\\\nonumber \\\\\\\\\\n\",\n    \"\\\\mathbf{x}_{t+1} &= \\\\mathbf{F}(\\\\mathbf{x}_t, \\\\mathbf{v}_t).\\\\nonumber \\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\",\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    \"$$\\n\",\n    \"    \\\\begin{align}\\n\",\n    \"        & S(V; \\\\mathbf{x}_0) = C(\\\\mathcal{H}(V; \\\\mathbf{x}_0)), \\\\nonumber \\\\\\\\\\n\",\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\",\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\",\n    \"    \\\\end{align}\\n\",\n    \"$$\\n\",\n    \"\\n\",\n    \"**[Step 3]** calculate weight for each sample sequence\\n\",\n    \"\\n\",\n    \"Weight for a each sample sequence is derived on the basis of information theory.  \\n\",\n    \"There are K sample sequences in total, represented with an index k.  \\n\",\n    \"Good control sequence with small cost value get more weight, and vice versa.  \\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"& w(V) = \\\\frac{1}{\\\\eta} \\\\exp\\n\",\n    \"\\\\left( \\n\",\n    \"    -\\\\frac{1}{\\\\lambda}\\n\",\n    \"    \\\\left(\\n\",\n    \"        S(V) + \\\\lambda(1-\\\\alpha) \\\\sum^{T-1}_{t=0} \\\\mathbf{u}_t^T \\\\Sigma^{-1} (\\\\mathbf{u}_t + \\\\epsilon_t) - \\\\rho\\n\",\n    \"    \\\\right)\\n\",\n    \"\\\\right) \\\\nonumber \\\\\\\\\\n\",\n    \"& \\\\eta = \\n\",\n    \"\\\\sum_{k=1}^K \\\\exp\\n\",\n    \"\\\\left( \\n\",\n    \"    -\\\\frac{1}{\\\\lambda}\\n\",\n    \"    \\\\left(\\n\",\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\",\n    \"    \\\\right)\\n\",\n    \"\\\\right)\\\\nonumber \\\\\\\\\\n\",\n    \"& \\\\rho = \\n\",\n    \"\\\\min_k \\n\",\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\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\",\n    \"\\n\",\n    \"Note that $\\\\rho$ is inserted into the formulation to avoid overflow errors during implementation.\\n\",\n    \"\\n\",\n    \"**[Step 4]** get optimal control input sequence\\n\",\n    \"\\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\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"    \\\\mathbf{u}_t^{i+1} % &= \\\\mathbb{E}_{\\\\mathbb{Q}_{\\\\hat{U}, \\\\Sigma}}[w(V)\\\\mathbf{v}_t]\\n\",\n    \"                 = u_t^i + \\\\sum_{k=1}^K w(V_k) \\\\epsilon_t^k \\\\nonumber \\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class MPPIControllerForPathTracking():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            delta_t: float = 0.05,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            vehicle_width: float = 3.0, # [m]\\n\",\n    \"            vehicle_length: float = 4.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"            horizon_step_T: int = 30,\\n\",\n    \"            number_of_samples_K: int = 1000,\\n\",\n    \"            param_exploration: float = 0.0,\\n\",\n    \"            param_lambda: float = 50.0,\\n\",\n    \"            param_alpha: float = 1.0,\\n\",\n    \"            sigma: np.ndarray = np.array([[0.5, 0.0], [0.0, 0.1]]), \\n\",\n    \"            stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            visualize_optimal_traj = True,  # if True, optimal trajectory is visualized\\n\",\n    \"            visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\\n\",\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\",\n    \"            collision_safety_margin_rate: float = 1.2, # safety margin for collision check\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize mppi controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # mppi parameters\\n\",\n    \"        self.dim_x = 4 # dimension of system state vector\\n\",\n    \"        self.dim_u = 2 # dimension of control input vector\\n\",\n    \"        self.T = horizon_step_T # prediction horizon\\n\",\n    \"        self.K = number_of_samples_K # number of sample trajectories\\n\",\n    \"        self.param_exploration = param_exploration  # constant parameter of mppi\\n\",\n    \"        self.param_lambda = param_lambda  # constant parameter of mppi\\n\",\n    \"        self.param_alpha = param_alpha # constant parameter of mppi\\n\",\n    \"        self.param_gamma = self.param_lambda * (1.0 - (self.param_alpha))  # constant parameter of mppi\\n\",\n    \"        self.Sigma = sigma # deviation of noise\\n\",\n    \"        self.stage_cost_weight = stage_cost_weight\\n\",\n    \"        self.terminal_cost_weight = terminal_cost_weight\\n\",\n    \"        self.visualize_optimal_traj = visualize_optimal_traj\\n\",\n    \"        self.visualze_sampled_trajs = visualze_sampled_trajs\\n\",\n    \"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.wheel_base = wheel_base #[m]\\n\",\n    \"        self.vehicle_w = vehicle_width #[m]\\n\",\n    \"        self.vehicle_l = vehicle_length #[m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # obstacle parameters\\n\",\n    \"        self.obstacle_circles = obstacle_circles\\n\",\n    \"        self.collision_safety_margin_rate = collision_safety_margin_rate\\n\",\n    \"\\n\",\n    \"        # mppi variables\\n\",\n    \"        self.u_prev = np.zeros((self.T, self.dim_u))\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\\n\",\n    \"        \\\"\\\"\\\"calculate optimal control input\\\"\\\"\\\"\\n\",\n    \"        # load privious control input sequence\\n\",\n    \"        u = self.u_prev\\n\",\n    \"\\n\",\n    \"        # set initial x value from observation\\n\",\n    \"        x0 = observed_x\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position \\n\",\n    \"        self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # prepare buffer\\n\",\n    \"        S = np.zeros((self.K)) # state cost list\\n\",\n    \"\\n\",\n    \"        # sample noise\\n\",\n    \"        epsilon = self._calc_epsilon(self.Sigma, self.K, self.T, self.dim_u) # size is self.K x self.T\\n\",\n    \"\\n\",\n    \"        # prepare buffer of sampled control input sequence\\n\",\n    \"        v = np.zeros((self.K, self.T, self.dim_u)) # control input sequence with noise\\n\",\n    \"\\n\",\n    \"        # loop for 0 ~ K-1 samples\\n\",\n    \"        for k in range(self.K):         \\n\",\n    \"\\n\",\n    \"            # set initial(t=0) state x i.e. observed state of the vehicle\\n\",\n    \"            x = x0\\n\",\n    \"\\n\",\n    \"            # loop for time step t = 1 ~ T\\n\",\n    \"            for t in range(1, self.T+1):\\n\",\n    \"\\n\",\n    \"                # get control input with noise\\n\",\n    \"                if k < (1.0-self.param_exploration)*self.K:\\n\",\n    \"                    v[k, t-1] = u[t-1] + epsilon[k, t-1] # sampling for exploitation\\n\",\n    \"                else:\\n\",\n    \"                    v[k, t-1] = epsilon[k, t-1] # sampling for exploration\\n\",\n    \"\\n\",\n    \"                # update x\\n\",\n    \"                x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"\\n\",\n    \"                # add stage cost\\n\",\n    \"                S[k] += self._c(x) + self.param_gamma * u[t-1].T @ np.linalg.inv(self.Sigma) @ v[k, t-1]\\n\",\n    \"\\n\",\n    \"            # add terminal cost\\n\",\n    \"            S[k] += self._phi(x)\\n\",\n    \"\\n\",\n    \"        # compute information theoretic weights for each sample\\n\",\n    \"        w = self._compute_weights(S)\\n\",\n    \"\\n\",\n    \"        # calculate w_k * epsilon_k\\n\",\n    \"        w_epsilon = np.zeros((self.T, self.dim_u))\\n\",\n    \"        for t in range(self.T): # loop for time step t = 0 ~ T-1\\n\",\n    \"            for k in range(self.K):\\n\",\n    \"                w_epsilon[t] += w[k] * epsilon[k, t]\\n\",\n    \"\\n\",\n    \"        # apply moving average filter for smoothing input sequence\\n\",\n    \"        w_epsilon = self._moving_average_filter(xx=w_epsilon, window_size=10)\\n\",\n    \"\\n\",\n    \"        # update control input sequence\\n\",\n    \"        u += w_epsilon\\n\",\n    \"\\n\",\n    \"        # calculate optimal trajectory\\n\",\n    \"        optimal_traj = np.zeros((self.T, self.dim_x))\\n\",\n    \"        if self.visualize_optimal_traj:\\n\",\n    \"            x = x0\\n\",\n    \"            for t in range(self.T):\\n\",\n    \"                x = self._F(x, self._g(u[t-1]))\\n\",\n    \"                optimal_traj[t] = x\\n\",\n    \"\\n\",\n    \"        # calculate sampled trajectories\\n\",\n    \"        sampled_traj_list = np.zeros((self.K, self.T, self.dim_x))\\n\",\n    \"        sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\\n\",\n    \"        if self.visualze_sampled_trajs:\\n\",\n    \"            for k in sorted_idx:\\n\",\n    \"                x = x0\\n\",\n    \"                for t in range(self.T):\\n\",\n    \"                    x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"                    sampled_traj_list[k, t] = x\\n\",\n    \"\\n\",\n    \"        # update privious control input sequence (shift 1 step to the left)\\n\",\n    \"        self.u_prev[:-1] = u[1:]\\n\",\n    \"        self.u_prev[-1] = u[-1]\\n\",\n    \"\\n\",\n    \"        # return optimal control input and input sequence\\n\",\n    \"        return u[0], u, optimal_traj, sampled_traj_list\\n\",\n    \"\\n\",\n    \"    def _calc_epsilon(self, sigma: np.ndarray, size_sample: int, size_time_step: int, size_dim_u: int) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"sample epsilon\\\"\\\"\\\"\\n\",\n    \"        # check if sigma row size == sigma col size == size_dim_u and size_dim_u > 0\\n\",\n    \"        if sigma.shape[0] != sigma.shape[1] or sigma.shape[0] != size_dim_u or size_dim_u < 1:\\n\",\n    \"            print(\\\"[ERROR] sigma must be a square matrix with the size of size_dim_u.\\\")\\n\",\n    \"            raise ValueError\\n\",\n    \"\\n\",\n    \"        # sample epsilon\\n\",\n    \"        mu = np.zeros((size_dim_u)) # set average as a zero vector\\n\",\n    \"        epsilon = np.random.multivariate_normal(mu, sigma, (size_sample, size_time_step))\\n\",\n    \"        return epsilon\\n\",\n    \"\\n\",\n    \"    def _g(self, v: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"clamp input\\\"\\\"\\\"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\\n\",\n    \"        v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\\n\",\n    \"        return v\\n\",\n    \"\\n\",\n    \"    def _c(self, x_t: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate stage cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_t\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate stage cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                     self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"        \\n\",\n    \"        # add penalty for collision with obstacles\\n\",\n    \"        stage_cost += self._is_collided(x_t) * 1.0e10\\n\",\n    \"\\n\",\n    \"        return stage_cost\\n\",\n    \"\\n\",\n    \"    def _phi(self, x_T: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate terminal cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_T\\n\",\n    \"        x, y, yaw, v = x_T\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate terminal cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                        self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"        \\n\",\n    \"        # add penalty for collision with obstacles\\n\",\n    \"        terminal_cost += self._is_collided(x_T) * 1.0e10\\n\",\n    \"        \\n\",\n    \"        return terminal_cost\\n\",\n    \"\\n\",\n    \"    def _is_collided(self,  x_t: np.ndarray) -> bool:\\n\",\n    \"        \\\"\\\"\\\"check if the vehicle is collided with obstacles\\\"\\\"\\\"\\n\",\n    \"        # vehicle shape parameters\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        safety_margin_rate = self.collision_safety_margin_rate\\n\",\n    \"        vw, vl = vw*safety_margin_rate, vl*safety_margin_rate\\n\",\n    \"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, _ = x_t\\n\",\n    \"\\n\",\n    \"        # key points for collision check\\n\",\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\",\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\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\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    \"\\n\",\n    \"        # check if the key points are inside the obstacles\\n\",\n    \"        for obs in self.obstacle_circles: # for each circular obstacles\\n\",\n    \"            obs_x, obs_y, obs_r = obs # [m] x, y, radius\\n\",\n    \"            for p in range(len(rotated_vehicle_shape_x)):\\n\",\n    \"                if (rotated_vehicle_shape_x[p]-obs_x)**2 + (rotated_vehicle_shape_y[p]-obs_y)**2 < obs_r**2:\\n\",\n    \"                    return 1.0 # collided\\n\",\n    \"\\n\",\n    \"        return 0.0 # not collided\\n\",\n    \"\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        \\\"\\\"\\\"rotate shape and return location on the x-y plane.\\\"\\\"\\\"\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\\n\",\n    \"\\n\",\n    \"    def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"calculate next state of the vehicle\\\"\\\"\\\"\\n\",\n    \"        # get previous state variables\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        steer, accel = v_t\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"\\n\",\n    \"        # return updated state\\n\",\n    \"        x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        return x_t_plus_1\\n\",\n    \"\\n\",\n    \"    def _compute_weights(self, S: np.ndarray) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"compute weights for each sample\\\"\\\"\\\"\\n\",\n    \"        # prepare buffer\\n\",\n    \"        w = np.zeros((self.K))\\n\",\n    \"\\n\",\n    \"        # calculate rho\\n\",\n    \"        rho = S.min()\\n\",\n    \"\\n\",\n    \"        # calculate eta\\n\",\n    \"        eta = 0.0\\n\",\n    \"        for k in range(self.K):\\n\",\n    \"            eta += np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\\n\",\n    \"\\n\",\n    \"        # calculate weight\\n\",\n    \"        for k in range(self.K):\\n\",\n    \"            w[k] = (1.0 / eta) * np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\\n\",\n    \"        return w\\n\",\n    \"\\n\",\n    \"    def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"apply moving average filter for smoothing input sequence\\n\",\n    \"        Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        b = np.ones(window_size)/window_size\\n\",\n    \"        dim = xx.shape[1]\\n\",\n    \"        xx_mean = np.zeros(xx.shape)\\n\",\n    \"\\n\",\n    \"        for d in range(dim):\\n\",\n    \"            xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\\\"same\\\")\\n\",\n    \"            n_conv = math.ceil(window_size/2)\\n\",\n    \"            xx_mean[0,d] *= window_size/n_conv\\n\",\n    \"            for i in range(1, n_conv):\\n\",\n    \"                xx_mean[i,d] *= window_size/(i+n_conv)\\n\",\n    \"                xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \\n\",\n    \"        return xx_mean\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Obstacle Avoidance\\n\",\n    \"- Longitudinal Control : MPPI Controller\\n\",\n    \"- Lateral Control : MPPI Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 150 # [steps]\\n\",\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    \"\\n\",\n    \"# obstacle params\\n\",\n    \"OBSTACLE_CIRCLES = np.array([\\n\",\n    \"    [+ 8.0, +5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\\n\",\n    \"    [+18.0, -5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\\n\",\n    \"])\\n\",\n    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    wheel_base=2.5,\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \"    obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 0.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize a mppi controller for the vehicle\\n\",\n    \"mppi = MPPIControllerForPathTracking(\\n\",\n    \"    delta_t = delta_t*2.0, # [s]\\n\",\n    \"    wheel_base = 2.5, # [m]\\n\",\n    \"    max_steer_abs = 0.523, # [rad]\\n\",\n    \"    max_accel_abs = 2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \"    horizon_step_T = 20, # [steps]\\n\",\n    \"    number_of_samples_K = 500, # [samples]\\n\",\n    \"    param_exploration = 0.05,\\n\",\n    \"    param_lambda = 100.0,\\n\",\n    \"    param_alpha = 0.98,\\n\",\n    \"    sigma = np.array([[0.075, 0.0], [0.0, 2.0]]),\\n\",\n    \"    stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    visualze_sampled_trajs = True, # if True, sampled trajectories are visualized\\n\",\n    \"    obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\\n\",\n    \"    collision_safety_margin_rate = 1.2, # safety margin for collision check\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate input force with MPPI\\n\",\n    \"        optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = mppi.calc_control_input(\\n\",\n    \"            observed_x = current_state\\n\",\n    \"        )\\n\",\n    \"    except IndexError as e:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\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\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"# save animation\\n\",\n    \"# vehicle.save_animation(\\\"mppi_obstacle_avoidance_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/mppi_pathtracking.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# MPPI (Model Predictive Path-Integral) Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle\\n\",\n    \"\\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.wheel_base = wheel_base#[m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"            optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\\n\",\n    \"            sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\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\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw the predicted optimal trajectory from mppi\\n\",\n    \"        if optimal_traj.any():\\n\",\n    \"            optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\\n\",\n    \"            optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        # draw the sampled trajectories from mppi\\n\",\n    \"        if sampled_traj_list.any():\\n\",\n    \"            min_alpha_value = 0.25\\n\",\n    \"            max_alpha_value = 0.35\\n\",\n    \"            for idx, sampled_traj in enumerate(sampled_traj_list):\\n\",\n    \"                # draw darker for better samples\\n\",\n    \"                alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\\n\",\n    \"                sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\\n\",\n    \"                sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\\n\",\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    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Controller : MPPI Controller\\n\",\n    \"\\n\",\n    \"### Note\\n\",\n    \"The following MPPI implementation follows Algorithms 1 and 2 of the reference paper. \\n\",\n    \"\\n\",\n    \"### Reference\\n\",\n    \"1. G. Williams et al. \\\"Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving\\\" \\n\",\n    \"    - URL : https://ieeexplore.ieee.org/document/8558663\\n\",\n    \"    - PDF : https://arxiv.org/pdf/1707.02342.pdf\\n\",\n    \"\\n\",\n    \"### Brief overview of MPPI algorithm\\n\",\n    \"Here is a general process flow to calculate optimal input with mppi algorithm.\\n\",\n    \"\\n\",\n    \"**[Step 1]** ramdomly sample input sequence\\n\",\n    \"\\n\",\n    \"Mean input sequence $U$ and ramdomly sampled input sequence $V$ are defined as follows.  \\n\",\n    \"Usually, optimal input sequence on the previous step is used as $U$. \\n\",\n    \"$$\\n\",\n    \"    \\\\begin{align}\\n\",\n    \"        & (\\\\mathbf{u}_0, \\\\mathbf{u}_1, ... \\\\mathbf{u}_{T-1}) = U \\\\in \\\\mathbb{R}^{m \\\\times T}, \\\\nonumber \\\\\\\\\\n\",\n    \"        & (\\\\mathbf{v}_0, \\\\mathbf{v}_1, ... \\\\mathbf{v}_{T-1}) = V \\\\in \\\\mathbb{R}^{m \\\\times T}, \\\\nonumber \\\\\\\\\\n\",\n    \"        & \\\\mathbf{v}_t = \\\\mathbf{u}_t + \\\\epsilon_t, \\\\nonumber \\\\\\\\\\n\",\n    \"        & \\\\epsilon_t \\\\sim \\\\mathcal{N}(0, \\\\Sigma).\\\\nonumber \\n\",\n    \"    \\\\end{align}\\n\",\n    \"$$\\n\",\n    \"\\n\",\n    \"\\n\",\n    \"**[Step 2]** predict future states and evaluate cost for each sample\\n\",\n    \"\\n\",\n    \"We assume a discrete time, continuous state-action dynamical system as a control target.  \\n\",\n    \"$\\\\mathbf{x}$ is a system state, and $\\\\mathbf{v}$ is a sampled control input.\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"\\\\mathbf{x}_t  &\\\\in \\\\mathbb{R}^{n}, \\\\nonumber \\\\\\\\\\n\",\n    \"\\\\mathbf{x}_{t+1} &= \\\\mathbf{F}(\\\\mathbf{x}_t, \\\\mathbf{v}_t).\\\\nonumber \\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\",\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    \"$$\\n\",\n    \"    \\\\begin{align}\\n\",\n    \"        & S(V; \\\\mathbf{x}_0) = C(\\\\mathcal{H}(V; \\\\mathbf{x}_0)), \\\\nonumber \\\\\\\\\\n\",\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\",\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\",\n    \"    \\\\end{align}\\n\",\n    \"$$\\n\",\n    \"\\n\",\n    \"**[Step 3]** calculate weight for each sample sequence\\n\",\n    \"\\n\",\n    \"Weight for a each sample sequence is derived on the basis of information theory.  \\n\",\n    \"There are K sample sequences in total, represented with an index k.  \\n\",\n    \"Good control sequence with small cost value get more weight, and vice versa.  \\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"& w(V) = \\\\frac{1}{\\\\eta} \\\\exp\\n\",\n    \"\\\\left( \\n\",\n    \"    -\\\\frac{1}{\\\\lambda}\\n\",\n    \"    \\\\left(\\n\",\n    \"        S(V) + \\\\lambda(1-\\\\alpha) \\\\sum^{T-1}_{t=0} \\\\mathbf{u}_t^T \\\\Sigma^{-1} (\\\\mathbf{u}_t + \\\\epsilon_t) - \\\\rho\\n\",\n    \"    \\\\right)\\n\",\n    \"\\\\right) \\\\nonumber \\\\\\\\\\n\",\n    \"& \\\\eta = \\n\",\n    \"\\\\sum_{k=1}^K \\\\exp\\n\",\n    \"\\\\left( \\n\",\n    \"    -\\\\frac{1}{\\\\lambda}\\n\",\n    \"    \\\\left(\\n\",\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\",\n    \"    \\\\right)\\n\",\n    \"\\\\right)\\\\nonumber \\\\\\\\\\n\",\n    \"& \\\\rho = \\n\",\n    \"\\\\min_k \\n\",\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\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\",\n    \"\\n\",\n    \"Note that $\\\\rho$ is inserted into the formulation to avoid overflow errors during implementation.\\n\",\n    \"\\n\",\n    \"**[Step 4]** get optimal control input sequence\\n\",\n    \"\\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\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"    \\\\mathbf{u}_t^{i+1} % &= \\\\mathbb{E}_{\\\\mathbb{Q}_{\\\\hat{U}, \\\\Sigma}}[w(V)\\\\mathbf{v}_t]\\n\",\n    \"                 = u_t^i + \\\\sum_{k=1}^K w(V_k) \\\\epsilon_t^k \\\\nonumber \\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class MPPIControllerForPathTracking():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            delta_t: float = 0.05,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"            horizon_step_T: int = 30,\\n\",\n    \"            number_of_samples_K: int = 1000,\\n\",\n    \"            param_exploration: float = 0.0,\\n\",\n    \"            param_lambda: float = 50.0,\\n\",\n    \"            param_alpha: float = 1.0,\\n\",\n    \"            sigma: np.ndarray = np.array([[0.5, 0.0], [0.0, 0.1]]), \\n\",\n    \"            stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"            visualize_optimal_traj = True,  # if True, optimal trajectory is visualized\\n\",\n    \"            visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize mppi controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # mppi parameters\\n\",\n    \"        self.dim_x = 4 # dimension of system state vector\\n\",\n    \"        self.dim_u = 2 # dimension of control input vector\\n\",\n    \"        self.T = horizon_step_T # prediction horizon\\n\",\n    \"        self.K = number_of_samples_K # number of sample trajectories\\n\",\n    \"        self.param_exploration = param_exploration  # constant parameter of mppi\\n\",\n    \"        self.param_lambda = param_lambda  # constant parameter of mppi\\n\",\n    \"        self.param_alpha = param_alpha # constant parameter of mppi\\n\",\n    \"        self.param_gamma = self.param_lambda * (1.0 - (self.param_alpha))  # constant parameter of mppi\\n\",\n    \"        self.Sigma = sigma # deviation of noise\\n\",\n    \"        self.stage_cost_weight = stage_cost_weight\\n\",\n    \"        self.terminal_cost_weight = terminal_cost_weight\\n\",\n    \"        self.visualize_optimal_traj = visualize_optimal_traj\\n\",\n    \"        self.visualze_sampled_trajs = visualze_sampled_trajs\\n\",\n    \"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.wheel_base = wheel_base#[m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # mppi variables\\n\",\n    \"        self.u_prev = np.zeros((self.T, self.dim_u))\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\\n\",\n    \"        \\\"\\\"\\\"calculate optimal control input\\\"\\\"\\\"\\n\",\n    \"        # load privious control input sequence\\n\",\n    \"        u = self.u_prev\\n\",\n    \"\\n\",\n    \"        # set initial x value from observation\\n\",\n    \"        x0 = observed_x\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position \\n\",\n    \"        self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # prepare buffer\\n\",\n    \"        S = np.zeros((self.K)) # state cost list\\n\",\n    \"\\n\",\n    \"        # sample noise\\n\",\n    \"        epsilon = self._calc_epsilon(self.Sigma, self.K, self.T, self.dim_u) # size is self.K x self.T\\n\",\n    \"\\n\",\n    \"        # prepare buffer of sampled control input sequence\\n\",\n    \"        v = np.zeros((self.K, self.T, self.dim_u)) # control input sequence with noise\\n\",\n    \"\\n\",\n    \"        # loop for 0 ~ K-1 samples\\n\",\n    \"        for k in range(self.K):         \\n\",\n    \"\\n\",\n    \"            # set initial(t=0) state x i.e. observed state of the vehicle\\n\",\n    \"            x = x0\\n\",\n    \"\\n\",\n    \"            # loop for time step t = 1 ~ T\\n\",\n    \"            for t in range(1, self.T+1):\\n\",\n    \"\\n\",\n    \"                # get control input with noise\\n\",\n    \"                if k < (1.0-self.param_exploration)*self.K:\\n\",\n    \"                    v[k, t-1] = u[t-1] + epsilon[k, t-1] # sampling for exploitation\\n\",\n    \"                else:\\n\",\n    \"                    v[k, t-1] = epsilon[k, t-1] # sampling for exploration\\n\",\n    \"\\n\",\n    \"                # update x\\n\",\n    \"                x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"\\n\",\n    \"                # add stage cost\\n\",\n    \"                S[k] += self._c(x) + self.param_gamma * u[t-1].T @ np.linalg.inv(self.Sigma) @ v[k, t-1]\\n\",\n    \"\\n\",\n    \"            # add terminal cost\\n\",\n    \"            S[k] += self._phi(x)\\n\",\n    \"\\n\",\n    \"        # compute information theoretic weights for each sample\\n\",\n    \"        w = self._compute_weights(S)\\n\",\n    \"\\n\",\n    \"        # calculate w_k * epsilon_k\\n\",\n    \"        w_epsilon = np.zeros((self.T, self.dim_u))\\n\",\n    \"        for t in range(self.T): # loop for time step t = 0 ~ T-1\\n\",\n    \"            for k in range(self.K):\\n\",\n    \"                w_epsilon[t] += w[k] * epsilon[k, t]\\n\",\n    \"\\n\",\n    \"        # apply moving average filter for smoothing input sequence\\n\",\n    \"        w_epsilon = self._moving_average_filter(xx=w_epsilon, window_size=10)\\n\",\n    \"\\n\",\n    \"        # update control input sequence\\n\",\n    \"        u += w_epsilon\\n\",\n    \"\\n\",\n    \"        # calculate optimal trajectory\\n\",\n    \"        optimal_traj = np.zeros((self.T, self.dim_x))\\n\",\n    \"        if self.visualize_optimal_traj:\\n\",\n    \"            x = x0\\n\",\n    \"            for t in range(self.T):\\n\",\n    \"                x = self._F(x, self._g(u[t-1]))\\n\",\n    \"                optimal_traj[t] = x\\n\",\n    \"\\n\",\n    \"        # calculate sampled trajectories\\n\",\n    \"        sampled_traj_list = np.zeros((self.K, self.T, self.dim_x))\\n\",\n    \"        sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\\n\",\n    \"        if self.visualze_sampled_trajs:\\n\",\n    \"            for k in sorted_idx:\\n\",\n    \"                x = x0\\n\",\n    \"                for t in range(self.T):\\n\",\n    \"                    x = self._F(x, self._g(v[k, t-1]))\\n\",\n    \"                    sampled_traj_list[k, t] = x\\n\",\n    \"\\n\",\n    \"        # update privious control input sequence (shift 1 step to the left)\\n\",\n    \"        self.u_prev[:-1] = u[1:]\\n\",\n    \"        self.u_prev[-1] = u[-1]\\n\",\n    \"\\n\",\n    \"        # return optimal control input and input sequence\\n\",\n    \"        return u[0], u, optimal_traj, sampled_traj_list\\n\",\n    \"\\n\",\n    \"    def _calc_epsilon(self, sigma: np.ndarray, size_sample: int, size_time_step: int, size_dim_u: int) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"sample epsilon\\\"\\\"\\\"\\n\",\n    \"        # check if sigma row size == sigma col size == size_dim_u and size_dim_u > 0\\n\",\n    \"        if sigma.shape[0] != sigma.shape[1] or sigma.shape[0] != size_dim_u or size_dim_u < 1:\\n\",\n    \"            print(\\\"[ERROR] sigma must be a square matrix with the size of size_dim_u.\\\")\\n\",\n    \"            raise ValueError\\n\",\n    \"\\n\",\n    \"        # sample epsilon\\n\",\n    \"        mu = np.zeros((size_dim_u)) # set average as a zero vector\\n\",\n    \"        epsilon = np.random.multivariate_normal(mu, sigma, (size_sample, size_time_step))\\n\",\n    \"        return epsilon\\n\",\n    \"\\n\",\n    \"    def _g(self, v: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"clamp input\\\"\\\"\\\"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\\n\",\n    \"        v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\\n\",\n    \"        return v\\n\",\n    \"\\n\",\n    \"    def _c(self, x_t: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate stage cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_t\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate stage cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                     self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"        return stage_cost\\n\",\n    \"\\n\",\n    \"    def _phi(self, x_T: np.ndarray) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate terminal cost\\\"\\\"\\\"\\n\",\n    \"        # parse x_T\\n\",\n    \"        x, y, yaw, v = x_T\\n\",\n    \"        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\\n\",\n    \"\\n\",\n    \"        # calculate terminal cost\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\\n\",\n    \"        terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\\\\n\",\n    \"                        self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\\n\",\n    \"        return terminal_cost\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\\n\",\n    \"\\n\",\n    \"    def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"calculate next state of the vehicle\\\"\\\"\\\"\\n\",\n    \"        # get previous state variables\\n\",\n    \"        x, y, yaw, v = x_t\\n\",\n    \"        steer, accel = v_t\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t\\n\",\n    \"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"\\n\",\n    \"        # return updated state\\n\",\n    \"        x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        return x_t_plus_1\\n\",\n    \"\\n\",\n    \"    def _compute_weights(self, S: np.ndarray) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"compute weights for each sample\\\"\\\"\\\"\\n\",\n    \"        # prepare buffer\\n\",\n    \"        w = np.zeros((self.K))\\n\",\n    \"\\n\",\n    \"        # calculate rho\\n\",\n    \"        rho = S.min()\\n\",\n    \"\\n\",\n    \"        # calculate eta\\n\",\n    \"        eta = 0.0\\n\",\n    \"        for k in range(self.K):\\n\",\n    \"            eta += np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\\n\",\n    \"\\n\",\n    \"        # calculate weight\\n\",\n    \"        for k in range(self.K):\\n\",\n    \"            w[k] = (1.0 / eta) * np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\\n\",\n    \"        return w\\n\",\n    \"\\n\",\n    \"    def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"apply moving average filter for smoothing input sequence\\n\",\n    \"        Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        b = np.ones(window_size)/window_size\\n\",\n    \"        dim = xx.shape[1]\\n\",\n    \"        xx_mean = np.zeros(xx.shape)\\n\",\n    \"\\n\",\n    \"        for d in range(dim):\\n\",\n    \"            xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\\\"same\\\")\\n\",\n    \"            n_conv = math.ceil(window_size/2)\\n\",\n    \"            xx_mean[0,d] *= window_size/n_conv\\n\",\n    \"            for i in range(1, n_conv):\\n\",\n    \"                xx_mean[i,d] *= window_size/(i+n_conv)\\n\",\n    \"                xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \\n\",\n    \"        return xx_mean\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : MPPI Controller\\n\",\n    \"- Lateral Control : MPPI Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    wheel_base=2.5,\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize a mppi controller for the vehicle\\n\",\n    \"mppi = MPPIControllerForPathTracking(\\n\",\n    \"    delta_t = delta_t*2.0, # [s]\\n\",\n    \"    wheel_base = 2.5, # [m]\\n\",\n    \"    max_steer_abs = 0.523, # [rad]\\n\",\n    \"    max_accel_abs = 2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \"    horizon_step_T = 20, # [steps]\\n\",\n    \"    number_of_samples_K = 500, # [samples]\\n\",\n    \"    param_exploration = 0.0,\\n\",\n    \"    param_lambda = 100.0,\\n\",\n    \"    param_alpha = 0.98,\\n\",\n    \"    sigma = np.array([[0.075, 0.0], [0.0, 2.0]]),\\n\",\n    \"    stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\\n\",\n    \"    visualze_sampled_trajs = True\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate input force with MPPI\\n\",\n    \"        optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = mppi.calc_control_input(\\n\",\n    \"            observed_x = current_state\\n\",\n    \"        )\\n\",\n    \"    except IndexError as e:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\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\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"# save animation\\n\",\n    \"# vehicle.save_animation(\\\"mppi_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/ovalpath.csv",
    "content": "x,y,yaw,ref_v\n0,0,0,5.0\n0.1,0,0,5.0\n0.2,0,0,5.0\n0.3,0,0,5.0\n0.4,0,0,5.0\n0.5,0,0,5.0\n0.6,0,0,5.0\n0.7,0,0,5.0\n0.8,0,0,5.0\n0.9,0,0,5.0\n1,0,0,5.0\n1.1,0,0,5.0\n1.2,0,0,5.0\n1.3,0,0,5.0\n1.4,0,0,5.0\n1.5,0,0,5.0\n1.6,0,0,5.0\n1.7,0,0,5.0\n1.8,0,0,5.0\n1.9,0,0,5.0\n2,0,0,5.0\n2.1,0,0,5.0\n2.2,0,0,5.0\n2.3,0,0,5.0\n2.4,0,0,5.0\n2.5,0,0,5.0\n2.6,0,0,5.0\n2.7,0,0,5.0\n2.8,0,0,5.0\n2.9,0,0,5.0\n3,0,0,5.0\n3.1,0,0,5.0\n3.2,0,0,5.0\n3.3,0,0,5.0\n3.4,0,0,5.0\n3.5,0,0,5.0\n3.6,0,0,5.0\n3.7,0,0,5.0\n3.8,0,0,5.0\n3.9,0,0,5.0\n4,0,0,5.0\n4.1,0,0,5.0\n4.2,0,0,5.0\n4.3,0,0,5.0\n4.4,0,0,5.0\n4.5,0,0,5.0\n4.6,0,0,5.0\n4.7,0,0,5.0\n4.8,0,0,5.0\n4.9,0,0,5.0\n5,0,0,5.0\n5.1,0,0,5.0\n5.2,0,0,5.0\n5.3,0,0,5.0\n5.4,0,0,5.0\n5.5,0,0,5.0\n5.6,0,0,5.0\n5.7,0,0,5.0\n5.8,0,0,5.0\n5.9,0,0,5.0\n6,0,0,5.0\n6.1,0,0,5.0\n6.2,0,0,5.0\n6.3,0,0,5.0\n6.4,0,0,5.0\n6.5,0,0,5.0\n6.6,0,0,5.0\n6.7,0,0,5.0\n6.8,0,0,5.0\n6.9,0,0,5.0\n7,0,0,5.0\n7.1,0,0,5.0\n7.2,0,0,5.0\n7.3,0,0,5.0\n7.4,0,0,5.0\n7.5,0,0,5.0\n7.6,0,0,5.0\n7.7,0,0,5.0\n7.8,0,0,5.0\n7.9,0,0,5.0\n8,0,0,5.0\n8.1,0,0,5.0\n8.2,0,0,5.0\n8.3,0,0,5.0\n8.4,0,0,5.0\n8.5,0,0,5.0\n8.6,0,0,5.0\n8.7,0,0,5.0\n8.8,0,0,5.0\n8.9,0,0,5.0\n9,0,0,5.0\n9.1,0,0,5.0\n9.2,0,0,5.0\n9.3,0,0,5.0\n9.4,0,0,5.0\n9.5,0,0,5.0\n9.6,0,0,5.0\n9.7,0,0,5.0\n9.8,0,0,5.0\n9.9,0,0,5.0\n10,0,0,5.0\n10.1,0,0,5.0\n10.2,0,0,5.0\n10.3,0,0,5.0\n10.4,0,0,5.0\n10.5,0,0,5.0\n10.6,0,0,5.0\n10.7,0,0,5.0\n10.8,0,0,5.0\n10.9,0,0,5.0\n11,0,0,5.0\n11.1,0,0,5.0\n11.2,0,0,5.0\n11.3,0,0,5.0\n11.4,0,0,5.0\n11.5,0,0,5.0\n11.6,0,0,5.0\n11.7,0,0,5.0\n11.8,0,0,5.0\n11.9,0,0,5.0\n12,0,0,5.0\n12.1,0,0,5.0\n12.2,0,0,5.0\n12.3,0,0,5.0\n12.4,0,0,5.0\n12.5,0,0,5.0\n12.6,0,0,5.0\n12.7,0,0,5.0\n12.8,0,0,5.0\n12.9,0,0,5.0\n13,0,0,5.0\n13.1,0,0,5.0\n13.2,0,0,5.0\n13.3,0,0,5.0\n13.4,0,0,5.0\n13.5,0,0,5.0\n13.6,0,0,5.0\n13.7,0,0,5.0\n13.8,0,0,5.0\n13.9,0,0,5.0\n14,0,0,5.0\n14.1,0,0,5.0\n14.2,0,0,5.0\n14.3,0,0,5.0\n14.4,0,0,5.0\n14.5,0,0,5.0\n14.6,0,0,5.0\n14.7,0,0,5.0\n14.8,0,0,5.0\n14.9,0,0,5.0\n15,0,0,5.0\n15.15707676,0.00082246,0.005235991,5.0\n15.3141363,0.003289748,0.015707961,5.0\n15.47116139,0.007401595,0.026179941,5.0\n15.62813481,0.013157549,0.036651914,5.0\n15.78503934,0.020556979,0.047123891,5.0\n15.94185779,0.029599074,0.057595866,5.0\n16.09857296,0.040282842,0.068067838,5.0\n16.25516765,0.052607111,0.078539814,5.0\n16.4116247,0.066570531,0.089011794,5.0\n16.56792695,0.082171569,0.099483764,5.0\n16.72405726,0.099408516,0.109955745,5.0\n16.8799985,0.11827948,0.120427721,5.0\n17.03573359,0.138782393,0.13089969,5.0\n17.19124543,0.160915006,0.141371673,5.0\n17.34651698,0.184674891,0.151843639,5.0\n17.5015312,0.210059444,0.162315627,5.0\n17.6562711,0.23706588,0.172787597,5.0\n17.81071972,0.265691239,0.183259567,5.0\n17.96486011,0.295932381,0.193731546,5.0\n18.11867536,0.327785989,0.204203529,5.0\n18.27214862,0.361248571,0.214675497,5.0\n18.42526305,0.396316457,0.225147475,5.0\n18.57800186,0.432985801,0.235619449,5.0\n18.73034831,0.471252583,0.246091417,5.0\n18.88228568,0.511112606,0.256563401,5.0\n19.03379731,0.552561498,0.267035378,5.0\n19.18486659,0.595594715,0.277507355,5.0\n19.33547695,0.640207537,0.287979332,5.0\n19.48561188,0.686395072,0.2984513,5.0\n19.63525492,0.734152256,0.308923266,5.0\n19.78438964,0.78347385,0.319395259,5.0\n19.9329997,0.834354446,0.329867228,5.0\n20.0810688,0.886788466,0.340339214,5.0\n20.22858071,0.940770158,0.350811172,5.0\n20.37551924,0.996293603,0.361283163,5.0\n20.52186829,1.053352712,0.371755123,5.0\n20.6676118,1.111941228,0.38222711,5.0\n20.8127338,1.172052726,0.392699066,5.0\n20.95721836,1.233680615,0.403171067,5.0\n21.10104965,1.296818135,0.41364302,5.0\n21.24421188,1.361458364,0.424115029,5.0\n21.38668937,1.427594213,0.434586984,5.0\n21.52846649,1.495218429,0.445058952,5.0\n21.66952769,1.564323596,0.455530924,5.0\n21.8098575,1.634902137,0.466002907,5.0\n21.94944053,1.706946312,0.476474891,5.0\n22.08826147,1.780448219,0.486946877,5.0\n22.22630511,1.855399799,0.49741883,5.0\n22.3635563,1.931792833,0.507890822,5.0\n22.5,2.009618943,0.518362774,5.0\n22.63562124,2.088869595,0.528834753,5.0\n22.77040514,2.169536098,0.539306755,5.0\n22.90433693,2.251609605,0.549778717,5.0\n23.03740192,2.335081117,0.560250696,5.0\n23.16958553,2.419941481,0.570722636,5.0\n23.30087324,2.506181389,0.58119465,5.0\n23.43125067,2.593791386,0.591666616,5.0\n23.56070352,2.682761863,0.602138582,5.0\n23.68921759,2.773083064,0.612610567,5.0\n23.81677878,2.864745084,0.623082576,5.0\n23.94337312,2.957737872,0.63355452,5.0\n24.06898672,3.05205123,0.644026491,5.0\n24.1936058,3.147674814,0.654498473,5.0\n24.3172167,3.24459814,0.664970444,5.0\n24.43980587,3.342810578,0.675442387,5.0\n24.56135985,3.442301358,0.685914397,5.0\n24.68186532,3.54305957,0.696386369,5.0\n24.80130906,3.645074165,0.706858365,5.0\n24.91967798,3.748333956,0.717330326,5.0\n25.0369591,3.852827618,0.727802278,5.0\n25.15313955,3.958543693,0.738274271,5.0\n25.26820659,4.065470589,0.748746268,5.0\n25.38214761,4.173596579,0.759218221,5.0\n25.49495011,4.282909806,0.769690199,5.0\n25.60660172,4.393398282,0.780162173,5.0\n25.71709019,4.505049892,0.790634181,5.0\n25.82640342,4.617852392,0.801106114,5.0\n25.93452941,4.731793411,0.811578101,5.0\n26.04145631,4.846860455,0.822050058,5.0\n26.14717238,4.963040905,0.832522079,5.0\n26.25166604,5.08032202,0.842994037,5.0\n26.35492583,5.19869094,0.853466006,5.0\n26.45694043,5.318134684,0.863937954,5.0\n26.55769864,5.438640154,0.874409967,5.0\n26.65718942,5.560194134,0.88488193,5.0\n26.75540186,5.682783296,0.895353898,5.0\n26.85232519,5.806394195,0.905825859,5.0\n26.94794877,5.931013277,0.916297882,5.0\n27.04226213,6.056626876,0.926769822,5.0\n27.13525492,6.183221216,0.937241796,5.0\n27.22691694,6.310782415,0.947713784,5.0\n27.31723814,6.439296485,0.958185765,5.0\n27.40620861,6.568749332,0.96865777,5.0\n27.49381861,6.699126761,0.979129692,5.0\n27.58005852,6.830414475,0.98960168,5.0\n27.66491888,6.962598075,1.000073677,5.0\n27.74839039,7.095663068,1.010545652,5.0\n27.8304639,7.229594859,1.021017597,5.0\n27.91113041,7.364378764,1.03148955,5.0\n27.99038106,7.5,1.041961572,5.0\n28.06820717,7.636443696,1.05243354,5.0\n28.1446002,7.773694888,1.062905534,5.0\n28.21955178,7.911738527,1.073377493,5.0\n28.29305369,8.050559473,1.083849451,5.0\n28.36509786,8.190142504,1.094321467,5.0\n28.4356764,8.330472312,1.10479342,5.0\n28.50478157,8.471533509,1.115265377,5.0\n28.57240579,8.613310627,1.125737347,5.0\n28.63854164,8.755788116,1.136209334,5.0\n28.70318186,8.898950354,1.146681371,5.0\n28.76631939,9.04278164,1.157153238,5.0\n28.82794727,9.187266203,1.16762532,5.0\n28.88805877,9.332388198,1.178097236,5.0\n28.94664729,9.47813171,1.188569198,5.0\n29.0037064,9.624480757,1.199041191,5.0\n29.05922984,9.77141929,1.2095132,5.0\n29.11321153,9.918931196,1.219985158,5.0\n29.16564555,10.0670003,1.230457121,5.0\n29.21652615,10.21561036,1.240929074,5.0\n29.26584774,10.36474508,1.251401092,5.0\n29.31360493,10.51438812,1.261873025,5.0\n29.35979246,10.66452305,1.272345057,5.0\n29.40440529,10.81513341,1.282816946,5.0\n29.4474385,10.96620269,1.293289015,5.0\n29.48888739,11.11771432,1.303760961,5.0\n29.52874742,11.26965169,1.314232883,5.0\n29.5670142,11.42199814,1.324704922,5.0\n29.60368354,11.57473695,1.335176902,5.0\n29.63875143,11.72785138,1.345648827,5.0\n29.67221401,11.88132464,1.356120843,5.0\n29.70406762,12.03513989,1.366592785,5.0\n29.73430876,12.18928028,1.377064794,5.0\n29.76293412,12.3437289,1.387536754,5.0\n29.78994056,12.4984688,1.398008705,5.0\n29.81532511,12.65348302,1.408480719,5.0\n29.83908499,12.80875457,1.418952719,5.0\n29.86121761,12.96426641,1.429424609,5.0\n29.88172052,13.1200015,1.439896655,5.0\n29.90059148,13.27594274,1.450368631,5.0\n29.91782843,13.43207305,1.460840563,5.0\n29.93342947,13.5883753,1.471312551,5.0\n29.94739289,13.74483235,1.481784533,5.0\n29.95971716,13.90142704,1.492256507,5.0\n29.97040093,14.05814221,1.502728476,5.0\n29.97944302,14.21496066,1.513200493,5.0\n29.98684245,14.37186519,1.523672436,5.0\n29.99259841,14.52883861,1.534144374,5.0\n29.99671025,14.6858637,1.544616431,5.0\n29.99917754,14.84292324,1.555088353,5.0\n30,15,1.565560336,5.0\n29.99917754,15.15707676,1.576032318,5.0\n29.99671025,15.3141363,1.5865043,5.0\n29.99259841,15.47116139,1.596976223,5.0\n29.98684245,15.62813481,1.607448279,5.0\n29.97944302,15.78503934,1.617920218,5.0\n29.97040093,15.94185779,1.628392161,5.0\n29.95971716,16.09857296,1.638864177,5.0\n29.94739289,16.25516765,1.649336147,5.0\n29.93342947,16.4116247,1.65980812,5.0\n29.91782843,16.56792695,1.670280103,5.0\n29.90059148,16.72405726,1.680752091,5.0\n29.88172052,16.8799985,1.691224023,5.0\n29.86121761,17.03573359,1.701695998,5.0\n29.83908499,17.19124543,1.712168044,5.0\n29.81532511,17.34651698,1.722639934,5.0\n29.78994056,17.5015312,1.733111934,5.0\n29.76293412,17.6562711,1.743583949,5.0\n29.73430876,17.81071972,1.7540559,5.0\n29.70406762,17.96486011,1.76452786,5.0\n29.67221401,18.11867536,1.774999868,5.0\n29.63875143,18.27214862,1.785471811,5.0\n29.60368354,18.42526305,1.795943826,5.0\n29.5670142,18.57800186,1.806415751,5.0\n29.52874742,18.73034831,1.816887731,5.0\n29.48888739,18.88228568,1.827359771,5.0\n29.4474385,19.03379731,1.837831692,5.0\n29.40440529,19.18486659,1.848303639,5.0\n29.35979246,19.33547695,1.858775708,5.0\n29.31360493,19.48561188,1.869247596,5.0\n29.26584774,19.63525492,1.879719629,5.0\n29.21652615,19.78438964,1.890191562,5.0\n29.16564555,19.9329997,1.900663579,5.0\n29.11321153,20.0810688,1.911135541,5.0\n29.05922984,20.22858071,1.921607486,5.0\n29.0037064,20.37551924,1.93207946,5.0\n28.94664729,20.52186829,1.942551456,5.0\n28.88805877,20.6676118,1.95302346,5.0\n28.82794727,20.8127338,1.963495405,5.0\n28.76631939,20.95721836,1.973967341,5.0\n28.70318186,21.10104965,1.984439406,5.0\n28.63854164,21.24421188,1.994911304,5.0\n28.57240579,21.38668937,2.005383317,5.0\n28.50478157,21.52846649,2.015855301,5.0\n28.4356764,21.66952769,2.026327268,5.0\n28.36509786,21.8098575,2.036799228,5.0\n28.29305369,21.94944053,2.04727119,5.0\n28.21955178,22.08826147,2.057743221,5.0\n28.1446002,22.22630511,2.068215157,5.0\n28.06820717,22.3635563,2.078687126,5.0\n27.99038106,22.5,2.089159101,5.0\n27.91113041,22.63562124,2.099631069,5.0\n27.8304639,22.77040514,2.11010312,5.0\n27.74839039,22.90433693,2.12057506,5.0\n27.66491888,23.03740192,2.131047012,5.0\n27.58005852,23.16958553,2.141518942,5.0\n27.49381861,23.30087324,2.151990988,5.0\n27.40620861,23.43125067,2.162462958,5.0\n27.31723814,23.56070352,2.172934872,5.0\n27.22691694,23.68921759,2.183406888,5.0\n27.13525492,23.81677878,2.193878903,5.0\n27.04226213,23.94337312,2.204350857,5.0\n26.94794877,24.06898672,2.214822828,5.0\n26.85232519,24.1936058,2.22529478,5.0\n26.75540186,24.3172167,2.235766791,5.0\n26.65718942,24.43980587,2.246238724,5.0\n26.55769864,24.56135985,2.256710724,5.0\n26.45694043,24.68186532,2.267182686,5.0\n26.35492583,24.80130906,2.277654716,5.0\n26.25166604,24.91967798,2.288126648,5.0\n26.14717238,25.0369591,2.298598595,5.0\n26.04145631,25.15313955,2.309070575,5.0\n25.93452941,25.26820659,2.319542613,5.0\n25.82640342,25.38214761,2.330014548,5.0\n25.71709019,25.49495011,2.340486539,5.0\n25.60660172,25.60660172,2.350958473,5.0\n25.49495011,25.71709019,2.361430508,5.0\n25.38214761,25.82640342,2.371902441,5.0\n25.26820659,25.93452941,2.382374432,5.0\n25.15313955,26.04145631,2.392846367,5.0\n25.0369591,26.14717238,2.403318406,5.0\n24.91967798,26.25166604,2.413790385,5.0\n24.80130906,26.35492583,2.424262333,5.0\n24.68186532,26.45694043,2.434734265,5.0\n24.56135985,26.55769864,2.445206294,5.0\n24.43980587,26.65718942,2.455678257,5.0\n24.3172167,26.75540186,2.466150257,5.0\n24.1936058,26.85232519,2.47662219,5.0\n24.06898672,26.94794877,2.487094201,5.0\n23.94337312,27.04226213,2.497566153,5.0\n23.81677878,27.13525492,2.508038123,5.0\n23.68921759,27.22691694,2.518510078,5.0\n23.56070352,27.31723814,2.528982092,5.0\n23.43125067,27.40620861,2.539454108,5.0\n23.30087324,27.49381861,2.549926022,5.0\n23.16958553,27.58005852,2.560397993,5.0\n23.03740192,27.66491888,2.570870039,5.0\n22.90433693,27.74839039,2.581341968,5.0\n22.77040514,27.8304639,2.59181392,5.0\n22.63562124,27.91113041,2.60228586,5.0\n22.5,27.99038106,2.612757912,5.0\n22.3635563,28.06820717,2.623229879,5.0\n22.22630511,28.1446002,2.633701854,5.0\n22.08826147,28.21955178,2.644173823,5.0\n21.94944053,28.29305369,2.65464576,5.0\n21.8098575,28.36509786,2.66511779,5.0\n21.66952769,28.4356764,2.675589752,5.0\n21.52846649,28.50478157,2.686061712,5.0\n21.38668937,28.57240579,2.696533679,5.0\n21.24421188,28.63854164,2.707005664,5.0\n21.10104965,28.70318186,2.717477677,5.0\n20.95721836,28.76631939,2.727949575,5.0\n20.8127338,28.82794727,2.73842164,5.0\n20.6676118,28.88805877,2.748893575,5.0\n20.52186829,28.94664729,2.75936552,5.0\n20.37551924,29.0037064,2.769837525,5.0\n20.22858071,29.05922984,2.78030952,5.0\n20.0810688,29.11321153,2.790781494,5.0\n19.9329997,29.16564555,2.801253439,5.0\n19.78438964,29.21652615,2.811725401,5.0\n19.63525492,29.26584774,2.822197419,5.0\n19.48561188,29.31360493,2.832669351,5.0\n19.33547695,29.35979246,2.843141384,5.0\n19.18486659,29.40440529,2.853613273,5.0\n19.03379731,29.4474385,2.864085342,5.0\n18.88228568,29.48888739,2.874557288,5.0\n18.73034831,29.52874742,2.885029209,5.0\n18.57800186,29.5670142,2.895501249,5.0\n18.42526305,29.60368354,2.905973229,5.0\n18.27214862,29.63875143,2.916445154,5.0\n18.11867536,29.67221401,2.926917169,5.0\n17.96486011,29.70406762,2.937389112,5.0\n17.81071972,29.73430876,2.94786112,5.0\n17.6562711,29.76293412,2.95833308,5.0\n17.5015312,29.78994056,2.968805031,5.0\n17.34651698,29.81532511,2.979277046,5.0\n17.19124543,29.83908499,2.989749046,5.0\n17.03573359,29.86121761,3.000220936,5.0\n16.8799985,29.88172052,3.010692982,5.0\n16.72405726,29.90059148,3.021164958,5.0\n16.56792695,29.91782843,3.03163689,5.0\n16.4116247,29.93342947,3.042108877,5.0\n16.25516765,29.94739289,3.05258086,5.0\n16.09857296,29.95971716,3.063052833,5.0\n15.94185779,29.97040093,3.073524803,5.0\n15.78503934,29.97944302,3.08399682,5.0\n15.62813481,29.98684245,3.094468763,5.0\n15.47116139,29.99259841,3.104940701,5.0\n15.3141363,29.99671025,3.115412757,5.0\n15.15707676,29.99917754,3.12588468,5.0\n15,30,3.136356663,5.0\n14.9,30,3.141592654,5.0\n14.8,30,3.141592654,5.0\n14.7,30,3.141592654,5.0\n14.6,30,3.141592654,5.0\n14.5,30,3.141592654,5.0\n14.4,30,3.141592654,5.0\n14.3,30,3.141592654,5.0\n14.2,30,3.141592654,5.0\n14.1,30,3.141592654,5.0\n14,30,3.141592654,5.0\n13.9,30,3.141592654,5.0\n13.8,30,3.141592654,5.0\n13.7,30,3.141592654,5.0\n13.6,30,3.141592654,5.0\n13.5,30,3.141592654,5.0\n13.4,30,3.141592654,5.0\n13.3,30,3.141592654,5.0\n13.2,30,3.141592654,5.0\n13.1,30,3.141592654,5.0\n13,30,3.141592654,5.0\n12.9,30,3.141592654,5.0\n12.8,30,3.141592654,5.0\n12.7,30,3.141592654,5.0\n12.6,30,3.141592654,5.0\n12.5,30,3.141592654,5.0\n12.4,30,3.141592654,5.0\n12.3,30,3.141592654,5.0\n12.2,30,3.141592654,5.0\n12.1,30,3.141592654,5.0\n12,30,3.141592654,5.0\n11.9,30,3.141592654,5.0\n11.8,30,3.141592654,5.0\n11.7,30,3.141592654,5.0\n11.6,30,3.141592654,5.0\n11.5,30,3.141592654,5.0\n11.4,30,3.141592654,5.0\n11.3,30,3.141592654,5.0\n11.2,30,3.141592654,5.0\n11.1,30,3.141592654,5.0\n11,30,3.141592654,5.0\n10.9,30,3.141592654,5.0\n10.8,30,3.141592654,5.0\n10.7,30,3.141592654,5.0\n10.6,30,3.141592654,5.0\n10.5,30,3.141592654,5.0\n10.4,30,3.141592654,5.0\n10.3,30,3.141592654,5.0\n10.2,30,3.141592654,5.0\n10.1,30,3.141592654,5.0\n10,30,3.141592654,5.0\n9.9,30,3.141592654,5.0\n9.8,30,3.141592654,5.0\n9.7,30,3.141592654,5.0\n9.6,30,3.141592654,5.0\n9.5,30,3.141592654,5.0\n9.4,30,3.141592654,5.0\n9.3,30,3.141592654,5.0\n9.2,30,3.141592654,5.0\n9.1,30,3.141592654,5.0\n9,30,3.141592654,5.0\n8.9,30,3.141592654,5.0\n8.8,30,3.141592654,5.0\n8.7,30,3.141592654,5.0\n8.6,30,3.141592654,5.0\n8.5,30,3.141592654,5.0\n8.4,30,3.141592654,5.0\n8.3,30,3.141592654,5.0\n8.2,30,3.141592654,5.0\n8.1,30,3.141592654,5.0\n8,30,3.141592654,5.0\n7.9,30,3.141592654,5.0\n7.8,30,3.141592654,5.0\n7.7,30,3.141592654,5.0\n7.6,30,3.141592654,5.0\n7.5,30,3.141592654,5.0\n7.4,30,3.141592654,5.0\n7.3,30,3.141592654,5.0\n7.2,30,3.141592654,5.0\n7.1,30,3.141592654,5.0\n7,30,3.141592654,5.0\n6.9,30,3.141592654,5.0\n6.8,30,3.141592654,5.0\n6.7,30,3.141592654,5.0\n6.6,30,3.141592654,5.0\n6.5,30,3.141592654,5.0\n6.4,30,3.141592654,5.0\n6.3,30,3.141592654,5.0\n6.2,30,3.141592654,5.0\n6.1,30,3.141592654,5.0\n6,30,3.141592654,5.0\n5.9,30,3.141592654,5.0\n5.8,30,3.141592654,5.0\n5.7,30,3.141592654,5.0\n5.6,30,3.141592654,5.0\n5.5,30,3.141592654,5.0\n5.4,30,3.141592654,5.0\n5.3,30,3.141592654,5.0\n5.2,30,3.141592654,5.0\n5.1,30,3.141592654,5.0\n5,30,3.141592654,5.0\n4.9,30,3.141592654,5.0\n4.8,30,3.141592654,5.0\n4.7,30,3.141592654,5.0\n4.6,30,3.141592654,5.0\n4.5,30,3.141592654,5.0\n4.4,30,3.141592654,5.0\n4.3,30,3.141592654,5.0\n4.2,30,3.141592654,5.0\n4.1,30,3.141592654,5.0\n4,30,3.141592654,5.0\n3.9,30,3.141592654,5.0\n3.8,30,3.141592654,5.0\n3.7,30,3.141592654,5.0\n3.6,30,3.141592654,5.0\n3.5,30,3.141592654,5.0\n3.4,30,3.141592654,5.0\n3.3,30,3.141592654,5.0\n3.2,30,3.141592654,5.0\n3.1,30,3.141592654,5.0\n3,30,3.141592654,5.0\n2.9,30,3.141592654,5.0\n2.8,30,3.141592654,5.0\n2.7,30,3.141592654,5.0\n2.6,30,3.141592654,5.0\n2.5,30,3.141592654,5.0\n2.4,30,3.141592654,5.0\n2.3,30,3.141592654,5.0\n2.2,30,3.141592654,5.0\n2.1,30,3.141592654,5.0\n2,30,3.141592654,5.0\n1.9,30,3.141592654,5.0\n1.8,30,3.141592654,5.0\n1.7,30,3.141592654,5.0\n1.6,30,3.141592654,5.0\n1.5,30,3.141592654,5.0\n1.4,30,3.141592654,5.0\n1.3,30,3.141592654,5.0\n1.2,30,3.141592654,5.0\n1.1,30,3.141592654,5.0\n1,30,3.141592654,5.0\n0.9,30,3.141592654,5.0\n0.8,30,3.141592654,5.0\n0.7,30,3.141592654,5.0\n0.6,30,3.141592654,5.0\n0.5,30,3.141592654,5.0\n0.4,30,3.141592654,5.0\n0.3,30,3.141592654,5.0\n0.2,30,3.141592654,5.0\n0.1,30,3.141592654,5.0\n0,30,3.141592654,5.0\n-0.1,30,3.141592654,5.0\n-0.2,30,3.141592654,5.0\n-0.3,30,3.141592654,5.0\n-0.4,30,3.141592654,5.0\n-0.5,30,3.141592654,5.0\n-0.6,30,3.141592654,5.0\n-0.7,30,3.141592654,5.0\n-0.8,30,3.141592654,5.0\n-0.9,30,3.141592654,5.0\n-1,30,3.141592654,5.0\n-1.1,30,3.141592654,5.0\n-1.2,30,3.141592654,5.0\n-1.3,30,3.141592654,5.0\n-1.4,30,3.141592654,5.0\n-1.5,30,3.141592654,5.0\n-1.6,30,3.141592654,5.0\n-1.7,30,3.141592654,5.0\n-1.8,30,3.141592654,5.0\n-1.9,30,3.141592654,5.0\n-2,30,3.141592654,5.0\n-2.1,30,3.141592654,5.0\n-2.2,30,3.141592654,5.0\n-2.3,30,3.141592654,5.0\n-2.4,30,3.141592654,5.0\n-2.5,30,3.141592654,5.0\n-2.6,30,3.141592654,5.0\n-2.7,30,3.141592654,5.0\n-2.8,30,3.141592654,5.0\n-2.9,30,3.141592654,5.0\n-3,30,3.141592654,5.0\n-3.1,30,3.141592654,5.0\n-3.2,30,3.141592654,5.0\n-3.3,30,3.141592654,5.0\n-3.4,30,3.141592654,5.0\n-3.5,30,3.141592654,5.0\n-3.6,30,3.141592654,5.0\n-3.7,30,3.141592654,5.0\n-3.8,30,3.141592654,5.0\n-3.9,30,3.141592654,5.0\n-4,30,3.141592654,5.0\n-4.1,30,3.141592654,5.0\n-4.2,30,3.141592654,5.0\n-4.3,30,3.141592654,5.0\n-4.4,30,3.141592654,5.0\n-4.5,30,3.141592654,5.0\n-4.6,30,3.141592654,5.0\n-4.7,30,3.141592654,5.0\n-4.8,30,3.141592654,5.0\n-4.9,30,3.141592654,5.0\n-5,30,3.141592654,5.0\n-5.1,30,3.141592654,5.0\n-5.2,30,3.141592654,5.0\n-5.3,30,3.141592654,5.0\n-5.4,30,3.141592654,5.0\n-5.5,30,3.141592654,5.0\n-5.6,30,3.141592654,5.0\n-5.7,30,3.141592654,5.0\n-5.8,30,3.141592654,5.0\n-5.9,30,3.141592654,5.0\n-6,30,3.141592654,5.0\n-6.1,30,3.141592654,5.0\n-6.2,30,3.141592654,5.0\n-6.3,30,3.141592654,5.0\n-6.4,30,3.141592654,5.0\n-6.5,30,3.141592654,5.0\n-6.6,30,3.141592654,5.0\n-6.7,30,3.141592654,5.0\n-6.8,30,3.141592654,5.0\n-6.9,30,3.141592654,5.0\n-7,30,3.141592654,5.0\n-7.1,30,3.141592654,5.0\n-7.2,30,3.141592654,5.0\n-7.3,30,3.141592654,5.0\n-7.4,30,3.141592654,5.0\n-7.5,30,3.141592654,5.0\n-7.6,30,3.141592654,5.0\n-7.7,30,3.141592654,5.0\n-7.8,30,3.141592654,5.0\n-7.9,30,3.141592654,5.0\n-8,30,3.141592654,5.0\n-8.1,30,3.141592654,5.0\n-8.2,30,3.141592654,5.0\n-8.3,30,3.141592654,5.0\n-8.4,30,3.141592654,5.0\n-8.5,30,3.141592654,5.0\n-8.6,30,3.141592654,5.0\n-8.7,30,3.141592654,5.0\n-8.8,30,3.141592654,5.0\n-8.9,30,3.141592654,5.0\n-9,30,3.141592654,5.0\n-9.1,30,3.141592654,5.0\n-9.2,30,3.141592654,5.0\n-9.3,30,3.141592654,5.0\n-9.4,30,3.141592654,5.0\n-9.5,30,3.141592654,5.0\n-9.6,30,3.141592654,5.0\n-9.7,30,3.141592654,5.0\n-9.8,30,3.141592654,5.0\n-9.9,30,3.141592654,5.0\n-10,30,3.141592654,5.0\n-10.1,30,3.141592654,5.0\n-10.2,30,3.141592654,5.0\n-10.3,30,3.141592654,5.0\n-10.4,30,3.141592654,5.0\n-10.5,30,3.141592654,5.0\n-10.6,30,3.141592654,5.0\n-10.7,30,3.141592654,5.0\n-10.8,30,3.141592654,5.0\n-10.9,30,3.141592654,5.0\n-11,30,3.141592654,5.0\n-11.1,30,3.141592654,5.0\n-11.2,30,3.141592654,5.0\n-11.3,30,3.141592654,5.0\n-11.4,30,3.141592654,5.0\n-11.5,30,3.141592654,5.0\n-11.6,30,3.141592654,5.0\n-11.7,30,3.141592654,5.0\n-11.8,30,3.141592654,5.0\n-11.9,30,3.141592654,5.0\n-12,30,3.141592654,5.0\n-12.1,30,3.141592654,5.0\n-12.2,30,3.141592654,5.0\n-12.3,30,3.141592654,5.0\n-12.4,30,3.141592654,5.0\n-12.5,30,3.141592654,5.0\n-12.6,30,3.141592654,5.0\n-12.7,30,3.141592654,5.0\n-12.8,30,3.141592654,5.0\n-12.9,30,3.141592654,5.0\n-13,30,3.141592654,5.0\n-13.1,30,3.141592654,5.0\n-13.2,30,3.141592654,5.0\n-13.3,30,3.141592654,5.0\n-13.4,30,3.141592654,5.0\n-13.5,30,3.141592654,5.0\n-13.6,30,3.141592654,5.0\n-13.7,30,3.141592654,5.0\n-13.8,30,3.141592654,5.0\n-13.9,30,3.141592654,5.0\n-14,30,3.141592654,5.0\n-14.1,30,3.141592654,5.0\n-14.2,30,3.141592654,5.0\n-14.3,30,3.141592654,5.0\n-14.4,30,3.141592654,5.0\n-14.5,30,3.141592654,5.0\n-14.6,30,3.141592654,5.0\n-14.7,30,3.141592654,5.0\n-14.8,30,3.141592654,5.0\n-14.9,30,3.141592654,5.0\n-15,30,3.141592654,5.0\n-15.15707676,29.99917754,3.146828644,5.0\n-15.3141363,29.99671025,3.157300627,5.0\n-15.47116139,29.99259841,3.16777255,5.0\n-15.62813481,29.98684245,3.178244606,5.0\n-15.78503934,29.97944302,3.188716545,5.0\n-15.94185779,29.97040093,3.199188488,5.0\n-16.09857296,29.95971716,3.209660504,5.0\n-16.25516765,29.94739289,3.220132474,5.0\n-16.4116247,29.93342947,3.230604447,5.0\n-16.56792695,29.91782843,3.24107643,5.0\n-16.72405726,29.90059148,3.251548417,5.0\n-16.8799985,29.88172052,3.26202035,5.0\n-17.03573359,29.86121761,3.272492325,5.0\n-17.19124543,29.83908499,3.282964371,5.0\n-17.34651698,29.81532511,3.293436261,5.0\n-17.5015312,29.78994056,3.303908261,5.0\n-17.6562711,29.76293412,3.314380276,5.0\n-17.81071972,29.73430876,3.324852227,5.0\n-17.96486011,29.70406762,3.335324187,5.0\n-18.11867536,29.67221401,3.345796195,5.0\n-18.27214862,29.63875143,3.356268138,5.0\n-18.42526305,29.60368354,3.366740153,5.0\n-18.57800186,29.5670142,3.377212078,5.0\n-18.73034831,29.52874742,3.387684058,5.0\n-18.88228568,29.48888739,3.398156098,5.0\n-19.03379731,29.4474385,3.408628019,5.0\n-19.18486659,29.40440529,3.419099965,5.0\n-19.33547695,29.35979246,3.429572034,5.0\n-19.48561188,29.31360493,3.440043923,5.0\n-19.63525492,29.26584774,3.450515956,5.0\n-19.78438964,29.21652615,3.460987889,5.0\n-19.9329997,29.16564555,3.471459906,5.0\n-20.0810688,29.11321153,3.481931868,5.0\n-20.22858071,29.05922984,3.492403813,5.0\n-20.37551924,29.0037064,3.502875787,5.0\n-20.52186829,28.94664729,3.513347783,5.0\n-20.6676118,28.88805877,3.523819787,5.0\n-20.8127338,28.82794727,3.534291732,5.0\n-20.95721836,28.76631939,3.544763667,5.0\n-21.10104965,28.70318186,3.555235732,5.0\n-21.24421188,28.63854164,3.56570763,5.0\n-21.38668937,28.57240579,3.576179644,5.0\n-21.52846649,28.50478157,3.586651628,5.0\n-21.66952769,28.4356764,3.597123595,5.0\n-21.8098575,28.36509786,3.607595555,5.0\n-21.94944053,28.29305369,3.618067517,5.0\n-22.08826147,28.21955178,3.628539548,5.0\n-22.22630511,28.1446002,3.639011484,5.0\n-22.3635563,28.06820717,3.649483453,5.0\n-22.5,27.99038106,3.659955428,5.0\n-22.63562124,27.91113041,3.670427396,5.0\n-22.77040514,27.8304639,3.680899447,5.0\n-22.90433693,27.74839039,3.691371387,5.0\n-23.03740192,27.66491888,3.701843339,5.0\n-23.16958553,27.58005852,3.712315269,5.0\n-23.30087324,27.49381861,3.722787314,5.0\n-23.43125067,27.40620861,3.733259285,5.0\n-23.56070352,27.31723814,3.743731199,5.0\n-23.68921759,27.22691694,3.754203215,5.0\n-23.81677878,27.13525492,3.764675229,5.0\n-23.94337312,27.04226213,3.775147184,5.0\n-24.06898672,26.94794877,3.785619154,5.0\n-24.1936058,26.85232519,3.796091106,5.0\n-24.3172167,26.75540186,3.806563117,5.0\n-24.43980587,26.65718942,3.81703505,5.0\n-24.56135985,26.55769864,3.82750705,5.0\n-24.68186532,26.45694043,3.837979013,5.0\n-24.80130906,26.35492583,3.848451043,5.0\n-24.91967798,26.25166604,3.858922975,5.0\n-25.0369591,26.14717238,3.869394922,5.0\n-25.15313955,26.04145631,3.879866901,5.0\n-25.26820659,25.93452941,3.89033894,5.0\n-25.38214761,25.82640342,3.900810875,5.0\n-25.49495011,25.71709019,3.911282866,5.0\n-25.60660172,25.60660172,3.921754799,5.0\n-25.71709019,25.49495011,3.932226835,5.0\n-25.82640342,25.38214761,3.942698768,5.0\n-25.93452941,25.26820659,3.953170759,5.0\n-26.04145631,25.15313955,3.963642694,5.0\n-26.14717238,25.0369591,3.974114733,5.0\n-26.25166604,24.91967798,3.984586712,5.0\n-26.35492583,24.80130906,3.995058659,5.0\n-26.45694043,24.68186532,4.005530591,5.0\n-26.55769864,24.56135985,4.016002621,5.0\n-26.65718942,24.43980587,4.026474583,5.0\n-26.75540186,24.3172167,4.036946584,5.0\n-26.85232519,24.1936058,4.047418517,5.0\n-26.94794877,24.06898672,4.057890528,5.0\n-27.04226213,23.94337312,4.06836248,5.0\n-27.13525492,23.81677878,4.07883445,5.0\n-27.22691694,23.68921759,4.089306405,5.0\n-27.31723814,23.56070352,4.099778419,5.0\n-27.40620861,23.43125067,4.110250435,5.0\n-27.49381861,23.30087324,4.120722349,5.0\n-27.58005852,23.16958553,4.13119432,5.0\n-27.66491888,23.03740192,4.141666365,5.0\n-27.74839039,22.90433693,4.152138295,5.0\n-27.8304639,22.77040514,4.162610247,5.0\n-27.91113041,22.63562124,4.173082187,5.0\n-27.99038106,22.5,4.183554238,5.0\n-28.06820717,22.3635563,4.194026206,5.0\n-28.1446002,22.22630511,4.204498181,5.0\n-28.21955178,22.08826147,4.21497015,5.0\n-28.29305369,21.94944053,4.225442086,5.0\n-28.36509786,21.8098575,4.235914117,5.0\n-28.4356764,21.66952769,4.246386079,5.0\n-28.50478157,21.52846649,4.256858039,5.0\n-28.57240579,21.38668937,4.267330006,5.0\n-28.63854164,21.24421188,4.27780199,5.0\n-28.70318186,21.10104965,4.288274004,5.0\n-28.76631939,20.95721836,4.298745902,5.0\n-28.82794727,20.8127338,4.309217967,5.0\n-28.88805877,20.6676118,4.319689902,5.0\n-28.94664729,20.52186829,4.330161847,5.0\n-29.0037064,20.37551924,4.340633851,5.0\n-29.05922984,20.22858071,4.351105847,5.0\n-29.11321153,20.0810688,4.361577821,5.0\n-29.16564555,19.9329997,4.372049766,5.0\n-29.21652615,19.78438964,4.382521728,5.0\n-29.26584774,19.63525492,4.392993745,5.0\n-29.31360493,19.48561188,4.403465678,5.0\n-29.35979246,19.33547695,4.413937711,5.0\n-29.40440529,19.18486659,4.4244096,5.0\n-29.4474385,19.03379731,4.434881669,5.0\n-29.48888739,18.88228568,4.445353615,5.0\n-29.52874742,18.73034831,4.455825536,5.0\n-29.5670142,18.57800186,4.466297576,5.0\n-29.60368354,18.42526305,4.476769556,5.0\n-29.63875143,18.27214862,4.487241481,5.0\n-29.67221401,18.11867536,4.497713496,5.0\n-29.70406762,17.96486011,4.508185439,5.0\n-29.73430876,17.81071972,4.518657447,5.0\n-29.76293412,17.6562711,4.529129407,5.0\n-29.78994056,17.5015312,4.539601358,5.0\n-29.81532511,17.34651698,4.550073373,5.0\n-29.83908499,17.19124543,4.560545373,5.0\n-29.86121761,17.03573359,4.571017263,5.0\n-29.88172052,16.8799985,4.581489309,5.0\n-29.90059148,16.72405726,4.591961284,5.0\n-29.91782843,16.56792695,4.602433217,5.0\n-29.93342947,16.4116247,4.612905204,5.0\n-29.94739289,16.25516765,4.623377187,5.0\n-29.95971716,16.09857296,4.63384916,5.0\n-29.97040093,15.94185779,4.64432113,5.0\n-29.97944302,15.78503934,4.654793146,5.0\n-29.98684245,15.62813481,4.665265089,5.0\n-29.99259841,15.47116139,4.675737028,5.0\n-29.99671025,15.3141363,4.686209084,5.0\n-29.99917754,15.15707676,4.696681007,5.0\n-30,15,4.707152989,5.0\n-29.99917754,14.84292324,4.717624971,5.0\n-29.99671025,14.6858637,4.728096954,5.0\n-29.99259841,14.52883861,4.738568877,5.0\n-29.98684245,14.37186519,4.749040933,5.0\n-29.97944302,14.21496066,4.759512871,5.0\n-29.97040093,14.05814221,4.769984814,5.0\n-29.95971716,13.90142704,4.780456831,5.0\n-29.94739289,13.74483235,4.790928801,5.0\n-29.93342947,13.5883753,4.801400774,5.0\n-29.91782843,13.43207305,4.811872757,5.0\n-29.90059148,13.27594274,4.822344744,5.0\n-29.88172052,13.1200015,4.832816676,5.0\n-29.86121761,12.96426641,4.843288652,5.0\n-29.83908499,12.80875457,4.853760698,5.0\n-29.81532511,12.65348302,4.864232588,5.0\n-29.78994056,12.4984688,4.874704588,5.0\n-29.76293412,12.3437289,4.885176603,5.0\n-29.73430876,12.18928028,4.895648554,5.0\n-29.70406762,12.03513989,4.906120514,5.0\n-29.67221401,11.88132464,4.916592522,5.0\n-29.63875143,11.72785138,4.927064465,5.0\n-29.60368354,11.57473695,4.93753648,5.0\n-29.5670142,11.42199814,4.948008405,5.0\n-29.52874742,11.26965169,4.958480385,5.0\n-29.48888739,11.11771432,4.968952425,5.0\n-29.4474385,10.96620269,4.979424346,5.0\n-29.40440529,10.81513341,4.989896292,5.0\n-29.35979246,10.66452305,5.000368361,5.0\n-29.31360493,10.51438812,5.01084025,5.0\n-29.26584774,10.36474508,5.021312283,5.0\n-29.21652615,10.21561036,5.031784215,5.0\n-29.16564555,10.0670003,5.042256233,5.0\n-29.11321153,9.918931196,5.052728186,5.0\n-29.05922984,9.77141929,5.063200149,5.0\n-29.0037064,9.624480757,5.073672107,5.0\n-28.94664729,9.47813171,5.084144116,5.0\n-28.88805877,9.332388198,5.094616109,5.0\n-28.82794727,9.187266203,5.105088071,5.0\n-28.76631939,9.04278164,5.115559987,5.0\n-28.70318186,8.898950354,5.126032069,5.0\n-28.63854164,8.755788116,5.136503936,5.0\n-28.57240579,8.613310627,5.146975973,5.0\n-28.50478157,8.471533509,5.15744796,5.0\n-28.4356764,8.330472312,5.16791993,5.0\n-28.36509786,8.190142504,5.178391887,5.0\n-28.29305369,8.050559473,5.188863841,5.0\n-28.21955178,7.911738527,5.199335857,5.0\n-28.1446002,7.773694888,5.209807814,5.0\n-28.06820717,7.636443696,5.220279773,5.0\n-27.99038106,7.5,5.230751767,5.0\n-27.91113041,7.364378764,5.241223735,5.0\n-27.8304639,7.229594859,5.251695757,5.0\n-27.74839039,7.095663068,5.262167711,5.0\n-27.66491888,6.962598075,5.272639655,5.0\n-27.58005852,6.830414475,5.28311163,5.0\n-27.49381861,6.699126761,5.293583627,5.0\n-27.40620861,6.568749332,5.304055616,5.0\n-27.31723814,6.439296485,5.314527537,5.0\n-27.22691694,6.310782415,5.324999542,5.0\n-27.13525492,6.183221216,5.335471523,5.0\n-27.04226213,6.056626876,5.345943511,5.0\n-26.94794877,5.931013277,5.356415485,5.0\n-26.85232519,5.806394195,5.366887425,5.0\n-26.75540186,5.682783296,5.377359448,5.0\n-26.65718942,5.560194134,5.387831409,5.0\n-26.55769864,5.438640154,5.398303377,5.0\n-26.45694043,5.318134684,5.40877534,5.0\n-26.35492583,5.19869094,5.419247353,5.0\n-26.25166604,5.08032202,5.429719301,5.0\n-26.14717238,4.963040905,5.44019127,5.0\n-26.04145631,4.846860455,5.450663228,5.0\n-25.93452941,4.731793411,5.461135249,5.0\n-25.82640342,4.617852392,5.471607206,5.0\n-25.71709019,4.505049892,5.482079193,5.0\n-25.60660172,4.393398282,5.492551126,5.0\n-25.49495011,4.282909806,5.503023134,5.0\n-25.38214761,4.173596579,5.513495109,5.0\n-25.26820659,4.065470589,5.523967086,5.0\n-25.15313955,3.958543693,5.53443904,5.0\n-25.0369591,3.852827618,5.544911036,5.0\n-24.91967798,3.748333956,5.555383029,5.0\n-24.80130906,3.645074165,5.565854981,5.0\n-24.68186532,3.54305957,5.576326942,5.0\n-24.56135985,3.442301358,5.586798938,5.0\n-24.43980587,3.342810578,5.59727091,5.0\n-24.3172167,3.24459814,5.60774292,5.0\n-24.1936058,3.147674814,5.618214863,5.0\n-24.06898672,3.05205123,5.628686834,5.0\n-23.94337312,2.957737872,5.639158817,5.0\n-23.81677878,2.864745084,5.649630787,5.0\n-23.68921759,2.773083064,5.660102731,5.0\n-23.56070352,2.682761863,5.670574741,5.0\n-23.43125067,2.593791386,5.681046725,5.0\n-23.30087324,2.506181389,5.691518691,5.0\n-23.16958553,2.419941481,5.701990657,5.0\n-23.03740192,2.335081117,5.712462671,5.0\n-22.90433693,2.251609605,5.722934611,5.0\n-22.77040514,2.169536098,5.73340659,5.0\n-22.63562124,2.088869595,5.743878552,5.0\n-22.5,2.009618943,5.754350554,5.0\n-22.3635563,1.931792833,5.764822533,5.0\n-22.22630511,1.855399799,5.775294486,5.0\n-22.08826147,1.780448219,5.785766477,5.0\n-21.94944053,1.706946312,5.79623843,5.0\n-21.8098575,1.634902137,5.806710416,5.0\n-21.66952769,1.564323596,5.8171824,5.0\n-21.52846649,1.495218429,5.827654383,5.0\n-21.38668937,1.427594213,5.838126356,5.0\n-21.24421188,1.361458364,5.848598323,5.0\n-21.10104965,1.296818135,5.859070278,5.0\n-20.95721836,1.233680615,5.869542287,5.0\n-20.8127338,1.172052726,5.880014241,5.0\n-20.6676118,1.111941228,5.890486241,5.0\n-20.52186829,1.053352712,5.900958197,5.0\n-20.37551924,0.996293603,5.911430184,5.0\n-20.22858071,0.940770158,5.921902144,5.0\n-20.0810688,0.886788466,5.932374136,5.0\n-19.9329997,0.834354446,5.942846093,5.0\n-19.78438964,0.78347385,5.953318079,5.0\n-19.63525492,0.734152256,5.963790048,5.0\n-19.48561188,0.686395072,5.974262041,5.0\n-19.33547695,0.640207537,5.984734007,5.0\n-19.18486659,0.595594715,5.995205975,5.0\n-19.03379731,0.552561498,6.005677953,5.0\n-18.88228568,0.511112606,6.016149929,5.0\n-18.73034831,0.471252583,6.026621906,5.0\n-18.57800186,0.432985801,6.037093891,5.0\n-18.42526305,0.396316457,6.047565858,5.0\n-18.27214862,0.361248571,6.058037833,5.0\n-18.11867536,0.327785989,6.068509811,5.0\n-17.96486011,0.295932381,6.078981778,5.0\n-17.81071972,0.265691239,6.089453762,5.0\n-17.6562711,0.23706588,6.09992574,5.0\n-17.5015312,0.210059444,6.11039771,5.0\n-17.34651698,0.184674891,6.120869681,5.0\n-17.19124543,0.160915006,6.131341668,5.0\n-17.03573359,0.138782393,6.141813634,5.0\n-16.8799985,0.11827948,6.152285617,5.0\n-16.72405726,0.099408516,6.162757586,5.0\n-16.56792695,0.082171569,6.173229562,5.0\n-16.4116247,0.066570531,6.183701544,5.0\n-16.25516765,0.052607111,6.194173514,5.0\n-16.09857296,0.040282842,6.204645493,5.0\n-15.94185779,0.029599074,6.215117469,5.0\n-15.78503934,0.020556979,6.225589441,5.0\n-15.62813481,0.013157549,6.236061416,5.0\n-15.47116139,0.007401595,6.246533393,5.0\n-15.3141363,0.003289748,6.257005366,5.0\n-15.15707676,0.00082246,6.267477347,5.0\n-15,0,6.277949316,5.0\n-14.9,0,0,5.0\n-14.8,0,0,5.0\n-14.7,0,0,5.0\n-14.6,0,0,5.0\n-14.5,0,0,5.0\n-14.4,0,0,5.0\n-14.3,0,0,5.0\n-14.2,0,0,5.0\n-14.1,0,0,5.0\n-14,0,0,5.0\n-13.9,0,0,5.0\n-13.8,0,0,5.0\n-13.7,0,0,5.0\n-13.6,0,0,5.0\n-13.5,0,0,5.0\n-13.4,0,0,5.0\n-13.3,0,0,5.0\n-13.2,0,0,5.0\n-13.1,0,0,5.0\n-13,0,0,5.0\n-12.9,0,0,5.0\n-12.8,0,0,5.0\n-12.7,0,0,5.0\n-12.6,0,0,5.0\n-12.5,0,0,5.0\n-12.4,0,0,5.0\n-12.3,0,0,5.0\n-12.2,0,0,5.0\n-12.1,0,0,5.0\n-12,0,0,5.0\n-11.9,0,0,5.0\n-11.8,0,0,5.0\n-11.7,0,0,5.0\n-11.6,0,0,5.0\n-11.5,0,0,5.0\n-11.4,0,0,5.0\n-11.3,0,0,5.0\n-11.2,0,0,5.0\n-11.1,0,0,5.0\n-11,0,0,5.0\n-10.9,0,0,5.0\n-10.8,0,0,5.0\n-10.7,0,0,5.0\n-10.6,0,0,5.0\n-10.5,0,0,5.0\n-10.4,0,0,5.0\n-10.3,0,0,5.0\n-10.2,0,0,5.0\n-10.1,0,0,5.0\n-10,0,0,5.0\n-9.9,0,0,5.0\n-9.8,0,0,5.0\n-9.7,0,0,5.0\n-9.6,0,0,5.0\n-9.5,0,0,5.0\n-9.4,0,0,5.0\n-9.3,0,0,5.0\n-9.2,0,0,5.0\n-9.1,0,0,5.0\n-9,0,0,5.0\n-8.9,0,0,5.0\n-8.8,0,0,5.0\n-8.7,0,0,5.0\n-8.6,0,0,5.0\n-8.5,0,0,5.0\n-8.4,0,0,5.0\n-8.3,0,0,5.0\n-8.2,0,0,5.0\n-8.1,0,0,5.0\n-8,0,0,5.0\n-7.9,0,0,5.0\n-7.8,0,0,5.0\n-7.7,0,0,5.0\n-7.6,0,0,5.0\n-7.5,0,0,5.0\n-7.4,0,0,5.0\n-7.3,0,0,5.0\n-7.2,0,0,5.0\n-7.1,0,0,5.0\n-7,0,0,5.0\n-6.9,0,0,5.0\n-6.8,0,0,5.0\n-6.7,0,0,5.0\n-6.6,0,0,5.0\n-6.5,0,0,5.0\n-6.4,0,0,5.0\n-6.3,0,0,5.0\n-6.2,0,0,5.0\n-6.1,0,0,5.0\n-6,0,0,5.0\n-5.9,0,0,5.0\n-5.8,0,0,5.0\n-5.7,0,0,5.0\n-5.6,0,0,5.0\n-5.5,0,0,5.0\n-5.4,0,0,5.0\n-5.3,0,0,5.0\n-5.2,0,0,5.0\n-5.1,0,0,5.0\n-5,0,0,5.0\n-4.9,0,0,5.0\n-4.8,0,0,5.0\n-4.7,0,0,5.0\n-4.6,0,0,5.0\n-4.5,0,0,5.0\n-4.4,0,0,5.0\n-4.3,0,0,5.0\n-4.2,0,0,5.0\n-4.1,0,0,5.0\n-4,0,0,5.0\n-3.9,0,0,5.0\n-3.8,0,0,5.0\n-3.7,0,0,5.0\n-3.6,0,0,5.0\n-3.5,0,0,5.0\n-3.4,0,0,5.0\n-3.3,0,0,5.0\n-3.2,0,0,5.0\n-3.1,0,0,5.0\n-3,0,0,5.0\n-2.9,0,0,5.0\n-2.8,0,0,5.0\n-2.7,0,0,5.0\n-2.6,0,0,5.0\n-2.5,0,0,5.0\n-2.4,0,0,5.0\n-2.3,0,0,5.0\n-2.2,0,0,5.0\n-2.1,0,0,5.0\n-2,0,0,5.0\n-1.9,0,0,5.0\n-1.8,0,0,5.0\n-1.7,0,0,5.0\n-1.6,0,0,5.0\n-1.5,0,0,5.0\n-1.4,0,0,5.0\n-1.3,0,0,5.0\n-1.2,0,0,5.0\n-1.1,0,0,5.0\n-1,0,0,5.0\n-0.9,0,0,5.0\n-0.8,0,0,5.0\n-0.7,0,0,5.0\n-0.6,0,0,5.0\n-0.5,0,0,5.0\n-0.4,0,0,5.0\n-0.3,0,0,5.0\n-0.2,0,0,5.0\n-0.1,0,0,5.0\n0,0,0,5.0\n"
  },
  {
    "path": "notebooks/pid.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# PID Control\\n\",\n    \"PID stands for Proportional Integral Derivative.\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle \\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Controller : PID Controller\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Longitudinal Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class PIDLongitudinalController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            p_gain: float = +1.2, # P Gain\\n\",\n    \"            i_gain: float = +0.4, # I Gain\\n\",\n    \"            d_gain: float = +0.1, # D Gain\\n\",\n    \"            target_velocity: float = 3.0, # [m/s]\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize pid controller for keeping constant velocity\\\"\\\"\\\"\\n\",\n    \"        # pid control parameters\\n\",\n    \"        self.K_p = p_gain\\n\",\n    \"        self.K_i = i_gain\\n\",\n    \"        self.K_d = d_gain\\n\",\n    \"        self.target_vel = target_velocity\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # calculate tracking error, its integral and derivative\\n\",\n    \"        r = self.target_vel\\n\",\n    \"        y = observed_vel\\n\",\n    \"        e = r - y # tracking error to the traget velocity\\n\",\n    \"        ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\\n\",\n    \"        de = (e - self.pre_e) / delta_t # derivative of the tracking error\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\\n\",\n    \"\\n\",\n    \"        # update previous tracking error\\n\",\n    \"        self.pre_e = e\\n\",\n    \"\\n\",\n    \"        return acc_cmd\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Keeping Speed Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 100 # [step]\\n\",\n    \"delta_t = 0.1 # [s]\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-3.0, 50.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize speed controller\\n\",\n    \"speed_controller = PIDLongitudinalController(\\n\",\n    \"    p_gain = 1.2,\\n\",\n    \"    i_gain = 0.4,\\n\",\n    \"    d_gain = 0.1,\\n\",\n    \"    target_velocity = 3.0, # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"    current_velocity = current_state[3]\\n\",\n    \"\\n\",\n    \"    # calculate control inputs\\n\",\n    \"    steer_input = 0.0  # steering command [rad] # set zero because this is just a test run of speed controller\\n\",\n    \"    accel_input = speed_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t) # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"pid_speed_control_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Lateral Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class PIDLateralController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            p_gain: float = +0.8, # P Gain\\n\",\n    \"            i_gain: float = +0.1, # I Gain\\n\",\n    \"            d_gain: float = +0.1, # D Gain\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize pid controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # pid control parameters\\n\",\n    \"        self.K_p = p_gain\\n\",\n    \"        self.K_i = i_gain\\n\",\n    \"        self.K_d = d_gain\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # set vehicle state variables from observation\\n\",\n    \"        x = observed_x[0]\\n\",\n    \"        y = observed_x[1]\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position \\n\",\n    \"        _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # which side of the reference path is the car on, the right or the left?\\n\",\n    \"        ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\\n\",\n    \"        x1, y1 = ref_x, ref_y\\n\",\n    \"        x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0*np.sin(ref_yaw)\\n\",\n    \"        vx, vy = x2 - x1, y2 - y1\\n\",\n    \"        wx, wy =  x - x1,  y - y1\\n\",\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    \"\\n\",\n    \"        # get tracking error, its integral and derivative\\n\",\n    \"        e = -np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # tracking error to the reference path\\n\",\n    \"        ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\\n\",\n    \"        de = (e - self.pre_e) / delta_t # derivative of the tracking error\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        steer_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\\n\",\n    \"\\n\",\n    \"        # update previous tracking error\\n\",\n    \"        self.pre_e = e\\n\",\n    \"\\n\",\n    \"        return steer_cmd\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : PID Controller\\n\",\n    \"- Lateral Control : PID Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize pid controllers for accelration control\\n\",\n    \"pid_lon_controller = PIDLongitudinalController(\\n\",\n    \"    p_gain = 1.2,\\n\",\n    \"    i_gain = 0.4,\\n\",\n    \"    d_gain = 0.1,\\n\",\n    \"    target_velocity = +5.0 # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# initialize pid controllers for steering control\\n\",\n    \"pid_lat_controller = PIDLateralController(\\n\",\n    \"    p_gain = 1.2,\\n\",\n    \"    i_gain = 10.0,\\n\",\n    \"    d_gain = 0.001,\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate control inputs\\n\",\n    \"        current_velocity = current_state[3]\\n\",\n    \"        accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\\n\",\n    \"        steer_input = pid_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\\n\",\n    \"\\n\",\n    \"    except IndexError as ex:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"pid_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/preliminary/linear_quadratic_regulator_tutorial.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Stabilize A Linear System With Linear Quadratic Regulator\\n\",\n    \"\\n\",\n    \"\\n\",\n    \"## Control Target System\\n\",\n    \"$$\\n\",\n    \"\\\\dot{x}(t) = \\\\begin{bmatrix} 2 & 0 \\\\\\\\ 0 & -5 \\\\end{bmatrix} x(t) + \\\\begin{bmatrix} 1 \\\\\\\\ -2 \\\\end{bmatrix} u(t)\\n\",\n    \"$$\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Case.1 : zero input\\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"{u}(t) = \\\\begin{bmatrix} 0 \\\\end{bmatrix} \\\\nonumber\\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": 1,\n   \"metadata\": {},\n   \"outputs\": [\n    {\n     \"name\": \"stdout\",\n     \"output_type\": \"stream\",\n     \"text\": [\n      \"t = 0.00 [s], x = [3.00, -5.00]\\n\",\n      \"t = 0.10 [s], x = [3.60, -2.50]\\n\",\n      \"t = 0.20 [s], x = [4.32, -1.25]\\n\",\n      \"t = 0.30 [s], x = [5.18, -0.62]\\n\",\n      \"t = 0.40 [s], x = [6.22, -0.31]\\n\",\n      \"t = 0.50 [s], x = [7.46, -0.16]\\n\",\n      \"t = 0.60 [s], x = [8.96, -0.08]\\n\",\n      \"t = 0.70 [s], x = [10.75, -0.04]\\n\",\n      \"t = 0.80 [s], x = [12.90, -0.02]\\n\",\n      \"t = 0.90 [s], x = [15.48, -0.01]\\n\",\n      \"t = 1.00 [s], x = [18.58, -0.00]\\n\",\n      \"t = 1.10 [s], x = [22.29, -0.00]\\n\",\n      \"t = 1.20 [s], x = [26.75, -0.00]\\n\",\n      \"t = 1.30 [s], x = [32.10, -0.00]\\n\",\n      \"t = 1.40 [s], x = [38.52, -0.00]\\n\",\n      \"t = 1.50 [s], x = [46.22, -0.00]\\n\",\n      \"t = 1.60 [s], x = [55.47, -0.00]\\n\",\n      \"t = 1.70 [s], x = [66.56, -0.00]\\n\",\n      \"t = 1.80 [s], x = [79.87, -0.00]\\n\",\n      \"t = 1.90 [s], x = [95.84, -0.00]\\n\",\n      \"t = 2.00 [s], x = [115.01, -0.00]\\n\",\n      \"t = 2.10 [s], x = [138.02, -0.00]\\n\",\n      \"t = 2.20 [s], x = [165.62, -0.00]\\n\",\n      \"t = 2.30 [s], x = [198.74, -0.00]\\n\",\n      \"t = 2.40 [s], x = [238.49, -0.00]\\n\",\n      \"t = 2.50 [s], x = [286.19, -0.00]\\n\",\n      \"t = 2.60 [s], x = [343.43, -0.00]\\n\",\n      \"t = 2.70 [s], x = [412.11, -0.00]\\n\",\n      \"t = 2.80 [s], x = [494.53, -0.00]\\n\",\n      \"t = 2.90 [s], x = [593.44, -0.00]\\n\",\n      \"t = 3.00 [s], x = [712.13, -0.00]\\n\",\n      \"t = 3.10 [s], x = [854.55, -0.00]\\n\",\n      \"t = 3.20 [s], x = [1025.47, -0.00]\\n\",\n      \"t = 3.30 [s], x = [1230.56, -0.00]\\n\",\n      \"t = 3.40 [s], x = [1476.67, -0.00]\\n\",\n      \"t = 3.50 [s], x = [1772.00, -0.00]\\n\",\n      \"t = 3.60 [s], x = [2126.41, -0.00]\\n\",\n      \"t = 3.70 [s], x = [2551.69, -0.00]\\n\",\n      \"t = 3.80 [s], x = [3062.02, -0.00]\\n\",\n      \"t = 3.90 [s], x = [3674.43, -0.00]\\n\",\n      \"t = 4.00 [s], x = [4409.31, -0.00]\\n\",\n      \"t = 4.10 [s], x = [5291.18, -0.00]\\n\",\n      \"t = 4.20 [s], x = [6349.41, -0.00]\\n\",\n      \"t = 4.30 [s], x = [7619.30, -0.00]\\n\",\n      \"t = 4.40 [s], x = [9143.15, -0.00]\\n\",\n      \"t = 4.50 [s], x = [10971.79, -0.00]\\n\",\n      \"t = 4.60 [s], x = [13166.14, -0.00]\\n\",\n      \"t = 4.70 [s], x = [15799.37, -0.00]\\n\",\n      \"t = 4.80 [s], x = [18959.25, -0.00]\\n\",\n      \"t = 4.90 [s], x = [22751.10, -0.00]\\n\",\n      \"t = 5.00 [s], x = [27301.31, -0.00]\\n\",\n      \"t = 5.10 [s], x = [32761.58, -0.00]\\n\",\n      \"t = 5.20 [s], x = [39313.89, -0.00]\\n\",\n      \"t = 5.30 [s], x = [47176.67, -0.00]\\n\",\n      \"t = 5.40 [s], x = [56612.01, -0.00]\\n\",\n      \"t = 5.50 [s], x = [67934.41, -0.00]\\n\",\n      \"t = 5.60 [s], x = [81521.29, -0.00]\\n\",\n      \"t = 5.70 [s], x = [97825.55, -0.00]\\n\",\n      \"t = 5.80 [s], x = [117390.65, -0.00]\\n\",\n      \"t = 5.90 [s], x = [140868.79, -0.00]\\n\",\n      \"t = 6.00 [s], x = [169042.54, -0.00]\\n\",\n      \"t = 6.10 [s], x = [202851.05, -0.00]\\n\",\n      \"t = 6.20 [s], x = [243421.26, -0.00]\\n\",\n      \"t = 6.30 [s], x = [292105.51, -0.00]\\n\",\n      \"t = 6.40 [s], x = [350526.62, -0.00]\\n\",\n      \"t = 6.50 [s], x = [420631.94, -0.00]\\n\",\n      \"t = 6.60 [s], x = [504758.33, -0.00]\\n\",\n      \"t = 6.70 [s], x = [605709.99, -0.00]\\n\",\n      \"t = 6.80 [s], x = [726851.99, -0.00]\\n\",\n      \"t = 6.90 [s], x = [872222.39, -0.00]\\n\",\n      \"t = 7.00 [s], x = [1046666.87, -0.00]\\n\",\n      \"t = 7.10 [s], x = [1256000.24, -0.00]\\n\",\n      \"t = 7.20 [s], x = [1507200.29, -0.00]\\n\",\n      \"t = 7.30 [s], x = [1808640.35, -0.00]\\n\",\n      \"t = 7.40 [s], x = [2170368.42, -0.00]\\n\",\n      \"t = 7.50 [s], x = [2604442.11, -0.00]\\n\",\n      \"t = 7.60 [s], x = [3125330.53, -0.00]\\n\",\n      \"t = 7.70 [s], x = [3750396.64, -0.00]\\n\",\n      \"t = 7.80 [s], x = [4500475.96, -0.00]\\n\",\n      \"t = 7.90 [s], x = [5400571.16, -0.00]\\n\",\n      \"t = 8.00 [s], x = [6480685.39, -0.00]\\n\",\n      \"t = 8.10 [s], x = [7776822.46, -0.00]\\n\",\n      \"t = 8.20 [s], x = [9332186.96, -0.00]\\n\",\n      \"t = 8.30 [s], x = [11198624.35, -0.00]\\n\",\n      \"t = 8.40 [s], x = [13438349.22, -0.00]\\n\",\n      \"t = 8.50 [s], x = [16126019.06, -0.00]\\n\",\n      \"t = 8.60 [s], x = [19351222.87, -0.00]\\n\",\n      \"t = 8.70 [s], x = [23221467.45, -0.00]\\n\",\n      \"t = 8.80 [s], x = [27865760.94, -0.00]\\n\",\n      \"t = 8.90 [s], x = [33438913.12, -0.00]\\n\",\n      \"t = 9.00 [s], x = [40126695.75, -0.00]\\n\",\n      \"t = 9.10 [s], x = [48152034.90, -0.00]\\n\",\n      \"t = 9.20 [s], x = [57782441.88, -0.00]\\n\",\n      \"t = 9.30 [s], x = [69338930.25, -0.00]\\n\",\n      \"t = 9.40 [s], x = [83206716.30, -0.00]\\n\",\n      \"t = 9.50 [s], x = [99848059.56, -0.00]\\n\",\n      \"t = 9.60 [s], x = [119817671.47, -0.00]\\n\",\n      \"t = 9.70 [s], x = [143781205.77, -0.00]\\n\",\n      \"t = 9.80 [s], x = [172537446.92, -0.00]\\n\",\n      \"t = 9.90 [s], x = [207044936.31, -0.00]\\n\"\n     ]\n    },\n    {\n     \"data\": {\n      \"image/png\": \"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\",\n      \"text/plain\": [\n       \"<Figure size 1000x1000 with 3 Axes>\"\n      ]\n     },\n     \"metadata\": {},\n     \"output_type\": \"display_data\"\n    }\n   ],\n   \"source\": [\n    \"# import libraries\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"\\n\",\n    \"# define linear system\\n\",\n    \"n = dim_x = 2 # state dimension\\n\",\n    \"m = dim_u = 1 # input dimension\\n\",\n    \"\\n\",\n    \"A = np.array([[2, 0],\\n\",\n    \"              [0, -5]])\\n\",\n    \"\\n\",\n    \"B = np.array([[1],\\n\",\n    \"              [-2]])\\n\",\n    \"\\n\",\n    \"# set simulation parameters\\n\",\n    \"dt = 0.1 # time step\\n\",\n    \"sim_steps = 100 # number of simulation steps\\n\",\n    \"\\n\",\n    \"# set initial state\\n\",\n    \"t = 0.0 # [s]\\n\",\n    \"x = np.array([[3.0],\\n\",\n    \"              [-5.0]])\\n\",\n    \"\\n\",\n    \"# prepare storage\\n\",\n    \"x_log = np.zeros((dim_x, sim_steps))\\n\",\n    \"u_log = np.zeros((dim_u, sim_steps))\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # announce current state\\n\",\n    \"    print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\\n\",\n    \"\\n\",\n    \"    ##### calculate control input #####\\n\",\n    \"    u = np.array([[0.0]]) # zero input\\n\",\n    \"    ###################################\\n\",\n    \"\\n\",\n    \"    # update state\\n\",\n    \"    x_dot = A @ x + B @ u\\n\",\n    \"    x = x + x_dot * dt\\n\",\n    \"\\n\",\n    \"    # update time\\n\",\n    \"    t = t + dt\\n\",\n    \"\\n\",\n    \"    # store data\\n\",\n    \"    x_log[:, i] = x.flatten()\\n\",\n    \"    u_log[:, i] = u.flatten()\\n\",\n    \"\\n\",\n    \"# plot results\\n\",\n    \"fig, ax = plt.subplots(3, 1, figsize=(10, 10))\\n\",\n    \"time = np.arange(0, sim_steps * dt, dt)\\n\",\n    \"\\n\",\n    \"ax[0].plot(time, x_log[0, :], label='x1', color=\\\"blue\\\")\\n\",\n    \"ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[0].set_xlabel('Time [s]')\\n\",\n    \"ax[0].set_ylabel(r'$x_1$')\\n\",\n    \"\\n\",\n    \"ax[1].plot(time, x_log[1, :], label='x2', color=\\\"blue\\\")\\n\",\n    \"ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[1].set_xlabel('Time [s]')\\n\",\n    \"ax[1].set_ylabel(r'$x_2$')\\n\",\n    \"\\n\",\n    \"ax[2].plot(time, u_log[0, :], label='u', color=\\\"purple\\\")\\n\",\n    \"ax[2].set_xlabel('Time [s]')\\n\",\n    \"ax[2].set_ylabel(r'$u$')\\n\",\n    \"\\n\",\n    \"plt.tight_layout()\\n\",\n    \"plt.show()\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Case.2 : Linear Quadratic Regulator\\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"{u}(t) &= - f \\\\cdot x(t) \\\\nonumber\\\\\\\\ \\n\",\n    \"       &= - \\\\begin{bmatrix} 4.43 & -0.08\\\\end{bmatrix} x(t) \\\\nonumber\\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": 32,\n   \"metadata\": {},\n   \"outputs\": [\n    {\n     \"name\": \"stdout\",\n     \"output_type\": \"stream\",\n     \"text\": [\n      \"solutions : \\n\",\n      \"### sol. 0\\n\",\n      \"0.790090716185432\\n\",\n      \"1.41489373711061\\n\",\n      \"-0.333070844337325\\n\",\n      \"### sol. 1\\n\",\n      \"-0.238962893661010\\n\",\n      \"-0.0144238134144138\\n\",\n      \"0.0957588124024186\\n\",\n      \"### sol. 2\\n\",\n      \"4.67613366903330\\n\",\n      \"0.118577328113591\\n\",\n      \"0.0993577861808820\\n\",\n      \"### sol. 3\\n\",\n      \"-0.247669654823031\\n\",\n      \"-0.172108476299586\\n\",\n      \"-2.76000493791945\\n\",\n      \"#########\\n\"\n     ]\n    }\n   ],\n   \"source\": [\n    \"from sympy import symbols, Eq, solve\\n\",\n    \"\\n\",\n    \"# define symbols\\n\",\n    \"p1, p2, p3 = symbols('p1 p2 p3')\\n\",\n    \"\\n\",\n    \"# define equations\\n\",\n    \"eq1 = Eq(-p1**2 + 4*p1 + 4*p1*p2 - 4*p2**2 + 1, 0)\\n\",\n    \"eq2 = Eq(2*p2**2 - 3*p2 - p1*p2 + 2*p1*p3 - 4*p2*p3, 0)\\n\",\n    \"eq3 = Eq(-p2**2 + 4*p2*p3 - 10*p3 - 4*p3**2 + 1, 0)\\n\",\n    \"\\n\",\n    \"# solve equations\\n\",\n    \"solutions = solve([eq1, eq2, eq3], [p1, p2, p3])\\n\",\n    \"\\n\",\n    \"# output solutions\\n\",\n    \"print(\\\"solutions : \\\")\\n\",\n    \"for idx, s in enumerate(solutions):\\n\",\n    \"    print(f\\\"### sol. {idx}\\\")\\n\",\n    \"    for j in range(len(s)):\\n\",\n    \"        print(s[j].evalf())\\n\",\n    \"print(\\\"###\\\"*3)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": 28,\n   \"metadata\": {},\n   \"outputs\": [\n    {\n     \"name\": \"stdout\",\n     \"output_type\": \"stream\",\n     \"text\": [\n      \"t = 0.00 [s], x = [3.00, -5.00]\\n\",\n      \"t = 0.10 [s], x = [2.23, 0.24]\\n\",\n      \"t = 0.20 [s], x = [1.69, 2.09]\\n\",\n      \"t = 0.30 [s], x = [1.30, 2.51]\\n\",\n      \"t = 0.40 [s], x = [1.00, 2.36]\\n\",\n      \"t = 0.50 [s], x = [0.78, 2.03]\\n\",\n      \"t = 0.60 [s], x = [0.60, 1.67]\\n\",\n      \"t = 0.70 [s], x = [0.47, 1.34]\\n\",\n      \"t = 0.80 [s], x = [0.37, 1.07]\\n\",\n      \"t = 0.90 [s], x = [0.29, 0.84]\\n\",\n      \"t = 1.00 [s], x = [0.22, 0.66]\\n\",\n      \"t = 1.10 [s], x = [0.17, 0.52]\\n\",\n      \"t = 1.20 [s], x = [0.14, 0.41]\\n\",\n      \"t = 1.30 [s], x = [0.11, 0.32]\\n\",\n      \"t = 1.40 [s], x = [0.08, 0.25]\\n\",\n      \"t = 1.50 [s], x = [0.06, 0.19]\\n\",\n      \"t = 1.60 [s], x = [0.05, 0.15]\\n\",\n      \"t = 1.70 [s], x = [0.04, 0.12]\\n\",\n      \"t = 1.80 [s], x = [0.03, 0.09]\\n\",\n      \"t = 1.90 [s], x = [0.02, 0.07]\\n\",\n      \"t = 2.00 [s], x = [0.02, 0.06]\\n\",\n      \"t = 2.10 [s], x = [0.01, 0.04]\\n\",\n      \"t = 2.20 [s], x = [0.01, 0.03]\\n\",\n      \"t = 2.30 [s], x = [0.01, 0.03]\\n\",\n      \"t = 2.40 [s], x = [0.01, 0.02]\\n\",\n      \"t = 2.50 [s], x = [0.01, 0.02]\\n\",\n      \"t = 2.60 [s], x = [0.00, 0.01]\\n\",\n      \"t = 2.70 [s], x = [0.00, 0.01]\\n\",\n      \"t = 2.80 [s], x = [0.00, 0.01]\\n\",\n      \"t = 2.90 [s], x = [0.00, 0.01]\\n\",\n      \"t = 3.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 3.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 4.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 5.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 6.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.90 [s], x = [0.00, 0.00]\\n\"\n     ]\n    },\n    {\n     \"data\": {\n      \"image/png\": \"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\",\n      \"text/plain\": [\n       \"<Figure size 1000x1000 with 3 Axes>\"\n      ]\n     },\n     \"metadata\": {},\n     \"output_type\": \"display_data\"\n    }\n   ],\n   \"source\": [\n    \"# import libraries\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"\\n\",\n    \"# define linear system\\n\",\n    \"n = dim_x = 2 # state dimension\\n\",\n    \"m = dim_u = 1 # input dimension\\n\",\n    \"\\n\",\n    \"A = np.array([[2, 0],\\n\",\n    \"              [0, -5]])\\n\",\n    \"\\n\",\n    \"B = np.array([[1],\\n\",\n    \"              [-2]])\\n\",\n    \"\\n\",\n    \"# set simulation parameters\\n\",\n    \"dt = 0.1 # time step\\n\",\n    \"sim_steps = 100 # number of simulation steps\\n\",\n    \"\\n\",\n    \"# set initial state\\n\",\n    \"t = 0.0 # [s]\\n\",\n    \"x = np.array([[3.0],\\n\",\n    \"              [-5.0]])\\n\",\n    \"\\n\",\n    \"# prepare storage\\n\",\n    \"x_log = np.zeros((dim_x, sim_steps))\\n\",\n    \"u_log = np.zeros((dim_u, sim_steps))\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # announce current state\\n\",\n    \"    print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\\n\",\n    \"\\n\",\n    \"    ##### calculate control input #####\\n\",\n    \"    f = np.array([[4.43, -0.08]]) # feedback gain martrix\\n\",\n    \"    u = - f @ x # state feedback control input\\n\",\n    \"    ###################################\\n\",\n    \"\\n\",\n    \"    # update state\\n\",\n    \"    x_dot = A @ x + B @ u\\n\",\n    \"    x = x + x_dot * dt\\n\",\n    \"\\n\",\n    \"    # update time\\n\",\n    \"    t = t + dt\\n\",\n    \"\\n\",\n    \"    # store data\\n\",\n    \"    x_log[:, i] = x.flatten()\\n\",\n    \"    u_log[:, i] = u.flatten()\\n\",\n    \"\\n\",\n    \"# plot results\\n\",\n    \"fig, ax = plt.subplots(3, 1, figsize=(10, 10))\\n\",\n    \"time = np.arange(0, sim_steps * dt, dt)\\n\",\n    \"\\n\",\n    \"ax[0].plot(time, x_log[0, :], label='x1', color=\\\"blue\\\")\\n\",\n    \"ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[0].set_xlabel('Time [s]')\\n\",\n    \"ax[0].set_ylabel(r'$x_1$')\\n\",\n    \"\\n\",\n    \"ax[1].plot(time, x_log[1, :], label='x2', color=\\\"blue\\\")\\n\",\n    \"ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[1].set_xlabel('Time [s]')\\n\",\n    \"ax[1].set_ylabel(r'$x_2$')\\n\",\n    \"\\n\",\n    \"ax[2].plot(time, u_log[0, :], label='u', color=\\\"purple\\\")\\n\",\n    \"ax[2].set_xlabel('Time [s]')\\n\",\n    \"ax[2].set_ylabel(r'$u$')\\n\",\n    \"\\n\",\n    \"plt.tight_layout()\\n\",\n    \"plt.show()\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \".venv\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 2\n}\n"
  },
  {
    "path": "notebooks/preliminary/state_feedback_control_tutorial.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Stabilize A Linear System With State Feedback Control\\n\",\n    \"\\n\",\n    \"\\n\",\n    \"## Control Target System\\n\",\n    \"$$\\n\",\n    \"\\\\dot{x}(t) = \\\\begin{bmatrix} 2 & 0 \\\\\\\\ 0 & -5 \\\\end{bmatrix} x(t) + \\\\begin{bmatrix} 1 \\\\\\\\ -2 \\\\end{bmatrix} u(t)\\n\",\n    \"$$\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Case.1 : zero input\\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"{u}(t) = \\\\begin{bmatrix} 0 \\\\end{bmatrix} \\\\nonumber\\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": 5,\n   \"metadata\": {},\n   \"outputs\": [\n    {\n     \"name\": \"stdout\",\n     \"output_type\": \"stream\",\n     \"text\": [\n      \"t = 0.00 [s], x = [3.00, -5.00]\\n\",\n      \"t = 0.10 [s], x = [3.60, -2.50]\\n\",\n      \"t = 0.20 [s], x = [4.32, -1.25]\\n\",\n      \"t = 0.30 [s], x = [5.18, -0.62]\\n\",\n      \"t = 0.40 [s], x = [6.22, -0.31]\\n\",\n      \"t = 0.50 [s], x = [7.46, -0.16]\\n\",\n      \"t = 0.60 [s], x = [8.96, -0.08]\\n\",\n      \"t = 0.70 [s], x = [10.75, -0.04]\\n\",\n      \"t = 0.80 [s], x = [12.90, -0.02]\\n\",\n      \"t = 0.90 [s], x = [15.48, -0.01]\\n\",\n      \"t = 1.00 [s], x = [18.58, -0.00]\\n\",\n      \"t = 1.10 [s], x = [22.29, -0.00]\\n\",\n      \"t = 1.20 [s], x = [26.75, -0.00]\\n\",\n      \"t = 1.30 [s], x = [32.10, -0.00]\\n\",\n      \"t = 1.40 [s], x = [38.52, -0.00]\\n\",\n      \"t = 1.50 [s], x = [46.22, -0.00]\\n\",\n      \"t = 1.60 [s], x = [55.47, -0.00]\\n\",\n      \"t = 1.70 [s], x = [66.56, -0.00]\\n\",\n      \"t = 1.80 [s], x = [79.87, -0.00]\\n\",\n      \"t = 1.90 [s], x = [95.84, -0.00]\\n\",\n      \"t = 2.00 [s], x = [115.01, -0.00]\\n\",\n      \"t = 2.10 [s], x = [138.02, -0.00]\\n\",\n      \"t = 2.20 [s], x = [165.62, -0.00]\\n\",\n      \"t = 2.30 [s], x = [198.74, -0.00]\\n\",\n      \"t = 2.40 [s], x = [238.49, -0.00]\\n\",\n      \"t = 2.50 [s], x = [286.19, -0.00]\\n\",\n      \"t = 2.60 [s], x = [343.43, -0.00]\\n\",\n      \"t = 2.70 [s], x = [412.11, -0.00]\\n\",\n      \"t = 2.80 [s], x = [494.53, -0.00]\\n\",\n      \"t = 2.90 [s], x = [593.44, -0.00]\\n\",\n      \"t = 3.00 [s], x = [712.13, -0.00]\\n\",\n      \"t = 3.10 [s], x = [854.55, -0.00]\\n\",\n      \"t = 3.20 [s], x = [1025.47, -0.00]\\n\",\n      \"t = 3.30 [s], x = [1230.56, -0.00]\\n\",\n      \"t = 3.40 [s], x = [1476.67, -0.00]\\n\",\n      \"t = 3.50 [s], x = [1772.00, -0.00]\\n\",\n      \"t = 3.60 [s], x = [2126.41, -0.00]\\n\",\n      \"t = 3.70 [s], x = [2551.69, -0.00]\\n\",\n      \"t = 3.80 [s], x = [3062.02, -0.00]\\n\",\n      \"t = 3.90 [s], x = [3674.43, -0.00]\\n\",\n      \"t = 4.00 [s], x = [4409.31, -0.00]\\n\",\n      \"t = 4.10 [s], x = [5291.18, -0.00]\\n\",\n      \"t = 4.20 [s], x = [6349.41, -0.00]\\n\",\n      \"t = 4.30 [s], x = [7619.30, -0.00]\\n\",\n      \"t = 4.40 [s], x = [9143.15, -0.00]\\n\",\n      \"t = 4.50 [s], x = [10971.79, -0.00]\\n\",\n      \"t = 4.60 [s], x = [13166.14, -0.00]\\n\",\n      \"t = 4.70 [s], x = [15799.37, -0.00]\\n\",\n      \"t = 4.80 [s], x = [18959.25, -0.00]\\n\",\n      \"t = 4.90 [s], x = [22751.10, -0.00]\\n\",\n      \"t = 5.00 [s], x = [27301.31, -0.00]\\n\",\n      \"t = 5.10 [s], x = [32761.58, -0.00]\\n\",\n      \"t = 5.20 [s], x = [39313.89, -0.00]\\n\",\n      \"t = 5.30 [s], x = [47176.67, -0.00]\\n\",\n      \"t = 5.40 [s], x = [56612.01, -0.00]\\n\",\n      \"t = 5.50 [s], x = [67934.41, -0.00]\\n\",\n      \"t = 5.60 [s], x = [81521.29, -0.00]\\n\",\n      \"t = 5.70 [s], x = [97825.55, -0.00]\\n\",\n      \"t = 5.80 [s], x = [117390.65, -0.00]\\n\",\n      \"t = 5.90 [s], x = [140868.79, -0.00]\\n\",\n      \"t = 6.00 [s], x = [169042.54, -0.00]\\n\",\n      \"t = 6.10 [s], x = [202851.05, -0.00]\\n\",\n      \"t = 6.20 [s], x = [243421.26, -0.00]\\n\",\n      \"t = 6.30 [s], x = [292105.51, -0.00]\\n\",\n      \"t = 6.40 [s], x = [350526.62, -0.00]\\n\",\n      \"t = 6.50 [s], x = [420631.94, -0.00]\\n\",\n      \"t = 6.60 [s], x = [504758.33, -0.00]\\n\",\n      \"t = 6.70 [s], x = [605709.99, -0.00]\\n\",\n      \"t = 6.80 [s], x = [726851.99, -0.00]\\n\",\n      \"t = 6.90 [s], x = [872222.39, -0.00]\\n\",\n      \"t = 7.00 [s], x = [1046666.87, -0.00]\\n\",\n      \"t = 7.10 [s], x = [1256000.24, -0.00]\\n\",\n      \"t = 7.20 [s], x = [1507200.29, -0.00]\\n\",\n      \"t = 7.30 [s], x = [1808640.35, -0.00]\\n\",\n      \"t = 7.40 [s], x = [2170368.42, -0.00]\\n\",\n      \"t = 7.50 [s], x = [2604442.11, -0.00]\\n\",\n      \"t = 7.60 [s], x = [3125330.53, -0.00]\\n\",\n      \"t = 7.70 [s], x = [3750396.64, -0.00]\\n\",\n      \"t = 7.80 [s], x = [4500475.96, -0.00]\\n\",\n      \"t = 7.90 [s], x = [5400571.16, -0.00]\\n\",\n      \"t = 8.00 [s], x = [6480685.39, -0.00]\\n\",\n      \"t = 8.10 [s], x = [7776822.46, -0.00]\\n\",\n      \"t = 8.20 [s], x = [9332186.96, -0.00]\\n\",\n      \"t = 8.30 [s], x = [11198624.35, -0.00]\\n\",\n      \"t = 8.40 [s], x = [13438349.22, -0.00]\\n\",\n      \"t = 8.50 [s], x = [16126019.06, -0.00]\\n\",\n      \"t = 8.60 [s], x = [19351222.87, -0.00]\\n\",\n      \"t = 8.70 [s], x = [23221467.45, -0.00]\\n\",\n      \"t = 8.80 [s], x = [27865760.94, -0.00]\\n\",\n      \"t = 8.90 [s], x = [33438913.12, -0.00]\\n\",\n      \"t = 9.00 [s], x = [40126695.75, -0.00]\\n\",\n      \"t = 9.10 [s], x = [48152034.90, -0.00]\\n\",\n      \"t = 9.20 [s], x = [57782441.88, -0.00]\\n\",\n      \"t = 9.30 [s], x = [69338930.25, -0.00]\\n\",\n      \"t = 9.40 [s], x = [83206716.30, -0.00]\\n\",\n      \"t = 9.50 [s], x = [99848059.56, -0.00]\\n\",\n      \"t = 9.60 [s], x = [119817671.47, -0.00]\\n\",\n      \"t = 9.70 [s], x = [143781205.77, -0.00]\\n\",\n      \"t = 9.80 [s], x = [172537446.92, -0.00]\\n\",\n      \"t = 9.90 [s], x = [207044936.31, -0.00]\\n\"\n     ]\n    },\n    {\n     \"data\": {\n      \"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\",\n      \"text/plain\": [\n       \"<Figure size 1000x1000 with 3 Axes>\"\n      ]\n     },\n     \"metadata\": {},\n     \"output_type\": \"display_data\"\n    }\n   ],\n   \"source\": [\n    \"# import libraries\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"\\n\",\n    \"# define linear system\\n\",\n    \"n = dim_x = 2 # state dimension\\n\",\n    \"m = dim_u = 1 # input dimension\\n\",\n    \"\\n\",\n    \"A = np.array([[2, 0],\\n\",\n    \"              [0, -5]])\\n\",\n    \"\\n\",\n    \"B = np.array([[1],\\n\",\n    \"              [-2]])\\n\",\n    \"\\n\",\n    \"# set simulation parameters\\n\",\n    \"dt = 0.1 # time step\\n\",\n    \"sim_steps = 100 # number of simulation steps\\n\",\n    \"\\n\",\n    \"# set initial state\\n\",\n    \"t = 0.0 # [s]\\n\",\n    \"x = np.array([[3.0],\\n\",\n    \"              [-5.0]])\\n\",\n    \"\\n\",\n    \"# prepare storage\\n\",\n    \"x_log = np.zeros((dim_x, sim_steps))\\n\",\n    \"u_log = np.zeros((dim_u, sim_steps))\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # announce current state\\n\",\n    \"    print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\\n\",\n    \"\\n\",\n    \"    ##### calculate control input #####\\n\",\n    \"    u = np.array([[0.0]]) # zero input\\n\",\n    \"    ###################################\\n\",\n    \"\\n\",\n    \"    # update state\\n\",\n    \"    x_dot = A @ x + B @ u\\n\",\n    \"    x = x + x_dot * dt\\n\",\n    \"\\n\",\n    \"    # update time\\n\",\n    \"    t = t + dt\\n\",\n    \"\\n\",\n    \"    # store data\\n\",\n    \"    x_log[:, i] = x.flatten()\\n\",\n    \"    u_log[:, i] = u.flatten()\\n\",\n    \"\\n\",\n    \"# plot results\\n\",\n    \"fig, ax = plt.subplots(3, 1, figsize=(10, 10))\\n\",\n    \"time = np.arange(0, sim_steps * dt, dt)\\n\",\n    \"\\n\",\n    \"ax[0].plot(time, x_log[0, :], label='x1', color=\\\"blue\\\")\\n\",\n    \"ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[0].set_xlabel('Time [s]')\\n\",\n    \"ax[0].set_ylabel(r'$x_1$')\\n\",\n    \"\\n\",\n    \"ax[1].plot(time, x_log[1, :], label='x2', color=\\\"blue\\\")\\n\",\n    \"ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[1].set_xlabel('Time [s]')\\n\",\n    \"ax[1].set_ylabel(r'$x_2$')\\n\",\n    \"\\n\",\n    \"ax[2].plot(time, u_log[0, :], label='u', color=\\\"purple\\\")\\n\",\n    \"ax[2].set_xlabel('Time [s]')\\n\",\n    \"ax[2].set_ylabel(r'$u$')\\n\",\n    \"\\n\",\n    \"plt.tight_layout()\\n\",\n    \"plt.show()\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Case.2 : state feedback control\\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"{u}(t) &= - f \\\\cdot x(t) \\\\nonumber\\\\\\\\ \\n\",\n    \"       &= - \\\\begin{bmatrix} 12/7 & 6/7\\\\end{bmatrix} x(t) \\\\nonumber\\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": 6,\n   \"metadata\": {},\n   \"outputs\": [\n    {\n     \"name\": \"stdout\",\n     \"output_type\": \"stream\",\n     \"text\": [\n      \"t = 0.00 [s], x = [3.00, -5.00]\\n\",\n      \"t = 0.10 [s], x = [3.51, -2.33]\\n\",\n      \"t = 0.20 [s], x = [3.81, -0.36]\\n\",\n      \"t = 0.30 [s], x = [3.95, 1.07]\\n\",\n      \"t = 0.40 [s], x = [3.98, 2.07]\\n\",\n      \"t = 0.50 [s], x = [3.91, 2.75]\\n\",\n      \"t = 0.60 [s], x = [3.79, 3.19]\\n\",\n      \"t = 0.70 [s], x = [3.62, 3.44]\\n\",\n      \"t = 0.80 [s], x = [3.43, 3.55]\\n\",\n      \"t = 0.90 [s], x = [3.22, 3.56]\\n\",\n      \"t = 1.00 [s], x = [3.01, 3.50]\\n\",\n      \"t = 1.10 [s], x = [2.80, 3.38]\\n\",\n      \"t = 1.20 [s], x = [2.59, 3.23]\\n\",\n      \"t = 1.30 [s], x = [2.38, 3.05]\\n\",\n      \"t = 1.40 [s], x = [2.19, 2.87]\\n\",\n      \"t = 1.50 [s], x = [2.01, 2.68]\\n\",\n      \"t = 1.60 [s], x = [1.84, 2.49]\\n\",\n      \"t = 1.70 [s], x = [1.67, 2.30]\\n\",\n      \"t = 1.80 [s], x = [1.53, 2.12]\\n\",\n      \"t = 1.90 [s], x = [1.39, 1.94]\\n\",\n      \"t = 2.00 [s], x = [1.26, 1.78]\\n\",\n      \"t = 2.10 [s], x = [1.14, 1.63]\\n\",\n      \"t = 2.20 [s], x = [1.04, 1.49]\\n\",\n      \"t = 2.30 [s], x = [0.94, 1.35]\\n\",\n      \"t = 2.40 [s], x = [0.85, 1.23]\\n\",\n      \"t = 2.50 [s], x = [0.77, 1.12]\\n\",\n      \"t = 2.60 [s], x = [0.70, 1.01]\\n\",\n      \"t = 2.70 [s], x = [0.63, 0.92]\\n\",\n      \"t = 2.80 [s], x = [0.57, 0.83]\\n\",\n      \"t = 2.90 [s], x = [0.51, 0.75]\\n\",\n      \"t = 3.00 [s], x = [0.46, 0.68]\\n\",\n      \"t = 3.10 [s], x = [0.42, 0.62]\\n\",\n      \"t = 3.20 [s], x = [0.38, 0.56]\\n\",\n      \"t = 3.30 [s], x = [0.34, 0.50]\\n\",\n      \"t = 3.40 [s], x = [0.31, 0.45]\\n\",\n      \"t = 3.50 [s], x = [0.28, 0.41]\\n\",\n      \"t = 3.60 [s], x = [0.25, 0.37]\\n\",\n      \"t = 3.70 [s], x = [0.22, 0.33]\\n\",\n      \"t = 3.80 [s], x = [0.20, 0.30]\\n\",\n      \"t = 3.90 [s], x = [0.18, 0.27]\\n\",\n      \"t = 4.00 [s], x = [0.16, 0.24]\\n\",\n      \"t = 4.10 [s], x = [0.15, 0.22]\\n\",\n      \"t = 4.20 [s], x = [0.13, 0.20]\\n\",\n      \"t = 4.30 [s], x = [0.12, 0.18]\\n\",\n      \"t = 4.40 [s], x = [0.11, 0.16]\\n\",\n      \"t = 4.50 [s], x = [0.10, 0.14]\\n\",\n      \"t = 4.60 [s], x = [0.09, 0.13]\\n\",\n      \"t = 4.70 [s], x = [0.08, 0.12]\\n\",\n      \"t = 4.80 [s], x = [0.07, 0.11]\\n\",\n      \"t = 4.90 [s], x = [0.06, 0.10]\\n\",\n      \"t = 5.00 [s], x = [0.06, 0.09]\\n\",\n      \"t = 5.10 [s], x = [0.05, 0.08]\\n\",\n      \"t = 5.20 [s], x = [0.05, 0.07]\\n\",\n      \"t = 5.30 [s], x = [0.04, 0.06]\\n\",\n      \"t = 5.40 [s], x = [0.04, 0.06]\\n\",\n      \"t = 5.50 [s], x = [0.03, 0.05]\\n\",\n      \"t = 5.60 [s], x = [0.03, 0.05]\\n\",\n      \"t = 5.70 [s], x = [0.03, 0.04]\\n\",\n      \"t = 5.80 [s], x = [0.02, 0.04]\\n\",\n      \"t = 5.90 [s], x = [0.02, 0.03]\\n\",\n      \"t = 6.00 [s], x = [0.02, 0.03]\\n\",\n      \"t = 6.10 [s], x = [0.02, 0.03]\\n\",\n      \"t = 6.20 [s], x = [0.02, 0.02]\\n\",\n      \"t = 6.30 [s], x = [0.01, 0.02]\\n\",\n      \"t = 6.40 [s], x = [0.01, 0.02]\\n\",\n      \"t = 6.50 [s], x = [0.01, 0.02]\\n\",\n      \"t = 6.60 [s], x = [0.01, 0.02]\\n\",\n      \"t = 6.70 [s], x = [0.01, 0.01]\\n\",\n      \"t = 6.80 [s], x = [0.01, 0.01]\\n\",\n      \"t = 6.90 [s], x = [0.01, 0.01]\\n\",\n      \"t = 7.00 [s], x = [0.01, 0.01]\\n\",\n      \"t = 7.10 [s], x = [0.01, 0.01]\\n\",\n      \"t = 7.20 [s], x = [0.01, 0.01]\\n\",\n      \"t = 7.30 [s], x = [0.01, 0.01]\\n\",\n      \"t = 7.40 [s], x = [0.00, 0.01]\\n\",\n      \"t = 7.50 [s], x = [0.00, 0.01]\\n\",\n      \"t = 7.60 [s], x = [0.00, 0.01]\\n\",\n      \"t = 7.70 [s], x = [0.00, 0.01]\\n\",\n      \"t = 7.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 7.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 8.90 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.00 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.10 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.20 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.30 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.40 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.50 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.60 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.70 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.80 [s], x = [0.00, 0.00]\\n\",\n      \"t = 9.90 [s], x = [0.00, 0.00]\\n\"\n     ]\n    },\n    {\n     \"data\": {\n      \"image/png\": \"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\",\n      \"text/plain\": [\n       \"<Figure size 1000x1000 with 3 Axes>\"\n      ]\n     },\n     \"metadata\": {},\n     \"output_type\": \"display_data\"\n    }\n   ],\n   \"source\": [\n    \"# import libraries\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"\\n\",\n    \"# define linear system\\n\",\n    \"n = dim_x = 2 # state dimension\\n\",\n    \"m = dim_u = 1 # input dimension\\n\",\n    \"\\n\",\n    \"A = np.array([[2, 0],\\n\",\n    \"              [0, -5]])\\n\",\n    \"\\n\",\n    \"B = np.array([[1],\\n\",\n    \"              [-2]])\\n\",\n    \"\\n\",\n    \"# set simulation parameters\\n\",\n    \"dt = 0.1 # time step\\n\",\n    \"sim_steps = 100 # number of simulation steps\\n\",\n    \"\\n\",\n    \"# set initial state\\n\",\n    \"t = 0.0 # [s]\\n\",\n    \"x = np.array([[3.0],\\n\",\n    \"              [-5.0]])\\n\",\n    \"\\n\",\n    \"# prepare storage\\n\",\n    \"x_log = np.zeros((dim_x, sim_steps))\\n\",\n    \"u_log = np.zeros((dim_u, sim_steps))\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # announce current state\\n\",\n    \"    print(f't = {t:.2f} [s], x = [{x[0, 0]:.2f}, {x[1, 0]:.2f}]')\\n\",\n    \"\\n\",\n    \"    ##### calculate control input #####\\n\",\n    \"    f = np.array([[12.0/7.0, 6.0/7.0]]) # feedback gain martrix\\n\",\n    \"    u = - f @ x # state feedback control input\\n\",\n    \"    ###################################\\n\",\n    \"\\n\",\n    \"    # update state\\n\",\n    \"    x_dot = A @ x + B @ u\\n\",\n    \"    x = x + x_dot * dt\\n\",\n    \"\\n\",\n    \"    # update time\\n\",\n    \"    t = t + dt\\n\",\n    \"\\n\",\n    \"    # store data\\n\",\n    \"    x_log[:, i] = x.flatten()\\n\",\n    \"    u_log[:, i] = u.flatten()\\n\",\n    \"\\n\",\n    \"# plot results\\n\",\n    \"fig, ax = plt.subplots(3, 1, figsize=(10, 10))\\n\",\n    \"time = np.arange(0, sim_steps * dt, dt)\\n\",\n    \"\\n\",\n    \"ax[0].plot(time, x_log[0, :], label='x1', color=\\\"blue\\\")\\n\",\n    \"ax[0].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[0].set_xlabel('Time [s]')\\n\",\n    \"ax[0].set_ylabel(r'$x_1$')\\n\",\n    \"\\n\",\n    \"ax[1].plot(time, x_log[1, :], label='x2', color=\\\"blue\\\")\\n\",\n    \"ax[1].axhline(y=0.0, linestyle='dashed', color='gray', label='0')\\n\",\n    \"ax[1].set_xlabel('Time [s]')\\n\",\n    \"ax[1].set_ylabel(r'$x_2$')\\n\",\n    \"\\n\",\n    \"ax[2].plot(time, u_log[0, :], label='u', color=\\\"purple\\\")\\n\",\n    \"ax[2].set_xlabel('Time [s]')\\n\",\n    \"ax[2].set_ylabel(r'$u$')\\n\",\n    \"\\n\",\n    \"plt.tight_layout()\\n\",\n    \"plt.show()\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \".venv\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 2\n}\n"
  },
  {
    "path": "notebooks/purepursuit.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Pure Pursuit Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle \\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"            look_ahead_point: list = [0.0, 0.0], # look ahead point\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj, look_ahead_point)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, look_ahead_point: list) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the look ahead point\\n\",\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    \"\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame += self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Longitudinal Controller : PID Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class PIDLongitudinalController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            p_gain: float = +1.2, # P Gain\\n\",\n    \"            i_gain: float = +0.4, # I Gain\\n\",\n    \"            d_gain: float = +0.1, # D Gain\\n\",\n    \"            target_velocity: float = 3.0, # [m/s]\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize pid controller for keeping constant velocity\\\"\\\"\\\"\\n\",\n    \"        # pid control parameters\\n\",\n    \"        self.K_p = p_gain\\n\",\n    \"        self.K_i = i_gain\\n\",\n    \"        self.K_d = d_gain\\n\",\n    \"        self.target_vel = target_velocity\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # calculate tracking error, its integral and derivative\\n\",\n    \"        r = self.target_vel\\n\",\n    \"        y = observed_vel\\n\",\n    \"        e = r - y # tracking error to the traget velocity\\n\",\n    \"        ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\\n\",\n    \"        de = (e - self.pre_e) / delta_t # derivative of the tracking error\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\\n\",\n    \"\\n\",\n    \"        # update previous tracking error\\n\",\n    \"        self.pre_e = e\\n\",\n    \"\\n\",\n    \"        return acc_cmd\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Lateral Controller : Pure Pursuit Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class PurePursuitLateralController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            look_ahead_dist: float = 5.0, # [m]\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize pure pursuit controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # pure pursuit control parameters\\n\",\n    \"        self.look_ahead_dist = look_ahead_dist\\n\",\n    \"        self.wheel_base = wheel_base\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # set vehicle state variables from observation\\n\",\n    \"        x = observed_x[0]\\n\",\n    \"        y = observed_x[1]\\n\",\n    \"        yaw = observed_x[2]\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position \\n\",\n    \"        _, ref_x, ref_y, ref_yaw, _ = self._get_lookahead_waypoint(x, y, self.look_ahead_dist, update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        alpha = np.arctan2(ref_y - y, ref_x - x) - yaw # vehicle heading angle error\\n\",\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    \"\\n\",\n    \"        return steer_cmd, [ref_x, ref_y]\\n\",\n    \"\\n\",\n    \"    def _get_lookahead_waypoint(self, x: float, y: float, look_ahead_dist: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the target waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [np.abs(-look_ahead_dist**2 + idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        target_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the look-ahead waypoint\\n\",\n    \"        ref_x = self.ref_path[target_idx,0]\\n\",\n    \"        ref_y = self.ref_path[target_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[target_idx,2]\\n\",\n    \"        ref_v = self.ref_path[target_idx,3]\\n\",\n    \"\\n\",\n    \"        # update target waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = target_idx\\n\",\n    \"\\n\",\n    \"        return target_idx, ref_x, ref_y, ref_yaw, ref_v\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : PID Controller\\n\",\n    \"- Lateral Control : Pure Pursuit Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize pid controller for acceleration control\\n\",\n    \"pid_lon_controller = PIDLongitudinalController(\\n\",\n    \"    p_gain = 1.2,\\n\",\n    \"    i_gain = 0.4,\\n\",\n    \"    d_gain = 0.1,\\n\",\n    \"    target_velocity = +5.0 # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# initialize pure pursuit controller for steering control\\n\",\n    \"purepursuit_lat_controller = PurePursuitLateralController(\\n\",\n    \"    look_ahead_dist = 5.0, # [m]\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate control inputs\\n\",\n    \"        current_velocity = current_state[3]\\n\",\n    \"        accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\\n\",\n    \"        steer_input, look_ahead_point = purepursuit_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\\n\",\n    \"\\n\",\n    \"    except IndexError as ex:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\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\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"purepursuit_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/stanley.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Stanley Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import math\\n\",\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle \\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Longitudinal Controller : PID Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class PIDLongitudinalController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            p_gain: float = +1.2, # P Gain\\n\",\n    \"            i_gain: float = +0.4, # I Gain\\n\",\n    \"            d_gain: float = +0.1, # D Gain\\n\",\n    \"            target_velocity: float = 3.0, # [m/s]\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize pid controller for keeping constant velocity\\\"\\\"\\\"\\n\",\n    \"        # pid control parameters\\n\",\n    \"        self.K_p = p_gain\\n\",\n    \"        self.K_i = i_gain\\n\",\n    \"        self.K_d = d_gain\\n\",\n    \"        self.target_vel = target_velocity\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # calculate tracking error, its integral and derivative\\n\",\n    \"        r = self.target_vel\\n\",\n    \"        y = observed_vel\\n\",\n    \"        e = r - y # tracking error to the traget velocity\\n\",\n    \"        ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\\n\",\n    \"        de = (e - self.pre_e) / delta_t # derivative of the tracking error\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\\n\",\n    \"\\n\",\n    \"        # update previous tracking error\\n\",\n    \"        self.pre_e = e\\n\",\n    \"\\n\",\n    \"        return acc_cmd\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Lateral Controller : Stanley Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class StanleyLateralController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m] distance between the center of gravity and the front axle\\n\",\n    \"            cross_track_error_gain: float = 0.5, # gain for the lateral error\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize stanley controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # stanley control parameters\\n\",\n    \"        self.k = cross_track_error_gain # gain for the lateral error\\n\",\n    \"        self.l_f = l_f # [m] distance between the center of gravity and the front axle\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # set vehicle state variables from observation\\n\",\n    \"        x = observed_x[0]\\n\",\n    \"        y = observed_x[1]\\n\",\n    \"        yaw = observed_x[2]\\n\",\n    \"        v = observed_x[3]\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position\\n\",\n    \"        front_x, front_y = x + self.l_f * np.cos(yaw), y + self.l_f * np.sin(yaw)\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(front_x, front_y, update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # which side of the reference path is the car on, the right or the left?\\n\",\n    \"        ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\\n\",\n    \"        x1, y1 = ref_x, ref_y\\n\",\n    \"        x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0*np.sin(ref_yaw)\\n\",\n    \"        vx, vy = x2 - x1, y2 - y1\\n\",\n    \"        wx, wy =  front_x - x1,  front_y - y1\\n\",\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    \"\\n\",\n    \"        # get tracking error\\n\",\n    \"        e = -np.sign(s) * np.sqrt((ref_x-front_x)**2 + (ref_y-front_y)**2) # lateral error \\n\",\n    \"        alpha = ref_yaw - yaw # heading error\\n\",\n    \"        alpha = np.arctan2(np.sin(alpha), np.cos(alpha)) # normalize heading error to [-pi, pi]\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        epsilon = 0.1 # to avoid zero division when v is too small\\n\",\n    \"        steer_cmd = alpha + np.arctan2(self.k * e, v + epsilon)\\n\",\n    \"\\n\",\n    \"        return steer_cmd\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : PID Controller\\n\",\n    \"- Lateral Control : Stanley Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize pid controller for acceleration control\\n\",\n    \"pid_lon_controller = PIDLongitudinalController(\\n\",\n    \"    p_gain = 1.2,\\n\",\n    \"    i_gain = 0.4,\\n\",\n    \"    d_gain = 0.1,\\n\",\n    \"    target_velocity = +5.0 # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# initialize stanley controller for steering control\\n\",\n    \"stanley_lat_controller = StanleyLateralController(\\n\",\n    \"    cross_track_error_gain = 1.5, # gain for the lateral error\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate control inputs\\n\",\n    \"        current_velocity = current_state[3]\\n\",\n    \"        accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\\n\",\n    \"        steer_input = stanley_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\\n\",\n    \"\\n\",\n    \"    except IndexError as ex:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"stanley_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/state_feedback.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# State Feedback Control\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Control Target : Vehicle \\n\",\n    \"- Longitudinal dynamics : Point Mass Model\\n\",\n    \"- Lateral dynamics : Kinematic Bicycle Model\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            l_f: float = 1.5, # [m]\\n\",\n    \"            l_r: float = 1.0, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.l_f = l_f # [m]\\n\",\n    \"        self.l_r = l_r # [m]\\n\",\n    \"        self.wheel_base = l_f + l_r # [m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        l_r = self.l_r\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        beta = np.arctan(l_r / l * np.tan(steer))\\n\",\n    \"        new_x = x + v * np.cos(yaw + beta) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw + beta) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.sin(beta) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Longitudinal Controller : PID Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class PIDLongitudinalController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            p_gain: float = +1.2, # P Gain\\n\",\n    \"            i_gain: float = +0.4, # I Gain\\n\",\n    \"            d_gain: float = +0.1, # D Gain\\n\",\n    \"            target_velocity: float = 3.0, # [m/s]\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize pid controller for keeping constant velocity\\\"\\\"\\\"\\n\",\n    \"        # pid control parameters\\n\",\n    \"        self.K_p = p_gain\\n\",\n    \"        self.K_i = i_gain\\n\",\n    \"        self.K_d = d_gain\\n\",\n    \"        self.target_vel = target_velocity\\n\",\n    \"        self.pre_e = 0.0 # previous tracking error\\n\",\n    \"        self.integrated_e = 0.0 # integrated tracking error\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # calculate tracking error, its integral and derivative\\n\",\n    \"        r = self.target_vel\\n\",\n    \"        y = observed_vel\\n\",\n    \"        e = r - y # tracking error to the traget velocity\\n\",\n    \"        ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\\n\",\n    \"        de = (e - self.pre_e) / delta_t # derivative of the tracking error\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\\n\",\n    \"\\n\",\n    \"        # update previous tracking error\\n\",\n    \"        self.pre_e = e\\n\",\n    \"\\n\",\n    \"        return acc_cmd\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Lateral Controller : State Feedback Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class StateFeedbackLateralController():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            f : np.ndarray = np.array([1.0, 1.0]), # feedback gain matrix\\n\",\n    \"            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\\n\",\n    \"    ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize state feedback controller for path-tracking\\\"\\\"\\\"\\n\",\n    \"        # feedback gain matrix\\n\",\n    \"        self.f = f\\n\",\n    \"\\n\",\n    \"        # ref_path info\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"        self.prev_waypoints_idx = 0\\n\",\n    \"\\n\",\n    \"    def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\\n\",\n    \"        \\\"\\\"\\\"calculate control input\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # set vehicle state variables from observation\\n\",\n    \"        x = observed_x[0]\\n\",\n    \"        y = observed_x[1]\\n\",\n    \"        yaw = observed_x[2]\\n\",\n    \"        v = observed_x[3]\\n\",\n    \"\\n\",\n    \"        # get the waypoint closest to current vehicle position\\n\",\n    \"        _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\\n\",\n    \"        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\\n\",\n    \"            print(\\\"[ERROR] Reached the end of the reference path.\\\")\\n\",\n    \"            raise IndexError\\n\",\n    \"\\n\",\n    \"        # which side of the reference path is the car on, the right or the left?\\n\",\n    \"        ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\\n\",\n    \"        x1, y1 = ref_x, ref_y\\n\",\n    \"        x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0 * np.sin(ref_yaw)\\n\",\n    \"        vx, vy = x2 - x1, y2 - y1\\n\",\n    \"        wx, wy =  x - x1,  y - y1\\n\",\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    \"\\n\",\n    \"        # get tracking error\\n\",\n    \"        y_e = np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # lateral error \\n\",\n    \"        theta_e = yaw - ref_yaw # heading error\\n\",\n    \"        theta_e = np.arctan2(np.sin(theta_e), np.cos(theta_e)) # normalize heading error to [-pi, pi]\\n\",\n    \"\\n\",\n    \"        # calculate control input\\n\",\n    \"        steer_cmd = -self.f @ np.array([y_e, theta_e])\\n\",\n    \"\\n\",\n    \"        return steer_cmd[0]\\n\",\n    \"\\n\",\n    \"    def solve_are(self, A, B, Q, R):\\n\",\n    \"        \\\"\\\"\\\"solve algebraic riccati equation with the Arimoto-Potter algorithm\\n\",\n    \"        Ref: https://qiita.com/trgkpc/items/8210927d5b035912a153\\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # define hamiltonian matrix\\n\",\n    \"        H = np.block([[A, -B @ np.linalg.inv(R) @ B.T],\\n\",\n    \"                    [-Q , -A.T]])\\n\",\n    \"\\n\",\n    \"        # solve eigenvalue problem\\n\",\n    \"        eigenvalue, w = np.linalg.eig(H)\\n\",\n    \"\\n\",\n    \"        # define Y and Z, which are used to calculate P\\n\",\n    \"        Y_, Z_ = [], []\\n\",\n    \"        n = len(w[0])//2\\n\",\n    \"\\n\",\n    \"        # sort eigenvalues\\n\",\n    \"        index_array = sorted([i for i in range(2*n)],\\n\",\n    \"            key = lambda x:eigenvalue[x].real)\\n\",\n    \"\\n\",\n    \"        # choose n eigenvalues which have smaller real part\\n\",\n    \"        for i in index_array[:n]:\\n\",\n    \"            Y_.append(w.T[i][:n])\\n\",\n    \"            Z_.append(w.T[i][n:])\\n\",\n    \"        Y = np.array(Y_).T\\n\",\n    \"        Z = np.array(Z_).T\\n\",\n    \"\\n\",\n    \"        # calculate P\\n\",\n    \"        if np.linalg.det(Y) != 0:\\n\",\n    \"            return Z @ np.linalg.inv(Y)\\n\",\n    \"        else:\\n\",\n    \"            print(\\\"Warning: Y is not regular matrix. Result may be wrong!\\\") # TODO : need to consider mathmatical meaning of this case.\\n\",\n    \"            return Z @ np.linalg.pinv(Y)\\n\",\n    \"\\n\",\n    \"    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\\n\",\n    \"        \\\"\\\"\\\"search the closest waypoint to the vehicle on the reference path\\\"\\\"\\\"\\n\",\n    \"        SEARCH_IDX_LEN = 200 # [points] forward search range\\n\",\n    \"        prev_idx = self.prev_waypoints_idx\\n\",\n    \"        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\\n\",\n    \"        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\\n\",\n    \"        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\\n\",\n    \"        min_d = min(d)\\n\",\n    \"        nearest_idx = d.index(min_d) + prev_idx\\n\",\n    \"\\n\",\n    \"        # get reference values of the nearest waypoint\\n\",\n    \"        ref_x = self.ref_path[nearest_idx,0]\\n\",\n    \"        ref_y = self.ref_path[nearest_idx,1]\\n\",\n    \"        ref_yaw = self.ref_path[nearest_idx,2]\\n\",\n    \"        ref_v = self.ref_path[nearest_idx,3]\\n\",\n    \"\\n\",\n    \"        # update nearest waypoint index if necessary\\n\",\n    \"        if update_prev_idx:\\n\",\n    \"            self.prev_waypoints_idx = nearest_idx \\n\",\n    \"\\n\",\n    \"        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Simulation of Path-Tracking\\n\",\n    \"- Longitudinal Control : PID Controller\\n\",\n    \"- Lateral Control : State Feedback Controller\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"delta_t = 0.05 # [sec]\\n\",\n    \"sim_steps = 1000 # [steps]\\n\",\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    \"\\n\",\n    \"# load and visualize reference path\\n\",\n    \"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\\n\",\n    \"plt.title(\\\"Reference Path\\\")\\n\",\n    \"plt.plot(ref_path[:,0], ref_path[:,1])\\n\",\n    \"plt.show()\\n\",\n    \"\\n\",\n    \"# initialize a vehicle as a control target\\n\",\n    \"vehicle = Vehicle(\\n\",\n    \"    max_steer_abs=0.523, # [rad]\\n\",\n    \"    max_accel_abs=2.000, # [m/s^2]\\n\",\n    \"    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"vehicle.reset(\\n\",\n    \"    init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\\n\",\n    \")\\n\",\n    \"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\\n\",\n    \"\\n\",\n    \"# initialize pid controller for acceleration control\\n\",\n    \"pid_lon_controller = PIDLongitudinalController(\\n\",\n    \"    p_gain = 1.2,\\n\",\n    \"    i_gain = 0.4,\\n\",\n    \"    d_gain = 0.1,\\n\",\n    \"    target_velocity = +5.0 # [m/s]\\n\",\n    \")\\n\",\n    \"\\n\",\n    \"# initialize state feedback controller for steering control\\n\",\n    \"state_feedback_lat_controller = StateFeedbackLateralController(\\n\",\n    \"    f = np.array([[2.0, 2.0]]), # feedback gain matrix\\n\",\n    \"    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\\n\",\n    \")\\n\",\n    \"\\\"\\\"\\\"Note : \\n\",\n    \"The feedback gain matrix f can be calculated with pole placement method, for example.\\n\",\n    \"\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_steps):\\n\",\n    \"\\n\",\n    \"    # get current state of vehicle\\n\",\n    \"    current_state = vehicle.get_state()\\n\",\n    \"\\n\",\n    \"    try:\\n\",\n    \"        # calculate control inputs\\n\",\n    \"        current_velocity = current_state[3]\\n\",\n    \"        accel_input = pid_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\\n\",\n    \"        steer_input = state_feedback_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\\n\",\n    \"\\n\",\n    \"    except IndexError as ex:\\n\",\n    \"        # the vehicle has reached the end of the reference path\\n\",\n    \"        print(\\\"[ERROR] IndexError detected. Terminate simulation.\\\")\\n\",\n    \"        break\\n\",\n    \"\\n\",\n    \"    # print current state and input force\\n\",\n    \"    print(steer_input)\\n\",\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    \"\\n\",\n    \"    # update states of vehicle\\n\",\n    \"    print(\\\"steer_input\\\", steer_input)\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation\\n\",\n    \"vehicle.show_animation(interval_ms=int(delta_t * 1000))\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"state_feedback_pathtracking_demo.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \"Python 3 (ipykernel)\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  }\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 4\n}\n"
  },
  {
    "path": "notebooks/unicycle_model.ipynb",
    "content": "{\n \"cells\": [\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"# Unicycle Model\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Definition of Coordinate Systems\\n\",\n    \"\\n\",\n    \"<img src=\\\"../media/def_of_frames.svg\\\" width=\\\"500px\\\" alt=\\\"definition_of_frames\\\">\\n\"\n   ]\n  },\n  {\n   \"attachments\": {},\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## State Equation\\n\",\n    \"\\n\",\n    \"<img src=\\\"../media/UM.svg\\\" width=\\\"500px\\\" alt=\\\"UM\\\">\\n\",\n    \"\\n\",\n    \"$$\\n\",\n    \"\\\\begin{align}\\n\",\n    \"\\\\frac{\\\\mathrm{d}}{\\\\mathrm{d}t}\\n\",\n    \"\\\\begin{bmatrix}\\n\",\n    \"p^G_x \\\\\\\\\\n\",\n    \"p^G_y \\\\\\\\\\n\",\n    \"\\\\phi \\\\\\\\\\n\",\n    \"V\\n\",\n    \"\\\\end{bmatrix}\\n\",\n    \"=\\n\",\n    \"\\\\begin{bmatrix}\\n\",\n    \"V \\\\cos\\\\phi \\\\\\\\\\n\",\n    \"V \\\\sin\\\\phi \\\\\\\\\\n\",\n    \"( V / l ) \\\\tan{\\\\delta} \\\\\\\\\\n\",\n    \"{a}\\n\",\n    \"\\\\end{bmatrix}.\\n\",\n    \"\\\\end{align}\\n\",\n    \"$$\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"import numpy as np\\n\",\n    \"import matplotlib.pyplot as plt\\n\",\n    \"from typing import Tuple\\n\",\n    \"from matplotlib import patches\\n\",\n    \"from matplotlib.animation import ArtistAnimation\\n\",\n    \"from IPython import display\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"class Vehicle():\\n\",\n    \"    def __init__(\\n\",\n    \"            self,\\n\",\n    \"            wheel_base: float = 2.5, # [m]\\n\",\n    \"            max_steer_abs: float = 0.523, # [rad]\\n\",\n    \"            max_accel_abs: float = 2.000, # [m/s^2]\\n\",\n    \"            ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\\n\",\n    \"            delta_t: float = 0.05, # [s]\\n\",\n    \"            visualize: bool = True,\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"initialize vehicle environment\\n\",\n    \"        state variables:\\n\",\n    \"            x: x-axis position in the global frame [m]\\n\",\n    \"            y: y-axis position in the global frame [m]\\n\",\n    \"            yaw: orientation in the global frame [rad]\\n\",\n    \"            v: longitudinal velocity [m/s]\\n\",\n    \"        control input:\\n\",\n    \"            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\\n\",\n    \"            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\\n\",\n    \"        Note: dynamics of the vehicle is the Unicycle Model. \\n\",\n    \"        \\\"\\\"\\\"\\n\",\n    \"        # vehicle parameters\\n\",\n    \"        self.wheel_base = wheel_base#[m]\\n\",\n    \"        self.max_steer_abs = max_steer_abs # [rad]\\n\",\n    \"        self.max_accel_abs = max_accel_abs # [m/s^2]\\n\",\n    \"        self.delta_t = delta_t #[s]\\n\",\n    \"        self.ref_path = ref_path\\n\",\n    \"\\n\",\n    \"        # visualization settings\\n\",\n    \"        self.vehicle_w = 3.00\\n\",\n    \"        self.vehicle_l = 4.00\\n\",\n    \"        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\\n\",\n    \"        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\\n\",\n    \"\\n\",\n    \"        # reset environment\\n\",\n    \"        self.visualize_flag = visualize\\n\",\n    \"        self.reset()\\n\",\n    \"\\n\",\n    \"    def reset(\\n\",\n    \"            self, \\n\",\n    \"            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"reset environment to initial state\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # reset state variables\\n\",\n    \"        self.state = init_state\\n\",\n    \"\\n\",\n    \"        # clear animation frames\\n\",\n    \"        self.frames = []\\n\",\n    \"\\n\",\n    \"        if self.visualize_flag:\\n\",\n    \"            # prepare figure\\n\",\n    \"            self.fig = plt.figure(figsize=(9,9))\\n\",\n    \"            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\\n\",\n    \"            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\\n\",\n    \"            self.steer_ax = plt.subplot2grid((3,4), (1,3))\\n\",\n    \"            self.accel_ax = plt.subplot2grid((3,4), (2,3))\\n\",\n    \"\\n\",\n    \"            # graph layout settings\\n\",\n    \"            ## main view\\n\",\n    \"            self.main_ax.set_aspect('equal')\\n\",\n    \"            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\\n\",\n    \"            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\\n\",\n    \"            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\\n\",\n    \"            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\\n\",\n    \"            ## mini map\\n\",\n    \"            self.minimap_ax.set_aspect('equal')\\n\",\n    \"            self.minimap_ax.axis('off')\\n\",\n    \"            ## steering angle view\\n\",\n    \"            self.steer_ax.set_title(\\\"Steering Angle\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.steer_ax.axis('off')\\n\",\n    \"            ## acceleration view\\n\",\n    \"            self.accel_ax.set_title(\\\"Acceleration\\\", fontsize=\\\"12\\\")\\n\",\n    \"            self.accel_ax.axis('off')\\n\",\n    \"            \\n\",\n    \"            # apply tight layout\\n\",\n    \"            self.fig.tight_layout()\\n\",\n    \"\\n\",\n    \"    def update(\\n\",\n    \"            self, \\n\",\n    \"            u: np.ndarray, \\n\",\n    \"            delta_t: float = 0.0, \\n\",\n    \"            append_frame: bool = True, \\n\",\n    \"            vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\\n\",\n    \"        ) -> None:\\n\",\n    \"        \\\"\\\"\\\"update state variables\\\"\\\"\\\"\\n\",\n    \"        # keep previous states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        # prepare params\\n\",\n    \"        l = self.wheel_base\\n\",\n    \"        dt = self.delta_t if delta_t == 0.0 else delta_t\\n\",\n    \"\\n\",\n    \"        # limit control inputs\\n\",\n    \"        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\\n\",\n    \"        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\\n\",\n    \"\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"        # update state variables\\n\",\n    \"        new_x = x + v * np.cos(yaw) * dt\\n\",\n    \"        new_y = y + v * np.sin(yaw) * dt\\n\",\n    \"        new_yaw = yaw + v / l * np.tan(steer) * dt\\n\",\n    \"        new_v = v + accel * dt\\n\",\n    \"        self.state = np.array([new_x, new_y, new_yaw, new_v])\\n\",\n    \"        \\\"\\\"\\\"< CORE OF VEHICLE DYNAMICS >\\\"\\\"\\\"\\n\",\n    \"\\n\",\n    \"        # record frame\\n\",\n    \"        if append_frame:\\n\",\n    \"            self.append_frame(steer, accel, vehicle_traj)\\n\",\n    \"\\n\",\n    \"    def get_state(self) -> np.ndarray:\\n\",\n    \"        \\\"\\\"\\\"return state variables\\\"\\\"\\\"\\n\",\n    \"        return self.state.copy()\\n\",\n    \"\\n\",\n    \"    def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\\n\",\n    \"        \\\"\\\"\\\"draw a frame of the animation.\\\"\\\"\\\"\\n\",\n    \"        # get current states\\n\",\n    \"        x, y, yaw, v = self.state\\n\",\n    \"\\n\",\n    \"        ### main view ###\\n\",\n    \"        # draw the vehicle shape\\n\",\n    \"        vw, vl = self.vehicle_w, self.vehicle_l\\n\",\n    \"        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\\n\",\n    \"        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\\n\",\n    \"        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\\\\n\",\n    \"            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\\n\",\n    \"        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw wheels\\n\",\n    \"        ww, wl = 0.4, 0.7 #[m]\\n\",\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\",\n    \"        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\\n\",\n    \"\\n\",\n    \"        ## rear-left wheel\\n\",\n    \"        wheel_shape_rl_x, wheel_shape_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_rl_x, wheel_rl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## rear-right wheel\\n\",\n    \"        wheel_shape_rr_x, wheel_shape_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_rr_x, wheel_rr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-left wheel\\n\",\n    \"        wheel_shape_fl_x, wheel_shape_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\\n\",\n    \"        wheel_fl_x, wheel_fl_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        ## front-right wheel\\n\",\n    \"        wheel_shape_fr_x, wheel_shape_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\\n\",\n    \"        wheel_fr_x, wheel_fr_y = \\\\\\n\",\n    \"            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\\n\",\n    \"        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\\n\",\n    \"\\n\",\n    \"        # draw the vehicle center circle\\n\",\n    \"        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\\n\",\n    \"        frame += [self.main_ax.add_artist(vehicle_center)]\\n\",\n    \"\\n\",\n    \"        # draw the reference path\\n\",\n    \"        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\\n\",\n    \"        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\\n\",\n    \"        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\\\"dashed\\\", linewidth=1.5)\\n\",\n    \"\\n\",\n    \"        # draw the information text\\n\",\n    \"        text = \\\"vehicle velocity = {v:>+6.1f} [m/s]\\\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\\n\",\n    \"        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\\n\",\n    \"\\n\",\n    \"        # draw vehicle trajectory\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\\n\",\n    \"            vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\\n\",\n    \"            frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\\\"solid\\\", linewidth=2.0)\\n\",\n    \"\\n\",\n    \"        ### mini map view ###\\n\",\n    \"        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\\n\",\n    \"        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\\\\n\",\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    \"        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\\n\",\n    \"        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\\n\",\n    \"        if vehicle_traj.any():\\n\",\n    \"            frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\\\"solid\\\", linewidth=1.0)\\n\",\n    \"\\n\",\n    \"        ### control input view ###\\n\",\n    \"        # steering angle\\n\",\n    \"        MAX_STEER = self.max_steer_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        s_abs = np.abs(steer)\\n\",\n    \"        if steer < 0.0: # when turning right\\n\",\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\",\n    \"        else: # when turning left\\n\",\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\",\n    \"        frame += steer_pie_obj\\n\",\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    \"\\n\",\n    \"        # acceleration\\n\",\n    \"        MAX_ACCEL = self.max_accel_abs\\n\",\n    \"        PIE_RATE = 3.0/4.0\\n\",\n    \"        PIE_STARTANGLE = 225 # [deg]\\n\",\n    \"        a_abs = np.abs(accel)\\n\",\n    \"        if accel > 0.0:\\n\",\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\",\n    \"        else:\\n\",\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\",\n    \"        frame += accel_pie_obj\\n\",\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    \"\\n\",\n    \"        # append frame\\n\",\n    \"        self.frames.append(frame)\\n\",\n    \"\\n\",\n    \"    # rotate shape and return location on the x-y plane.\\n\",\n    \"    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\\n\",\n    \"        transformed_x = []\\n\",\n    \"        transformed_y = []\\n\",\n    \"        if len(xlist) != len(ylist):\\n\",\n    \"            print(\\\"[ERROR] xlist and ylist must have the same size.\\\")\\n\",\n    \"            raise AttributeError\\n\",\n    \"\\n\",\n    \"        for i, xval in enumerate(xlist):\\n\",\n    \"            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\\n\",\n    \"            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\\n\",\n    \"        transformed_x.append(transformed_x[0])\\n\",\n    \"        transformed_y.append(transformed_y[0])\\n\",\n    \"        return transformed_x, transformed_y\\n\",\n    \"\\n\",\n    \"    def show_animation(self, interval_ms: int) -> None:\\n\",\n    \"        \\\"\\\"\\\"show animation of the recorded frames\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\\n\",\n    \"        html = display.HTML(ani.to_jshtml())\\n\",\n    \"        display.display(html)\\n\",\n    \"        plt.close()\\n\",\n    \"\\n\",\n    \"    def save_animation(self, filename: str, interval: int, movie_writer: str=\\\"ffmpeg\\\") -> None:\\n\",\n    \"        \\\"\\\"\\\"save animation of the recorded frames (ffmpeg required)\\\"\\\"\\\"\\n\",\n    \"        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\\n\",\n    \"        ani.save(filename, writer=movie_writer)\\n\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Run Simulation\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"### Zig Zag Run\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 100 # [step]\\n\",\n    \"delta_t = 0.1 # [s]\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-3.0, 50.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array(vehicle.get_state()[:2])\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"    steer_input = 0.6 * np.sin(i/5.0)  # steering command [rad]\\n\",\n    \"    accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"ucm.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  },\n  {\n   \"cell_type\": \"markdown\",\n   \"metadata\": {},\n   \"source\": [\n    \"## Steady Input\"\n   ]\n  },\n  {\n   \"cell_type\": \"code\",\n   \"execution_count\": null,\n   \"metadata\": {},\n   \"outputs\": [],\n   \"source\": [\n    \"# simulation settings\\n\",\n    \"sim_step = 200 # [step]\\n\",\n    \"delta_t = 0.1 # [s]\\n\",\n    \"\\n\",\n    \"# initialize vehicle simulator\\n\",\n    \"ref_path_x = np.linspace(-25.0, 25.0, 10)\\n\",\n    \"ref_path_y = np.zeros(10)\\n\",\n    \"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\\n\",\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\",\n    \"vehicle_trajectory = np.array(vehicle.get_state()[:2])\\n\",\n    \"\\n\",\n    \"# simulation loop\\n\",\n    \"for i in range(sim_step):\\n\",\n    \"    steer_input = 0.3  # steering command [rad]\\n\",\n    \"    accel_input = 2.0 # acceleration command [m/s^2]\\n\",\n    \"    vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\\n\",\n    \"    vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\\n\",\n    \"\\n\",\n    \"# show animation on jupyter notebook\\n\",\n    \"vehicle.show_animation(interval_ms=delta_t*1000)\\n\",\n    \"\\n\",\n    \"# save animation as a mp4 file if necessary\\n\",\n    \"# vehicle.save_animation(\\\"ucm.mp4\\\", interval=int(delta_t * 1000), movie_writer=\\\"ffmpeg\\\") # ffmpeg is required to write mp4 file\"\n   ]\n  }\n ],\n \"metadata\": {\n  \"kernelspec\": {\n   \"display_name\": \".venv\",\n   \"language\": \"python\",\n   \"name\": \"python3\"\n  },\n  \"language_info\": {\n   \"codemirror_mode\": {\n    \"name\": \"ipython\",\n    \"version\": 3\n   },\n   \"file_extension\": \".py\",\n   \"mimetype\": \"text/x-python\",\n   \"name\": \"python\",\n   \"nbconvert_exporter\": \"python\",\n   \"pygments_lexer\": \"ipython3\",\n   \"version\": \"3.10.5\"\n  },\n  \"orig_nbformat\": 4\n },\n \"nbformat\": 4,\n \"nbformat_minor\": 2\n}\n"
  },
  {
    "path": "pyproject.toml",
    "content": "[tool.poetry]\nname = \"path-tracking\"\nversion = \"0.1.0\"\ndescription = \"\"\nauthors = [\"MizuhoAOKI <mizuhoaoki1998@gmail.com>\"]\nreadme = \"README.md\"\npackages = [{include = \"path_tracking\"}]\n\n[tool.poetry.dependencies]\npython = \">=3.10,<3.13\"\nmatplotlib = \"^3.8.0\"\nnumpy = \"^1.26.0\"\nnotebook = \"^7.0.4\"\nsympy = \"^1.12\"\n\n[build-system]\nrequires = [\"poetry-core\"]\nbuild-backend = \"poetry.core.masonry.api\"\n"
  }
]