Showing preview only (712K chars total). Download the full file or copy to clipboard to get everything.
Repository: MizuhoAOKI/path_tracking_catalog
Branch: master
Commit: 1770cb6c6865
Files: 23
Total size: 691.0 KB
Directory structure:
gitextract_vv2ds5dc/
├── .gitignore
├── .python-version
├── LICENSE.txt
├── README.md
├── media/
│ └── .gitkeep
├── notebooks/
│ ├── bangbang.ipynb
│ ├── dwa_obstacle_avoidance.ipynb
│ ├── dwa_pathtracking.ipynb
│ ├── dynamic_bicycle_model.ipynb
│ ├── fuzzy.ipynb
│ ├── kinematic_bicycle_model.ipynb
│ ├── lqr.ipynb
│ ├── mppi_obstacle_avoidance.ipynb
│ ├── mppi_pathtracking.ipynb
│ ├── ovalpath.csv
│ ├── pid.ipynb
│ ├── preliminary/
│ │ ├── linear_quadratic_regulator_tutorial.ipynb
│ │ └── state_feedback_control_tutorial.ipynb
│ ├── purepursuit.ipynb
│ ├── stanley.ipynb
│ ├── state_feedback.ipynb
│ └── unicycle_model.ipynb
└── pyproject.toml
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
*.mp4
.ipynb_checkpoints
__pycache__/
*.pyc
# Packages
*.egg
!/tests/**/*.egg
/*.egg-info
/dist/*
build
_build
.cache
*.so
# Installer logs
pip-log.txt
# Unit test / coverage reports
.coverage
.pytest_cache
.DS_Store
.idea/*
# .python-version
.vscode/*
/test.py
/test_*.*
/setup.cfg
MANIFEST.in
/setup.py
/docs/site/*
/tests/fixtures/simple_project/setup.py
/tests/fixtures/project_with_extras/setup.py
.mypy_cache
.venv
/releases/*
pip-wheel-metadata
/poetry.toml
poetry/core/*
================================================
FILE: .python-version
================================================
3.10.5
================================================
FILE: LICENSE.txt
================================================
MIT License
Copyright (c) 2024 MizuhoAOKI
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: README.md
================================================
[](https://opensource.org/licenses/MIT)
[](https://python-poetry.org/)
# Path Tracking Catalog
25 path-tracking algorithms are (goint to be) implemented with python.
https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/1b2b39f6-87fc-4f3d-9b18-4ae32f71b5fd
- [Vehicle Models for Simulation](#vehicle-models-for-simulation)
- [x] [Dynamic Bicycle Model](#dynamic-bicycle-model)
- [x] [Kinematic Bicycle Model](#kinematic-bicycle-model)
- [x] [Unicycle Model](#unicycle-model)
- [Control Algorithms](#control-algorithms)
- [x] [Bang-Bang Control](#bang-bang-control)
- [x] [PID Control](#pid-control)
- [x] [Pure Pursuit Control](#pure-pursuit-control)
- [x] [Stanley Control](#stanley-control)
- [x] [Fuzzy Logic Control](#fuzzy-logic-control)
- [ ] Genetic Algorithm
- [x] [Dynamic Window Approach](#dynamic-window-approach)
- [ ] State Lattice Planner
- [x] [State Feedback Control](#state-feedback-control)
- [x] [Linear Quadratic Regulator (infinite horizon)](#linear-quadratic-regulator)
- [ ] Linear Quadratic Regulator (finite horizon)
- [ ] Differential Dynamic Programming (infinite horizon)
- [ ] Differential Dynamic Programming (finite horizon)
- [ ] Iterative LQR (infinite horizon)
- [ ] Iterative LQR (finite horizon)
- [ ] Linear Model Predictive Control (formulated as Quadratic Programming)
- [ ] Nonlinear Model Predictive Control (solved by C/GMRES method)
- [x] [Model Predictive Path-Integral Control](#model-predictive-path-integral-control)
- [ ] Sliding Mode Control
- [ ] Q-Learning
- [ ] Multi Layer Perceptron
- [ ] Linear Quadratic Gaussian
- [ ] H∞ Control (LMI)
- [ ] Lyapunov
- [ ] Adaptive Control
## Setup
```sh
git clone https://github.com/MizuhoAOKI/path_tracking_catalog.git
cd path_tracking_catalog
poetry install
```
## Vehicle Models for Simulation
### Definition of Coordinate Systems
<img src="./media/def_of_frames.svg" width="500px" alt="definition_of_frames">
### Dynamic Bicycle Model
```math
\begin{align}
&\frac{\mathrm{d}}{\mathrm{d}t}
\begin{bmatrix}
p^G_x \\
p^G_y \\
\phi \\
v^B_x \\
v^B_y \\
\omega \\
\end{bmatrix}
=
\begin{bmatrix}
v^B_x \cos\phi - v^B_y \sin\phi \\
v^B_x \sin\phi + v^B_y \cos\phi \\
\omega \\
{a}\cos\beta - (F_{f}^{\rm{lat}}\sin {{\delta}})/m + v^B_y \omega \\
{a}\sin\beta + F_{r}^{\rm{lat}}/m + F_{f}^{\rm{lat}} \cos{\delta}/m - v^B_x \omega \\
(F_{f}^{\rm{lat}}l_f\cos{\delta} - F_{r}^{\rm{lat}}l_r)/I_z\
\end{bmatrix}, \\ \\
& F_{f}^{\rm{lat}} = - C_f \left( \frac{v^B_y + l_f \omega}{v^B_x} - {\delta} \right), \\ \\
& F_{r}^{\rm{lat}} = - C_r \left( \frac{v^B_y - l_r \omega}{v^B_x} \right), \\ \\
& \beta = \tan^{-1} \left( \frac{v^B_y}{v^B_x} \right) \approx \frac{v^B_y}{v^B_x} \ \ \ (\because v^B_y \ll v^B_x ).
\end{align}
```
<p align="center">
<img src="./media/DBM.svg" width="500px" alt="DBM" />
</p>
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/d51c3821-9b35-4e91-8235-e63b18f33f03
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/dynamic_bicycle_model.ipynb
```
### Kinematic Bicycle Model
```math
\begin{align}
& \frac{\mathrm{d}}{\mathrm{d}t}
\begin{bmatrix}
p^G_x \\
p^G_y \\
\phi \\
V
\end{bmatrix}
=
\begin{bmatrix}
V \cos(\phi + \beta) \\
V \sin(\phi + \beta) \\
(V/l_r) \sin\beta \\
{a}
\end{bmatrix},\\ \\
& \beta = \tan^{-1} \left( \frac{l_r}{l_f + l_r} \tan({\delta}) \right).
\end{align}
```
<p align="center">
<img src="./media/KBM.svg" width="500px" alt="KBM" />
</p>
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/b85fe31c-3e4a-47a9-bc54-694cde225bd5
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/kinematic_bicycle_model.ipynb
```
### Unicycle Model
```math
\begin{align}
\frac{\mathrm{d}}{\mathrm{d}t}
\begin{bmatrix}
p^G_x \\
p^G_y \\
\phi \\
V
\end{bmatrix}
=
\begin{bmatrix}
V \cos\phi \\
V \sin\phi \\
( V / l ) \tan{\delta} \\
{a}
\end{bmatrix}.
\end{align}
```
<p align="center">
<img src="./media/UM.svg" width="500px" alt="UM" />
</p>
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/8cba0010-6a21-4830-8974-b4b57c166bcf
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/unicycle_model.ipynb
```
## Control Algorithms
### Bang-Bang Control
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/cc88214e-3914-4126-ac57-3f63d8397094
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/bangbang.ipynb
```
### PID Control
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/83e813d9-b611-49da-abe2-45333bfb80d2
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/pid.ipynb
```
### Pure-Pursuit Control
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/a23f8437-d695-4848-83fb-a8424f311683
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/purepursuit.ipynb
```
### Stanley Control
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/43f3ce4f-8181-45ad-bc08-ac96f3a91e2b
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/stanley.ipynb
```
### Fuzzy Logic Control
https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/d8f9f256-4b2c-4b9e-8b57-e33f05e59e6f
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/fuzzy.ipynb
```
### Dynamic Window Approach
#### Simple Path Tracking
https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/23078e50-46a5-48eb-b89b-4a2111b320f0
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/dwa_pathtracking.ipynb
```
#### Path Tracking with Obstacle Avoidance
https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/7b2a80b7-8d3e-4d37-89f4-23337efcb937
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/dwa_obstacle_avoidance.ipynb
```
### State Lattice Planner
### State Feedback Control
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/8e48380b-f840-49cc-91e0-07dad356b5be
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/state_feedback.ipynb
```
### Linear Quadratic Regulator
https://github.com/MizuhoAOKI/path_tracking/assets/63337525/13d6abbc-2904-422d-acba-17ede074a181
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/lqr.ipynb
```
### Model Predictive Control
### Model Predictive Path-Integral Control
#### Simple Path Tracking
https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/acfefe3a-a22a-4cbc-a5c8-a83086943684
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/mppi_pathtracking.ipynb
```
#### Path Tracking with Obstacle Avoidance
https://github.com/MizuhoAOKI/path_tracking_catalog/assets/63337525/fe84eaf6-8795-490d-9ef1-efecc427052e
```sh
cd path_tracking_catalog
poetry run jupyter notebook notebooks/mppi_obstacle_avoidance.ipynb
```
### Sliding Mode Control
### Q-Learning
================================================
FILE: media/.gitkeep
================================================
================================================
FILE: notebooks/bangbang.ipynb
================================================
{
"cells": [
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"# Bang-Bang Control"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import math\n",
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from typing import Tuple\n",
"from matplotlib import patches\n",
"from matplotlib.animation import ArtistAnimation\n",
"from IPython import display"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Control Target : Vehicle \n",
"- Longitudinal dynamics : Point Mass Model\n",
"- Lateral dynamics : Kinematic Bicycle Model"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class Vehicle():\n",
" def __init__(\n",
" self,\n",
" l_f: float = 1.5, # [m]\n",
" l_r: float = 1.0, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n",
" delta_t: float = 0.05, # [s]\n",
" visualize: bool = True,\n",
" ) -> None:\n",
" \"\"\"initialize vehicle environment\n",
" state variables:\n",
" x: x-axis position in the global frame [m]\n",
" y: y-axis position in the global frame [m]\n",
" yaw: orientation in the global frame [rad]\n",
" v: longitudinal velocity [m/s]\n",
" control input:\n",
" steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
" accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
" Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n",
" \"\"\"\n",
" # vehicle parameters\n",
" self.l_f = l_f # [m]\n",
" self.l_r = l_r # [m]\n",
" self.wheel_base = l_f + l_r # [m]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.delta_t = delta_t #[s]\n",
" self.ref_path = ref_path\n",
"\n",
" # visualization settings\n",
" self.vehicle_w = 3.00\n",
" self.vehicle_l = 4.00\n",
" self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
" self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
"\n",
" # reset environment\n",
" self.visualize_flag = visualize\n",
" self.reset()\n",
"\n",
" def reset(\n",
" self, \n",
" init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n",
" ) -> None:\n",
" \"\"\"reset environment to initial state\"\"\"\n",
"\n",
" # reset state variables\n",
" self.state = init_state\n",
"\n",
" # clear animation frames\n",
" self.frames = []\n",
"\n",
" if self.visualize_flag:\n",
" # prepare figure\n",
" self.fig = plt.figure(figsize=(9,9))\n",
" self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
" self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
" self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
" self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
"\n",
" # graph layout settings\n",
" ## main view\n",
" self.main_ax.set_aspect('equal')\n",
" self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
" self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
" self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
" self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
" ## mini map\n",
" self.minimap_ax.set_aspect('equal')\n",
" self.minimap_ax.axis('off')\n",
" ## steering angle view\n",
" self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
" self.steer_ax.axis('off')\n",
" ## acceleration view\n",
" self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
" self.accel_ax.axis('off')\n",
" \n",
" # apply tight layout\n",
" self.fig.tight_layout()\n",
"\n",
" def update(\n",
" self, \n",
" u: np.ndarray, \n",
" delta_t: float = 0.0, \n",
" append_frame: bool = True, \n",
" vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n",
" ) -> None:\n",
" \"\"\"update state variables\"\"\"\n",
" # keep previous states\n",
" x, y, yaw, v = self.state\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" l_r = self.l_r\n",
" dt = self.delta_t if delta_t == 0.0 else delta_t\n",
"\n",
" # limit control inputs\n",
" steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
" accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
" # update state variables\n",
" beta = np.arctan(l_r / l * np.tan(steer))\n",
" new_x = x + v * np.cos(yaw + beta) * dt\n",
" new_y = y + v * np.sin(yaw + beta) * dt\n",
" new_yaw = yaw + v / l * np.sin(beta) * dt\n",
" new_v = v + accel * dt\n",
" self.state = np.array([new_x, new_y, new_yaw, new_v])\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
"\n",
" # record frame\n",
" if append_frame:\n",
" self.append_frame(steer, accel, vehicle_traj)\n",
"\n",
" def get_state(self) -> np.ndarray:\n",
" \"\"\"return state variables\"\"\"\n",
" return self.state.copy()\n",
"\n",
" def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n",
" \"\"\"draw a frame of the animation.\"\"\"\n",
" # get current states\n",
" x, y, yaw, v = self.state\n",
"\n",
" ### main view ###\n",
" # draw the vehicle shape\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
" frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
"\n",
" # draw wheels\n",
" ww, wl = 0.4, 0.7 #[m]\n",
" wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
" wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
"\n",
" ## rear-left wheel\n",
" wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
" wheel_rl_x, wheel_rl_y = \\\n",
" self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
"\n",
" ## rear-right wheel\n",
" wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
" wheel_rr_x, wheel_rr_y = \\\n",
" self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
"\n",
" ## front-left wheel\n",
" wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
" wheel_fl_x, wheel_fl_y = \\\n",
" self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
"\n",
" ## front-right wheel\n",
" wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
" wheel_fr_x, wheel_fr_y = \\\n",
" self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
"\n",
" # draw the vehicle center circle\n",
" vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n",
" frame += [self.main_ax.add_artist(vehicle_center)]\n",
"\n",
" # draw the reference path\n",
" ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
" ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
" frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
"\n",
" # draw the information text\n",
" text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
" frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
"\n",
" # draw vehicle trajectory\n",
" if vehicle_traj.any():\n",
" vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n",
" vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n",
" frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n",
"\n",
" ### mini map view ###\n",
" frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
" rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
" frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
" frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
" if vehicle_traj.any():\n",
" frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n",
"\n",
" ### control input view ###\n",
" # steering angle\n",
" MAX_STEER = self.max_steer_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" s_abs = np.abs(steer)\n",
" if steer < 0.0: # when turning right\n",
" steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else: # when turning left\n",
" steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += steer_pie_obj\n",
" frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # acceleration\n",
" MAX_ACCEL = self.max_accel_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" a_abs = np.abs(accel)\n",
" if accel > 0.0:\n",
" accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else:\n",
" accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += accel_pie_obj\n",
" frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # append frame\n",
" self.frames.append(frame)\n",
"\n",
" # rotate shape and return location on the x-y plane.\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def show_animation(self, interval_ms: int) -> None:\n",
" \"\"\"show animation of the recorded frames\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
" html = display.HTML(ani.to_jshtml())\n",
" display.display(html)\n",
" plt.close()\n",
"\n",
" def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
" \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
" ani.save(filename, writer=movie_writer)\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"# Controller : Bang-Bang Controller"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Longitudinal Controller"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class BangBangLongitudinalController():\n",
" def __init__(\n",
" self,\n",
" acceleration_cmd: float = +1.0, # [m/s^2]\n",
" deceleration_cmd: float = -1.0, # [m/s-2]\n",
" target_velocity: float = 3.0, # [m/s]\n",
" ) -> None:\n",
" \"\"\"initialize bang-bang controller for keeping constant velocity\"\"\"\n",
" self.acc_cmd = acceleration_cmd\n",
" self.dec_cmd = deceleration_cmd\n",
" self.target_vel = target_velocity\n",
"\n",
" def calc_control_input(self, observed_vel: float) -> None:\n",
" \"\"\"calculate control input\"\"\"\n",
" if observed_vel < self.target_vel:\n",
" return self.acc_cmd\n",
" else:\n",
" return self.dec_cmd"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Simulation of Keeping Speed Control"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"sim_step = 100 # [step]\n",
"delta_t = 0.1 # [s]\n",
"\n",
"# initialize vehicle simulator\n",
"ref_path_x = np.linspace(-3.0, 50.0, 10)\n",
"ref_path_y = np.zeros(10)\n",
"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n",
"vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, v]\n",
"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n",
"\n",
"# initialize speed controller\n",
"speed_controller = BangBangLongitudinalController(\n",
" acceleration_cmd = +1.0, # [m/s^2]\n",
" deceleration_cmd = -1.0, # [m/s]\n",
" target_velocity = 3.0, # [m/s]\n",
")\n",
"\n",
"# simulation loop\n",
"for i in range(sim_step):\n",
"\n",
" # get current state of vehicle\n",
" current_state = vehicle.get_state()\n",
" current_velocity = current_state[3]\n",
"\n",
" # calculate control inputs\n",
" steer_input = 0.0 # steering command [rad] # set zero because this is just a test run of speed controller\n",
" accel_input = speed_controller.calc_control_input(observed_vel=current_velocity) # acceleration command [m/s^2]\n",
" vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation on jupyter notebook\n",
"vehicle.show_animation(interval_ms=delta_t*1000)\n",
"\n",
"# save animation as a mp4 file if necessary\n",
"# vehicle.save_animation(\"bangbang_speed_control_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Lateral Controller"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class BangBangLateralController():\n",
" def __init__(\n",
" self,\n",
" left_steering_cmd: float = 0.5, # [rad]\n",
" right_steering_cmd: float = 0.5, # [rad]\n",
" ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n",
" ) -> None:\n",
" \"\"\"initialize bang-bang controller for path-tracking\"\"\"\n",
" # bang-bang control parameters\n",
" self.left_steering_cmd = left_steering_cmd\n",
" self.right_steering_cmd = right_steering_cmd\n",
"\n",
" # ref_path info\n",
" self.ref_path = ref_path\n",
" self.prev_waypoints_idx = 0\n",
"\n",
" def calc_control_input(self, observed_x: np.ndarray) -> float:\n",
" \"\"\"calculate control input\"\"\"\n",
"\n",
" # set vehicle state variables from observation\n",
" x = observed_x[0]\n",
" y = observed_x[1]\n",
"\n",
" # get the waypoint closest to current vehicle position \n",
" _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\n",
" if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n",
" print(\"[ERROR] Reached the end of the reference path.\")\n",
" raise IndexError\n",
"\n",
" # which side of the reference path is the car on, the right or the left?\n",
" ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n",
" x1, y1 = ref_x, ref_y\n",
" x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0*np.sin(ref_yaw)\n",
" vx, vy = x2 - x1, y2 - y1\n",
" wx, wy = x - x1, y - y1\n",
" s = vx * wy - vy * wx\n",
"\n",
" if s > 0:\n",
" # vehicle is on the left\n",
" return self.right_steering_cmd\n",
" else:\n",
" # vehicle is on the right\n",
" return self.left_steering_cmd\n",
"\n",
" def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n",
" \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n",
"\n",
" SEARCH_IDX_LEN = 200 # [points] forward search range\n",
" prev_idx = self.prev_waypoints_idx\n",
" dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n",
" dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n",
" d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n",
" min_d = min(d)\n",
" nearest_idx = d.index(min_d) + prev_idx\n",
"\n",
" # get reference values of the nearest waypoint\n",
" ref_x = self.ref_path[nearest_idx,0]\n",
" ref_y = self.ref_path[nearest_idx,1]\n",
" ref_yaw = self.ref_path[nearest_idx,2]\n",
" ref_v = self.ref_path[nearest_idx,3]\n",
"\n",
" # update nearest waypoint index if necessary\n",
" if update_prev_idx:\n",
" self.prev_waypoints_idx = nearest_idx \n",
"\n",
" return nearest_idx, ref_x, ref_y, ref_yaw, ref_v"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Simulation of Path-Tracking\n",
"- Longitudinal Control : Bang-Bang Controller\n",
"- Lateral Control : Bang-Bang Controller"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"delta_t = 0.05 # [sec]\n",
"sim_steps = 1000 # [steps]\n",
"print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n",
"\n",
"# load and visualize reference path\n",
"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n",
"plt.title(\"Reference Path\")\n",
"plt.plot(ref_path[:,0], ref_path[:,1])\n",
"plt.show()\n",
"\n",
"# initialize a vehicle as a control target\n",
"vehicle = Vehicle(\n",
" max_steer_abs=0.523, # [rad]\n",
" max_accel_abs=2.000, # [m/s^2]\n",
" ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\n",
")\n",
"vehicle.reset(\n",
" init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n",
")\n",
"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n",
"\n",
"# initialize bang-bang controllers for the vehicle\n",
"bangbang_lon_controller = BangBangLongitudinalController(\n",
" acceleration_cmd = +1.0, # [m/s^2]\n",
" deceleration_cmd = -1.0, # [m/s^2]\n",
" target_velocity = +5.0 # [m/s]\n",
")\n",
"bangbang_lat_controller = BangBangLateralController(\n",
" left_steering_cmd = +0.5, # [rad]\n",
" right_steering_cmd = -0.5, # [rad]\n",
" ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\n",
")\n",
"\n",
"# simulation loop\n",
"for i in range(sim_steps):\n",
"\n",
" # get current state of vehicle\n",
" current_state = vehicle.get_state()\n",
"\n",
" try:\n",
" # calculate control inputs\n",
" current_velocity = current_state[3]\n",
" accel_input = bangbang_lon_controller.calc_control_input(observed_vel=current_velocity)\n",
" steer_input = bangbang_lat_controller.calc_control_input(observed_x=current_state)\n",
"\n",
" except IndexError as ex:\n",
" # the vehicle has reached the end of the reference path\n",
" print(\"[ERROR] IndexError detected. Terminate simulation.\")\n",
" break\n",
"\n",
" # print current state and input force\n",
" print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n",
"\n",
" # update states of vehicle\n",
" vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation\n",
"vehicle.show_animation(interval_ms=int(delta_t * 1000))\n",
"\n",
"# save animation as a mp4 file if necessary\n",
"# vehicle.save_animation(\"bangbang_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.5"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
================================================
FILE: notebooks/dwa_obstacle_avoidance.ipynb
================================================
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Dynamic Window Approach"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import math\n",
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from typing import Tuple\n",
"from matplotlib import patches\n",
"from matplotlib.animation import ArtistAnimation\n",
"from IPython import display"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Control Target : Vehicle\n",
"\n",
"- Longitudinal dynamics : Point Mass Model\n",
"- Lateral dynamics : Kinematic Bicycle Model"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class Vehicle():\n",
" def __init__(\n",
" self,\n",
" wheel_base: float = 2.5, # [m]\n",
" vehicle_width = 3.0, # [m]\n",
" vehicle_length = 4.0, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\n",
" obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n",
" delta_t: float = 0.05, # [s]\n",
" visualize: bool = True,\n",
" ) -> None:\n",
" \"\"\"initialize vehicle environment\n",
" state variables:\n",
" x: x-axis position in the global frame [m]\n",
" y: y-axis position in the global frame [m]\n",
" yaw: orientation in the global frame [rad]\n",
" v: longitudinal velocity [m/s]\n",
" control input:\n",
" steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
" accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
" Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n",
" \"\"\"\n",
" # vehicle parameters\n",
" self.wheel_base = wheel_base#[m]\n",
" self.vehicle_w = vehicle_width\n",
" self.vehicle_l = vehicle_length\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.delta_t = delta_t #[s]\n",
" self.ref_path = ref_path\n",
"\n",
" # obstacle parameters\n",
" self.obstacle_circles = obstacle_circles\n",
"\n",
" # visualization settings\n",
" self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
" self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
" self.minimap_view_x_lim_min, self.minimap_view_x_lim_max = -40.0, 40.0\n",
" self.minimap_view_y_lim_min, self.minimap_view_y_lim_max = -10.0, 40.0\n",
"\n",
" # reset environment\n",
" self.visualize_flag = visualize\n",
" self.reset()\n",
"\n",
" def reset(\n",
" self, \n",
" init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n",
" ) -> None:\n",
" \"\"\"reset environment to initial state\"\"\"\n",
"\n",
" # reset state variables\n",
" self.state = init_state\n",
"\n",
" # clear animation frames\n",
" self.frames = []\n",
"\n",
" if self.visualize_flag:\n",
" # prepare figure\n",
" self.fig = plt.figure(figsize=(9,9))\n",
" self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
" self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
" self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
" self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
"\n",
" # graph layout settings\n",
" ## main view\n",
" self.main_ax.set_aspect('equal')\n",
" self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
" self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
" self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
" self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
" ## mini map\n",
" self.minimap_ax.set_aspect('equal')\n",
" self.minimap_ax.axis('off')\n",
" self.minimap_ax.set_xlim(self.minimap_view_x_lim_min, self.minimap_view_x_lim_max)\n",
" self.minimap_ax.set_ylim(self.minimap_view_y_lim_min, self.minimap_view_y_lim_max)\n",
" ## steering angle view\n",
" self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
" self.steer_ax.axis('off')\n",
" ## acceleration view\n",
" self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
" self.accel_ax.axis('off')\n",
" \n",
" # apply tight layout\n",
" self.fig.tight_layout()\n",
"\n",
" def update(\n",
" self, \n",
" u: np.ndarray, \n",
" delta_t: float = 0.0, \n",
" append_frame: bool = True, \n",
" vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n",
" optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\n",
" sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\n",
" ) -> None:\n",
" \"\"\"update state variables\"\"\"\n",
" # keep previous states\n",
" x, y, yaw, v = self.state\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" dt = self.delta_t if delta_t == 0.0 else delta_t\n",
"\n",
" # limit control inputs\n",
" steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
" accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" # update state variables\n",
" new_x = x + v * np.cos(yaw) * dt\n",
" new_y = y + v * np.sin(yaw) * dt\n",
" new_yaw = yaw + v / l * np.tan(steer) * dt\n",
" new_v = v + accel * dt\n",
" self.state = np.array([new_x, new_y, new_yaw, new_v])\n",
"\n",
" # record frame\n",
" if append_frame:\n",
" self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\n",
"\n",
" def get_state(self) -> np.ndarray:\n",
" \"\"\"return state variables\"\"\"\n",
" return self.state.copy()\n",
"\n",
" def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, optimal_traj: np.ndarray, sampled_traj_list: np.ndarray) -> list:\n",
" \"\"\"draw a frame of the animation.\"\"\"\n",
" # get current states\n",
" x, y, yaw, v = self.state\n",
"\n",
" ### main view ###\n",
" # draw the vehicle shape\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
" frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
"\n",
" # draw wheels\n",
" ww, wl = 0.4, 0.7 #[m]\n",
" wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
" wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
"\n",
" ## rear-left wheel\n",
" wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
" wheel_rl_x, wheel_rl_y = \\\n",
" self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
"\n",
" ## rear-right wheel\n",
" wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
" wheel_rr_x, wheel_rr_y = \\\n",
" self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
"\n",
" ## front-left wheel\n",
" wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
" wheel_fl_x, wheel_fl_y = \\\n",
" self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
"\n",
" ## front-right wheel\n",
" wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
" wheel_fr_x, wheel_fr_y = \\\n",
" self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
"\n",
" # draw the vehicle center circle\n",
" vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\n",
" frame += [self.main_ax.add_artist(vehicle_center)]\n",
"\n",
" # draw the reference path\n",
" ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
" ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
" frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
"\n",
" # draw the information text\n",
" text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
" frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
"\n",
" # draw the predicted optimal trajectory from mppi\n",
" if optimal_traj.any():\n",
" optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\n",
" optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\n",
" frame += self.main_ax.plot(optimal_traj_x_offset, optimal_traj_y_offset, color='#005aff', linestyle=\"solid\", linewidth=1.5, zorder=5)\n",
"\n",
" # draw vehicle trajectory\n",
" if vehicle_traj.any():\n",
" vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n",
" vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n",
" frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n",
"\n",
" # draw the sampled trajectories from mppi\n",
" if sampled_traj_list.any():\n",
" min_alpha_value = 0.25\n",
" max_alpha_value = 0.35\n",
" for idx, sampled_traj in enumerate(sampled_traj_list):\n",
" # draw darker for better samples\n",
" alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\n",
" sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\n",
" sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\n",
" frame += self.main_ax.plot(sampled_traj_x_offset, sampled_traj_y_offset, color='gray', linestyle=\"solid\", linewidth=0.2, zorder=4, alpha=alpha_value)\n",
"\n",
" # draw the circular obstacles in the main view\n",
" for obs in self.obstacle_circles:\n",
" obs_x, obs_y, obs_r = obs\n",
" obs_circle = patches.Circle([obs_x-x, obs_y-y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n",
" frame += [self.main_ax.add_artist(obs_circle)]\n",
"\n",
" ### mini map view ###\n",
" frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
" rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
" frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
" frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
" if vehicle_traj.any():\n",
" frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n",
"\n",
" # draw the circular obstacles in the mini map view\n",
" for obs in self.obstacle_circles:\n",
" obs_x, obs_y, obs_r = obs\n",
" obs_circle = patches.Circle([obs_x, obs_y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n",
" frame += [self.minimap_ax.add_artist(obs_circle)]\n",
"\n",
" ### control input view ###\n",
" # steering angle\n",
" MAX_STEER = self.max_steer_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" s_abs = np.abs(steer)\n",
" if steer < 0.0: # when turning right\n",
" steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else: # when turning left\n",
" steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += steer_pie_obj\n",
" frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # acceleration\n",
" MAX_ACCEL = self.max_accel_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" a_abs = np.abs(accel)\n",
" if accel > 0.0:\n",
" accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else:\n",
" accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += accel_pie_obj\n",
" frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # append frame\n",
" self.frames.append(frame)\n",
"\n",
" # rotate shape and return location on the x-y plane.\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def show_animation(self, interval_ms: int) -> None:\n",
" \"\"\"show animation of the recorded frames\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
" html = display.HTML(ani.to_jshtml())\n",
" display.display(html)\n",
" plt.close()\n",
"\n",
" def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
" \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
" ani.save(filename, writer=movie_writer)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Controller : Dynamic Window Approach"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class DWAControllerForPathTracking():\n",
" def __init__(\n",
" self,\n",
" delta_t: float = 0.05,\n",
" wheel_base: float = 2.5, # [m]\n",
" vehicle_width: float = 3.0, # [m]\n",
" vehicle_length: float = 4.0, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n",
" horizon_step_T: int = 30,\n",
" param_steer_resolution: float = 0.01, # [rad]\n",
" param_accel_resolution: float = 0.025, # [m/s^2]\n",
" param_steer_window_size: float = 0.2, # [rad]\n",
" param_accel_window_size: float = 0.2, # [m/s^2]\n",
" stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" visualize_optimal_traj = True, # if True, optimal trajectory is visualized\n",
" visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\n",
" obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n",
" collision_safety_margin_rate: float = 1.2, # safety margin for collision check\n",
" ) -> None:\n",
" \"\"\"initialize dwa controller for path-tracking\"\"\"\n",
" # dwa parameters\n",
" self.dim_x = 4 # dimension of system state vector\n",
" self.dim_u = 2 # dimension of control input vector\n",
" self.T = horizon_step_T # prediction horizon\n",
" self.param_steer_resolution = param_steer_resolution # [rad]\n",
" self.param_accel_resolution = param_accel_resolution # [m/s^2]\n",
" self.param_steer_window_size = param_steer_window_size # [rad]\n",
" self.param_accel_window_size = param_accel_window_size # [m/s^2]\n",
" self.stage_cost_weight = stage_cost_weight\n",
" self.terminal_cost_weight = terminal_cost_weight\n",
" self.visualize_optimal_traj = visualize_optimal_traj\n",
" self.visualze_sampled_trajs = visualze_sampled_trajs\n",
"\n",
" # vehicle parameters\n",
" self.delta_t = delta_t #[s]\n",
" self.wheel_base = wheel_base #[m]\n",
" self.vehicle_w = vehicle_width #[m]\n",
" self.vehicle_l = vehicle_length #[m]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.ref_path = ref_path\n",
"\n",
" # obstacle parameters\n",
" self.obstacle_circles = obstacle_circles\n",
" self.collision_safety_margin_rate = collision_safety_margin_rate\n",
"\n",
" # dwa variables\n",
" self.u_prev = np.zeros((self.T, self.dim_u))\n",
"\n",
" # ref_path info\n",
" self.prev_waypoints_idx = 0\n",
"\n",
" def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\n",
" \"\"\"calculate optimal control input\"\"\"\n",
" # load privious control input sequence\n",
" u = self.u_prev\n",
"\n",
" # set initial x value from observation\n",
" x0 = observed_x\n",
"\n",
" # get the waypoint closest to current vehicle position \n",
" self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\n",
" if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n",
" print(\"[ERROR] Reached the end of the reference path.\")\n",
" raise IndexError\n",
"\n",
" # clarify exploration range of control input space\n",
" steer_prev = u[0, 0]\n",
" accel_prev = u[0, 1]\n",
" steer_min = np.clip(steer_prev - self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n",
" steer_max = np.clip(steer_prev + self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n",
" accel_min = np.clip(accel_prev - self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n",
" accel_max = np.clip(accel_prev + self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" # sample control input sequence\n",
" dynamic_window_steer = np.linspace(steer_min, steer_max, int((steer_max-steer_min)/self.param_steer_resolution)+1)\n",
" dynamic_window_accel = np.linspace(accel_min, accel_max, int((accel_max-accel_min)/self.param_accel_resolution)+1)\n",
" dynamic_window = np.array(np.meshgrid(dynamic_window_steer, dynamic_window_accel)).T # size is steer_sample_size x accel_sample_size x self.udim(=2)\n",
" sample_num = dynamic_window.shape[0] * dynamic_window.shape[1]\n",
"\n",
" # prepare buffer\n",
" ## sampled control input sequence\n",
" v = np.zeros((sample_num, self.T, self.dim_u)) # control input sequence with noise\n",
" ## cost of each sample\n",
" S = np.zeros((sample_num))\n",
" ## counter of samples\n",
" k = 0\n",
"\n",
" # loop for 0 ~ K-1 samples\n",
" for i, j in np.ndindex(dynamic_window.shape[0], dynamic_window.shape[1]):\n",
"\n",
" # set initial(t=0) state x i.e. observed state of the vehicle\n",
" x = x0\n",
"\n",
" # loop for time step t = 1 ~ T\n",
" for t in range(1, self.T+1):\n",
"\n",
" # get control input, which is constant during the prediction horizon\n",
" v[k, t-1] = dynamic_window[i, j]\n",
"\n",
" # update x\n",
" x = self._F(x, self._g(v[k, t-1]))\n",
"\n",
" # add stage cost\n",
" S[k] += self._c(x)\n",
"\n",
" # add terminal cost\n",
" S[k] += self._phi(x)\n",
"\n",
" # update counter of samples\n",
" k += 1\n",
"\n",
" # get optimal control input\n",
" k_opt = np.argmin(S)\n",
" u = v[k_opt]\n",
"\n",
" # calculate optimal trajectory\n",
" optimal_traj = np.zeros((self.T, self.dim_x))\n",
" if self.visualize_optimal_traj:\n",
" x = x0\n",
" for t in range(self.T):\n",
" x = self._F(x, self._g(u[t-1]))\n",
" optimal_traj[t] = x\n",
"\n",
" # calculate sampled trajectories\n",
" sampled_traj_list = np.zeros((k, self.T, self.dim_x))\n",
" sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\n",
" if self.visualze_sampled_trajs:\n",
" for k in sorted_idx:\n",
" x = x0\n",
" for t in range(self.T):\n",
" x = self._F(x, self._g(v[k, t-1]))\n",
" sampled_traj_list[k, t] = x\n",
"\n",
" # update privious control input sequence (shift 1 step to the left)\n",
" self.u_prev[:-1] = u[1:]\n",
" self.u_prev[-1] = u[-1]\n",
"\n",
" # return optimal control input and input sequence\n",
" return u[0], u, optimal_traj, sampled_traj_list\n",
"\n",
" def _g(self, v: np.ndarray) -> float:\n",
" \"\"\"clamp input\"\"\"\n",
" # limit control inputs\n",
" v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\n",
" v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\n",
" return v\n",
"\n",
" def _c(self, x_t: np.ndarray) -> float:\n",
" \"\"\"calculate stage cost\"\"\"\n",
" # parse x_t\n",
" x, y, yaw, v = x_t\n",
" yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n",
"\n",
" # calculate stage cost\n",
" _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n",
" stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\n",
" self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\n",
"\n",
" # add penalty for collision with obstacles\n",
" stage_cost += self._is_collided(x_t) * 1.0e10\n",
"\n",
" return stage_cost\n",
"\n",
" def _phi(self, x_T: np.ndarray) -> float:\n",
" \"\"\"calculate terminal cost\"\"\"\n",
" # parse x_T\n",
" x, y, yaw, v = x_T\n",
" yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n",
"\n",
" # calculate terminal cost\n",
" _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n",
" terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\n",
" self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\n",
"\n",
" # add penalty for collision with obstacles\n",
" terminal_cost += self._is_collided(x_T) * 1.0e10\n",
"\n",
" return terminal_cost\n",
"\n",
" def _is_collided(self, x_t: np.ndarray) -> bool:\n",
" \"\"\"check if the vehicle is collided with obstacles\"\"\"\n",
" # vehicle shape parameters\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" safety_margin_rate = self.collision_safety_margin_rate\n",
" vw, vl = vw*safety_margin_rate, vl*safety_margin_rate\n",
"\n",
" # get current states\n",
" x, y, yaw, _ = x_t\n",
"\n",
" # key points for collision check\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, 0.0, +0.5*vl, +0.5*vl, +0.5*vl, 0.0, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, +0.5*vw, 0.0, -0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
"\n",
" # check if the key points are inside the obstacles\n",
" for obs in self.obstacle_circles: # for each circular obstacles\n",
" obs_x, obs_y, obs_r = obs # [m] x, y, radius\n",
" for p in range(len(rotated_vehicle_shape_x)):\n",
" if (rotated_vehicle_shape_x[p]-obs_x)**2 + (rotated_vehicle_shape_y[p]-obs_y)**2 < obs_r**2:\n",
" return 1.0 # collided\n",
"\n",
" return 0.0 # not collided\n",
"\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" \"\"\"rotate shape and return location on the x-y plane.\"\"\"\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n",
" \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n",
"\n",
" SEARCH_IDX_LEN = 200 # [points] forward search range\n",
" prev_idx = self.prev_waypoints_idx\n",
" dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n",
" dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n",
" d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n",
" min_d = min(d)\n",
" nearest_idx = d.index(min_d) + prev_idx\n",
"\n",
" # get reference values of the nearest waypoint\n",
" ref_x = self.ref_path[nearest_idx,0]\n",
" ref_y = self.ref_path[nearest_idx,1]\n",
" ref_yaw = self.ref_path[nearest_idx,2]\n",
" ref_v = self.ref_path[nearest_idx,3]\n",
"\n",
" # update nearest waypoint index if necessary\n",
" if update_prev_idx:\n",
" self.prev_waypoints_idx = nearest_idx \n",
"\n",
" return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\n",
"\n",
" def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\n",
" \"\"\"calculate next state of the vehicle\"\"\"\n",
" # get previous state variables\n",
" x, y, yaw, v = x_t\n",
" steer, accel = v_t\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" dt = self.delta_t\n",
"\n",
" # update state variables\n",
" new_x = x + v * np.cos(yaw) * dt\n",
" new_y = y + v * np.sin(yaw) * dt\n",
" new_yaw = yaw + v / l * np.tan(steer) * dt\n",
" new_v = v + accel * dt\n",
"\n",
" # return updated state\n",
" x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\n",
" return x_t_plus_1\n",
"\n",
" def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\n",
" \"\"\"apply moving average filter for smoothing input sequence\n",
" Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\n",
" \"\"\"\n",
" b = np.ones(window_size)/window_size\n",
" dim = xx.shape[1]\n",
" xx_mean = np.zeros(xx.shape)\n",
"\n",
" for d in range(dim):\n",
" xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\"same\")\n",
" n_conv = math.ceil(window_size/2)\n",
" xx_mean[0,d] *= window_size/n_conv\n",
" for i in range(1, n_conv):\n",
" xx_mean[i,d] *= window_size/(i+n_conv)\n",
" xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \n",
" return xx_mean\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Simulation of Obstacle Avoidance\n",
"- Longitudinal Control : Dynamic Window Approach\n",
"- Lateral Control : Dynamic Window Approach"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"delta_t = 0.05 # [sec]\n",
"sim_steps = 150 # [steps]\n",
"print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n",
"\n",
"# obstacle params\n",
"OBSTACLE_CIRCLES = np.array([\n",
" [+ 8.0, +5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n",
" [+18.0, -5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n",
"])\n",
"\n",
"# load and visualize reference path\n",
"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n",
"plt.title(\"Reference Path\")\n",
"plt.plot(ref_path[:,0], ref_path[:,1])\n",
"plt.show()\n",
"\n",
"# initialize a vehicle as a control target\n",
"vehicle = Vehicle(\n",
" wheel_base=2.5,\n",
" max_steer_abs=0.523, # [rad]\n",
" max_accel_abs=2.000, # [m/s^2]\n",
" ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\n",
" obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n",
")\n",
"vehicle.reset(\n",
" init_state = np.array([0.0, 0.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n",
")\n",
"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n",
"\n",
"# initialize a dwa controller for the vehicle\n",
"dwa = DWAControllerForPathTracking(\n",
" delta_t = delta_t*2.0, # [s]\n",
" wheel_base = 2.5, # [m]\n",
" max_steer_abs = 0.523, # [rad]\n",
" max_accel_abs = 2.000, # [m/s^2]\n",
" ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\n",
" horizon_step_T = 20, # [steps]\n",
" param_steer_resolution = 0.02, # [rad]\n",
" param_accel_resolution = 0.025, # [m/s^2]\n",
" param_steer_window_size = 0.5, # [rad]\n",
" param_accel_window_size = 0.5, # [m/s^2]\n",
" stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" visualze_sampled_trajs = True,\n",
" obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n",
" collision_safety_margin_rate = 1.2, # safety margin for collision check\n",
")\n",
"\n",
"# simulation loop\n",
"for i in range(sim_steps):\n",
"\n",
" # get current state of vehicle\n",
" current_state = vehicle.get_state()\n",
"\n",
" try:\n",
" # calculate input force with DWA\n",
" optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = dwa.calc_control_input(\n",
" observed_x = current_state\n",
" )\n",
" except IndexError as e:\n",
" # the vehicle has reached the end of the reference path\n",
" print(\"[ERROR] IndexError detected. Terminate simulation.\")\n",
" break\n",
"\n",
" # print current state and input force\n",
" print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={optimal_input[0]:>+6.2f}[rad], accel={optimal_input[1]:>+6.2f}[m/s]\")\n",
"\n",
" # update states of vehicle\n",
" vehicle.update(u=optimal_input, delta_t=delta_t, vehicle_traj=vehicle_trajectory, optimal_traj=optimal_traj[:, 0:2], sampled_traj_list=sampled_traj_list[:, :, 0:2])\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation\n",
"vehicle.show_animation(interval_ms=int(delta_t * 1000))\n",
"# save animation\n",
"# vehicle.save_animation(\"dwa_obstacle_avoidance_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.5"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
================================================
FILE: notebooks/dwa_pathtracking.ipynb
================================================
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Dynamic Window Approach"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import math\n",
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from typing import Tuple\n",
"from matplotlib import patches\n",
"from matplotlib.animation import ArtistAnimation\n",
"from IPython import display"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Control Target : Vehicle\n",
"\n",
"- Longitudinal dynamics : Point Mass Model\n",
"- Lateral dynamics : Kinematic Bicycle Model"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class Vehicle():\n",
" def __init__(\n",
" self,\n",
" wheel_base: float = 2.5, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\n",
" delta_t: float = 0.05, # [s]\n",
" visualize: bool = True,\n",
" ) -> None:\n",
" \"\"\"initialize vehicle environment\n",
" state variables:\n",
" x: x-axis position in the global frame [m]\n",
" y: y-axis position in the global frame [m]\n",
" yaw: orientation in the global frame [rad]\n",
" v: longitudinal velocity [m/s]\n",
" control input:\n",
" steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
" accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
" Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n",
" \"\"\"\n",
" # vehicle parameters\n",
" self.wheel_base = wheel_base#[m]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.delta_t = delta_t #[s]\n",
" self.ref_path = ref_path\n",
"\n",
" # visualization settings\n",
" self.vehicle_w = 3.00\n",
" self.vehicle_l = 4.00\n",
" self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
" self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
"\n",
" # reset environment\n",
" self.visualize_flag = visualize\n",
" self.reset()\n",
"\n",
" def reset(\n",
" self, \n",
" init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n",
" ) -> None:\n",
" \"\"\"reset environment to initial state\"\"\"\n",
"\n",
" # reset state variables\n",
" self.state = init_state\n",
"\n",
" # clear animation frames\n",
" self.frames = []\n",
"\n",
" if self.visualize_flag:\n",
" # prepare figure\n",
" self.fig = plt.figure(figsize=(9,9))\n",
" self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
" self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
" self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
" self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
"\n",
" # graph layout settings\n",
" ## main view\n",
" self.main_ax.set_aspect('equal')\n",
" self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
" self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
" self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
" self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
" ## mini map\n",
" self.minimap_ax.set_aspect('equal')\n",
" self.minimap_ax.axis('off')\n",
" ## steering angle view\n",
" self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
" self.steer_ax.axis('off')\n",
" ## acceleration view\n",
" self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
" self.accel_ax.axis('off')\n",
" \n",
" # apply tight layout\n",
" self.fig.tight_layout()\n",
"\n",
" def update(\n",
" self, \n",
" u: np.ndarray, \n",
" delta_t: float = 0.0, \n",
" append_frame: bool = True, \n",
" vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n",
" optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from dwa\n",
" sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from dwa\n",
" ) -> None:\n",
" \"\"\"update state variables\"\"\"\n",
" # keep previous states\n",
" x, y, yaw, v = self.state\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" dt = self.delta_t if delta_t == 0.0 else delta_t\n",
"\n",
" # limit control inputs\n",
" steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
" accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" # update state variables\n",
" new_x = x + v * np.cos(yaw) * dt\n",
" new_y = y + v * np.sin(yaw) * dt\n",
" new_yaw = yaw + v / l * np.tan(steer) * dt\n",
" new_v = v + accel * dt\n",
" self.state = np.array([new_x, new_y, new_yaw, new_v])\n",
"\n",
" # record frame\n",
" if append_frame:\n",
" self.append_frame(steer, accel, vehicle_traj, optimal_traj, sampled_traj_list)\n",
"\n",
" def get_state(self) -> np.ndarray:\n",
" \"\"\"return state variables\"\"\"\n",
" return self.state.copy()\n",
"\n",
" def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray, optimal_traj: np.ndarray, sampled_traj_list: np.ndarray) -> list:\n",
" \"\"\"draw a frame of the animation.\"\"\"\n",
" # get current states\n",
" x, y, yaw, v = self.state\n",
"\n",
" ### main view ###\n",
" # draw the vehicle shape\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
" frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
"\n",
" # draw wheels\n",
" ww, wl = 0.4, 0.7 #[m]\n",
" wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
" wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
"\n",
" ## rear-left wheel\n",
" wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
" wheel_rl_x, wheel_rl_y = \\\n",
" self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
"\n",
" ## rear-right wheel\n",
" wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
" wheel_rr_x, wheel_rr_y = \\\n",
" self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
"\n",
" ## front-left wheel\n",
" wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
" wheel_fl_x, wheel_fl_y = \\\n",
" self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
"\n",
" ## front-right wheel\n",
" wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
" wheel_fr_x, wheel_fr_y = \\\n",
" self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
"\n",
" # draw the vehicle center circle\n",
" vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\n",
" frame += [self.main_ax.add_artist(vehicle_center)]\n",
"\n",
" # draw the reference path\n",
" ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
" ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
" frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
"\n",
" # draw the information text\n",
" text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
" frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
"\n",
" # draw the predicted optimal trajectory from dwa\n",
" if optimal_traj.any():\n",
" optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\n",
" optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\n",
" frame += self.main_ax.plot(optimal_traj_x_offset, optimal_traj_y_offset, color='#005aff', linestyle=\"solid\", linewidth=1.5, zorder=5)\n",
"\n",
" # draw vehicle trajectory\n",
" if vehicle_traj.any():\n",
" vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n",
" vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n",
" frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n",
"\n",
" # draw the sampled trajectories from dwa\n",
" if sampled_traj_list.any():\n",
" min_alpha_value = 0.25\n",
" max_alpha_value = 0.35\n",
" for idx, sampled_traj in enumerate(sampled_traj_list):\n",
" # draw darker for better samples\n",
" alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\n",
" sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\n",
" sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\n",
" frame += self.main_ax.plot(sampled_traj_x_offset, sampled_traj_y_offset, color='gray', linestyle=\"solid\", linewidth=0.2, zorder=4, alpha=alpha_value)\n",
"\n",
" ### mini map view ###\n",
" frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
" rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
" frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
" frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
" if vehicle_traj.any():\n",
" frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n",
"\n",
" ### control input view ###\n",
" # steering angle\n",
" MAX_STEER = self.max_steer_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" s_abs = np.abs(steer)\n",
" if steer < 0.0: # when turning right\n",
" steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else: # when turning left\n",
" steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += steer_pie_obj\n",
" frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # acceleration\n",
" MAX_ACCEL = self.max_accel_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" a_abs = np.abs(accel)\n",
" if accel > 0.0:\n",
" accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else:\n",
" accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += accel_pie_obj\n",
" frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # append frame\n",
" self.frames.append(frame)\n",
"\n",
" # rotate shape and return location on the x-y plane.\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def show_animation(self, interval_ms: int) -> None:\n",
" \"\"\"show animation of the recorded frames\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
" html = display.HTML(ani.to_jshtml())\n",
" display.display(html)\n",
" plt.close()\n",
"\n",
" def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
" \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
" ani.save(filename, writer=movie_writer)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Controller : Dynamic Window Approach"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class DWAControllerForPathTracking():\n",
" def __init__(\n",
" self,\n",
" delta_t: float = 0.05,\n",
" wheel_base: float = 2.5, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n",
" horizon_step_T: int = 30,\n",
" param_steer_resolution: float = 0.01, # [rad]\n",
" param_accel_resolution: float = 0.025, # [m/s^2]\n",
" param_steer_window_size: float = 0.2, # [rad]\n",
" param_accel_window_size: float = 0.2, # [m/s^2]\n",
" stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" visualize_optimal_traj = True, # if True, optimal trajectory is visualized\n",
" visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\n",
" ) -> None:\n",
" \"\"\"initialize dwa controller for path-tracking\"\"\"\n",
" # dwa parameters\n",
" self.dim_x = 4 # dimension of system state vector\n",
" self.dim_u = 2 # dimension of control input vector\n",
" self.T = horizon_step_T # prediction horizon\n",
" self.param_steer_resolution = param_steer_resolution # [rad]\n",
" self.param_accel_resolution = param_accel_resolution # [m/s^2]\n",
" self.param_steer_window_size = param_steer_window_size # [rad]\n",
" self.param_accel_window_size = param_accel_window_size # [m/s^2]\n",
" self.stage_cost_weight = stage_cost_weight\n",
" self.terminal_cost_weight = terminal_cost_weight\n",
" self.visualize_optimal_traj = visualize_optimal_traj\n",
" self.visualze_sampled_trajs = visualze_sampled_trajs\n",
"\n",
" # vehicle parameters\n",
" self.delta_t = delta_t #[s]\n",
" self.wheel_base = wheel_base#[m]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.ref_path = ref_path\n",
"\n",
" # dwa variables\n",
" self.u_prev = np.zeros((self.T, self.dim_u))\n",
"\n",
" # ref_path info\n",
" self.prev_waypoints_idx = 0\n",
"\n",
" def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\n",
" \"\"\"calculate optimal control input\"\"\"\n",
" # load privious control input sequence\n",
" u = self.u_prev\n",
"\n",
" # set initial x value from observation\n",
" x0 = observed_x\n",
"\n",
" # get the waypoint closest to current vehicle position \n",
" self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\n",
" if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n",
" print(\"[ERROR] Reached the end of the reference path.\")\n",
" raise IndexError\n",
"\n",
" # clarify exploration range of control input space\n",
" steer_prev = u[0, 0]\n",
" accel_prev = u[0, 1]\n",
" steer_min = np.clip(steer_prev - self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n",
" steer_max = np.clip(steer_prev + self.param_steer_window_size / 2.0, -self.max_steer_abs, self.max_steer_abs)\n",
" accel_min = np.clip(accel_prev - self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n",
" accel_max = np.clip(accel_prev + self.param_accel_window_size / 2.0, -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" # sample control input sequence\n",
" dynamic_window_steer = np.linspace(steer_min, steer_max, int((steer_max-steer_min)/self.param_steer_resolution)+1)\n",
" dynamic_window_accel = np.linspace(accel_min, accel_max, int((accel_max-accel_min)/self.param_accel_resolution)+1)\n",
" dynamic_window = np.array(np.meshgrid(dynamic_window_steer, dynamic_window_accel)).T # size is steer_sample_size x accel_sample_size x self.udim(=2)\n",
" sample_num = dynamic_window.shape[0] * dynamic_window.shape[1]\n",
"\n",
" # prepare buffer\n",
" ## sampled control input sequence\n",
" v = np.zeros((sample_num, self.T, self.dim_u)) # control input sequence with noise\n",
" ## cost of each sample\n",
" S = np.zeros((sample_num))\n",
" ## counter of samples\n",
" k = 0\n",
"\n",
" # loop for 0 ~ K-1 samples\n",
" for i, j in np.ndindex(dynamic_window.shape[0], dynamic_window.shape[1]):\n",
"\n",
" # set initial(t=0) state x i.e. observed state of the vehicle\n",
" x = x0\n",
"\n",
" # loop for time step t = 1 ~ T\n",
" for t in range(1, self.T+1):\n",
"\n",
" # get control input, which is constant during the prediction horizon\n",
" v[k, t-1] = dynamic_window[i, j]\n",
"\n",
" # update x\n",
" x = self._F(x, self._g(v[k, t-1]))\n",
"\n",
" # add stage cost\n",
" S[k] += self._c(x)\n",
"\n",
" # add terminal cost\n",
" S[k] += self._phi(x)\n",
"\n",
" # update counter of samples\n",
" k += 1\n",
"\n",
" # get optimal control input\n",
" k_opt = np.argmin(S)\n",
" u = v[k_opt]\n",
"\n",
" # calculate optimal trajectory\n",
" optimal_traj = np.zeros((self.T, self.dim_x))\n",
" if self.visualize_optimal_traj:\n",
" x = x0\n",
" for t in range(self.T):\n",
" x = self._F(x, self._g(u[t-1]))\n",
" optimal_traj[t] = x\n",
"\n",
" # calculate sampled trajectories\n",
" sampled_traj_list = np.zeros((k, self.T, self.dim_x))\n",
" sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\n",
" if self.visualze_sampled_trajs:\n",
" for k in sorted_idx:\n",
" x = x0\n",
" for t in range(self.T):\n",
" x = self._F(x, self._g(v[k, t-1]))\n",
" sampled_traj_list[k, t] = x\n",
"\n",
" # update privious control input sequence (shift 1 step to the left)\n",
" self.u_prev[:-1] = u[1:]\n",
" self.u_prev[-1] = u[-1]\n",
"\n",
" # return optimal control input and input sequence\n",
" return u[0], u, optimal_traj, sampled_traj_list\n",
"\n",
" def _g(self, v: np.ndarray) -> float:\n",
" \"\"\"clamp input\"\"\"\n",
" # limit control inputs\n",
" v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\n",
" v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\n",
" return v\n",
"\n",
" def _c(self, x_t: np.ndarray) -> float:\n",
" \"\"\"calculate stage cost\"\"\"\n",
" # parse x_t\n",
" x, y, yaw, v = x_t\n",
" yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n",
"\n",
" # calculate stage cost\n",
" _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n",
" stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\n",
" self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\n",
" return stage_cost\n",
"\n",
" def _phi(self, x_T: np.ndarray) -> float:\n",
" \"\"\"calculate terminal cost\"\"\"\n",
" # parse x_T\n",
" x, y, yaw, v = x_T\n",
" yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n",
"\n",
" # calculate terminal cost\n",
" _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n",
" terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\n",
" self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\n",
" return terminal_cost\n",
"\n",
" def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n",
" \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n",
"\n",
" SEARCH_IDX_LEN = 200 # [points] forward search range\n",
" prev_idx = self.prev_waypoints_idx\n",
" dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n",
" dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n",
" d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n",
" min_d = min(d)\n",
" nearest_idx = d.index(min_d) + prev_idx\n",
"\n",
" # get reference values of the nearest waypoint\n",
" ref_x = self.ref_path[nearest_idx,0]\n",
" ref_y = self.ref_path[nearest_idx,1]\n",
" ref_yaw = self.ref_path[nearest_idx,2]\n",
" ref_v = self.ref_path[nearest_idx,3]\n",
"\n",
" # update nearest waypoint index if necessary\n",
" if update_prev_idx:\n",
" self.prev_waypoints_idx = nearest_idx \n",
"\n",
" return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\n",
"\n",
" def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\n",
" \"\"\"calculate next state of the vehicle\"\"\"\n",
" # get previous state variables\n",
" x, y, yaw, v = x_t\n",
" steer, accel = v_t\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" dt = self.delta_t\n",
"\n",
" # update state variables\n",
" new_x = x + v * np.cos(yaw) * dt\n",
" new_y = y + v * np.sin(yaw) * dt\n",
" new_yaw = yaw + v / l * np.tan(steer) * dt\n",
" new_v = v + accel * dt\n",
"\n",
" # return updated state\n",
" x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\n",
" return x_t_plus_1\n",
"\n",
" def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\n",
" \"\"\"apply moving average filter for smoothing input sequence\n",
" Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\n",
" \"\"\"\n",
" b = np.ones(window_size)/window_size\n",
" dim = xx.shape[1]\n",
" xx_mean = np.zeros(xx.shape)\n",
"\n",
" for d in range(dim):\n",
" xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\"same\")\n",
" n_conv = math.ceil(window_size/2)\n",
" xx_mean[0,d] *= window_size/n_conv\n",
" for i in range(1, n_conv):\n",
" xx_mean[i,d] *= window_size/(i+n_conv)\n",
" xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \n",
" return xx_mean\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Simulation of Path-Tracking\n",
"- Longitudinal Control : Dynamic Window Approach\n",
"- Lateral Control : Dynamic Window Approach"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"delta_t = 0.05 # [sec]\n",
"sim_steps = 1000 # [steps]\n",
"print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n",
"\n",
"# load and visualize reference path\n",
"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n",
"plt.title(\"Reference Path\")\n",
"plt.plot(ref_path[:,0], ref_path[:,1])\n",
"plt.show()\n",
"\n",
"# initialize a vehicle as a control target\n",
"vehicle = Vehicle(\n",
" wheel_base=2.5,\n",
" max_steer_abs=0.523, # [rad]\n",
" max_accel_abs=2.000, # [m/s^2]\n",
" ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\n",
")\n",
"vehicle.reset(\n",
" init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n",
")\n",
"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n",
"\n",
"# initialize a dwa controller for the vehicle\n",
"dwa = DWAControllerForPathTracking(\n",
" delta_t = delta_t*2.0, # [s]\n",
" wheel_base = 2.5, # [m]\n",
" max_steer_abs = 0.523, # [rad]\n",
" max_accel_abs = 2.000, # [m/s^2]\n",
" ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\n",
" horizon_step_T = 20, # [steps]\n",
" param_steer_resolution = 0.01, # [rad]\n",
" param_accel_resolution = 0.025, # [m/s^2]\n",
" param_steer_window_size = 0.2, # [rad]\n",
" param_accel_window_size = 0.2, # [m/s^2]\n",
" stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
" visualze_sampled_trajs = True\n",
")\n",
"\n",
"# simulation loop\n",
"for i in range(sim_steps):\n",
"\n",
" # get current state of vehicle\n",
" current_state = vehicle.get_state()\n",
"\n",
" try:\n",
" # calculate input force with DWA\n",
" optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = dwa.calc_control_input(\n",
" observed_x = current_state\n",
" )\n",
" except IndexError as e:\n",
" # the vehicle has reached the end of the reference path\n",
" print(\"[ERROR] IndexError detected. Terminate simulation.\")\n",
" break\n",
"\n",
" # print current state and input force\n",
" print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={optimal_input[0]:>+6.2f}[rad], accel={optimal_input[1]:>+6.2f}[m/s]\")\n",
"\n",
" # update states of vehicle\n",
" vehicle.update(u=optimal_input, delta_t=delta_t, vehicle_traj=vehicle_trajectory, optimal_traj=optimal_traj[:, 0:2], sampled_traj_list=sampled_traj_list[:, :, 0:2])\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation\n",
"vehicle.show_animation(interval_ms=int(delta_t * 1000))\n",
"# save animation\n",
"# vehicle.save_animation(\"dwa_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.5"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
================================================
FILE: notebooks/dynamic_bicycle_model.ipynb
================================================
{
"cells": [
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"# Dynamic Bicycle Model"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Definition of Coordinate Systems\n",
"\n",
"<img src=\"../media/def_of_frames.svg\" width=\"500px\" alt=\"definition_of_frames\">\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## State Equation\n",
"\n",
"<img src=\"../media/DBM.svg\" width=\"500px\" alt=\"DBM\">\n",
"\n",
"$$\n",
"\\begin{align}\n",
"&\\frac{\\mathrm{d}}{\\mathrm{d}t}\n",
"\\begin{bmatrix}\n",
"p^G_x \\\\\n",
"p^G_y \\\\\n",
"\\phi \\\\\n",
"v^B_x \\\\\n",
"v^B_y \\\\\n",
"\\omega \\\\\n",
"\\end{bmatrix}\n",
"=\n",
"\\begin{bmatrix}\n",
"v^B_x \\cos\\phi - v^B_y \\sin\\phi \\\\\n",
"v^B_x \\sin\\phi + v^B_y \\cos\\phi \\\\\n",
"\\omega \\\\\n",
"{a}\\cos\\beta - (F_{f}^{\\rm{lat}}\\sin {{\\delta}})/m + v^B_y \\omega \\\\\n",
"{a}\\sin\\beta + F_{r}^{\\rm{lat}}/m + F_{f}^{\\rm{lat}} \\cos{\\delta}/m - v^B_x \\omega \\\\\n",
"(F_{f}^{\\rm{lat}}l_f\\cos{\\delta} - F_{r}^{\\rm{lat}}l_r)/I_z\\\n",
"\\end{bmatrix}, \\\\ \\\\\n",
"& F_{f}^{\\rm{lat}} = - C_f \\left( \\frac{v^B_y + l_f \\omega}{v^B_x} - {\\delta} \\right), \\\\ \\\\\n",
"& F_{r}^{\\rm{lat}} = - C_r \\left( \\frac{v^B_y - l_r \\omega}{v^B_x} \\right), \\\\ \\\\\n",
"& \\beta = \\tan^{-1} \\left( \\frac{v^B_y}{v^B_x} \\right) \\approx \\frac{v^B_y}{v^B_x} \\ \\ \\ (\\because v^B_y \\ll v^B_x ).\n",
"\\end{align}\n",
"$$\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from typing import Tuple\n",
"from matplotlib import patches\n",
"from matplotlib.animation import ArtistAnimation\n",
"from IPython import display"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class Vehicle():\n",
" def __init__(\n",
" self,\n",
" l_f: float = 1.1, # [m]\n",
" l_r: float = 1.4, # [m]\n",
" mass: float = 1000.0, # [kg]\n",
" I_z: float = 1300.0, # [kg*m^2]\n",
" C_f: float = 5000.0 * 2.0, # [N/rad]\n",
" C_r: float = 6000.0 * 2.0, # [N/rad]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n",
" delta_t: float = 0.05, # [s]\n",
" visualize: bool = True,\n",
" ) -> None:\n",
" \"\"\"initialize vehicle environment\n",
" state variables:\n",
" x: x-axis position in the global frame [m]\n",
" y: y-axis position in the global frame [m]\n",
" yaw: orientation in the global frame [rad]\n",
" vx: x-axis velocity [m/s]\n",
" vy: y-axis velocity [m/s]\n",
" omega: angular velocity [rad/s]\n",
" control input:\n",
" steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
" accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
" Note: dynamics of the vehicle is the Dynamic Bicycle Model. \n",
" \"\"\"\n",
" # vehicle parameters\n",
" self.l_f = l_f # [m]\n",
" self.l_r = l_r # [m]\n",
" self.wheel_base = l_f + l_r # [m]\n",
" self.mass = mass # [kg]\n",
" self.I_z = I_z # [kg*m^2]\n",
" self.C_f = C_f # [N/rad]\n",
" self.C_r = C_r # [N/rad]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.delta_t = delta_t #[s]\n",
" self.ref_path = ref_path\n",
"\n",
" # visualization settings\n",
" self.vehicle_w = 3.00\n",
" self.vehicle_l = 4.00\n",
" self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
" self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
"\n",
" # reset environment\n",
" self.visualize_flag = visualize\n",
" self.reset()\n",
"\n",
" def reset(\n",
" self, \n",
" init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]), # [x, y, yaw, vx, vy, omega]\n",
" ) -> None:\n",
" \"\"\"reset environment to initial state\"\"\"\n",
"\n",
" # reset state variables\n",
" self.state = init_state\n",
"\n",
" # clear animation frames\n",
" self.frames = []\n",
"\n",
" if self.visualize_flag:\n",
" # prepare figure\n",
" self.fig = plt.figure(figsize=(9,9))\n",
" self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
" self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
" self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
" self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
"\n",
" # graph layout settings\n",
" ## main view\n",
" self.main_ax.set_aspect('equal')\n",
" self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
" self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
" self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
" self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
" ## mini map\n",
" self.minimap_ax.set_aspect('equal')\n",
" self.minimap_ax.axis('off')\n",
" ## steering angle view\n",
" self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
" self.steer_ax.axis('off')\n",
" ## acceleration view\n",
" self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
" self.accel_ax.axis('off')\n",
" \n",
" # apply tight layout\n",
" self.fig.tight_layout()\n",
"\n",
" def update(\n",
" self, \n",
" u: np.ndarray, \n",
" delta_t: float = 0.0, \n",
" append_frame: bool = True, \n",
" vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n",
" ) -> None:\n",
" \"\"\"update state variables\"\"\"\n",
" # keep previous states\n",
" x, y, yaw, vx, vy, omega = self.state\n",
"\n",
" # prepare params\n",
" l_f = self.l_f\n",
" l_r = self.l_r\n",
" m = self.mass\n",
" C_f = self.C_f\n",
" C_r = self.C_r\n",
" I_z = self.I_z\n",
" dt = self.delta_t if delta_t == 0.0 else delta_t\n",
"\n",
" # limit control inputs\n",
" steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
" accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
" # calculate tire forces\n",
" F_fy = - C_f * ((vy + l_f * omega) / vx - steer)\n",
" F_ry = - C_r * ((vy - l_r * omega) / vx)\n",
"\n",
" # update state variables\n",
" beta = vy / vx\n",
" new_x = x + (vx * np.cos(yaw) - vy * np.sin(yaw)) * dt\n",
" new_y = y + (vx * np.sin(yaw) + vy * np.cos(yaw)) * dt\n",
" new_yaw = yaw + omega * dt\n",
" new_vx = vx + (accel * np.cos(beta) - (F_fy * np.sin(steer) / m) + vy * omega) * dt\n",
" new_vy = vy + (accel * np.sin(beta) + F_ry / m + (F_fy * np.cos(steer) / m) - vx * omega) * dt\n",
" new_omega = omega + ((F_fy * l_f * np.cos(steer) - F_ry * l_r) / I_z) * dt\n",
" self.state = np.array([new_x, new_y, new_yaw, new_vx, new_vy, new_omega])\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
"\n",
" # record frame\n",
" if append_frame:\n",
" self.append_frame(steer, accel, vehicle_traj)\n",
"\n",
" def get_state(self) -> np.ndarray:\n",
" \"\"\"return state variables\"\"\"\n",
" return self.state.copy()\n",
"\n",
" def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n",
" \"\"\"draw a frame of the animation.\"\"\"\n",
" # get current states\n",
" x, y, yaw, vx, vy, omega = self.state\n",
" v = np.sqrt(vx**2 + vy**2) # vehicle velocity\n",
"\n",
" ### main view ###\n",
" # draw the vehicle shape\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
" frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
"\n",
" # draw wheels\n",
" ww, wl = 0.4, 0.7 #[m]\n",
" wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
" wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
"\n",
" ## rear-left wheel\n",
" wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
" wheel_rl_x, wheel_rl_y = \\\n",
" self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
"\n",
" ## rear-right wheel\n",
" wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
" wheel_rr_x, wheel_rr_y = \\\n",
" self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
"\n",
" ## front-left wheel\n",
" wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
" wheel_fl_x, wheel_fl_y = \\\n",
" self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
"\n",
" ## front-right wheel\n",
" wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
" wheel_fr_x, wheel_fr_y = \\\n",
" self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
"\n",
" # draw the vehicle center circle\n",
" vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n",
" frame += [self.main_ax.add_artist(vehicle_center)]\n",
"\n",
" # draw the reference path\n",
" ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
" ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
" frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
"\n",
" # draw the information text\n",
" text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
" frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
"\n",
" # draw vehicle trajectory\n",
" if vehicle_traj.any():\n",
" vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n",
" vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n",
" frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n",
"\n",
" ### mini map view ###\n",
" frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
" rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
" frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
" frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
" if vehicle_traj.any():\n",
" frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n",
"\n",
" ### control input view ###\n",
" # steering angle\n",
" MAX_STEER = self.max_steer_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" s_abs = np.abs(steer)\n",
" if steer < 0.0: # when turning right\n",
" steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else: # when turning left\n",
" steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += steer_pie_obj\n",
" frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # acceleration\n",
" MAX_ACCEL = self.max_accel_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" a_abs = np.abs(accel)\n",
" if accel > 0.0:\n",
" accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else:\n",
" accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += accel_pie_obj\n",
" frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # append frame\n",
" self.frames.append(frame)\n",
"\n",
" # rotate shape and return location on the x-y plane.\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def show_animation(self, interval_ms: int) -> None:\n",
" \"\"\"show animation of the recorded frames\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
" html = display.HTML(ani.to_jshtml())\n",
" display.display(html)\n",
" plt.close()\n",
"\n",
" def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
" \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
" ani.save(filename, writer=movie_writer)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Run Simulation"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Zig Zag Run"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"sim_step = 200 # [step]\n",
"delta_t = 0.05 # [s] # Note : shorter delta_t is recommended for dynamic bicycle model\n",
"\n",
"# initialize vehicle simulator\n",
"ref_path_x = np.linspace(-3.0, 50.0, 10)\n",
"ref_path_y = np.zeros(10)\n",
"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n",
"vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, vx, vy, omega]\n",
"vehicle_trajectory = np.array(vehicle.get_state()[:2])\n",
"\n",
"# simulation loop\n",
"for i in range(sim_step):\n",
" steer_input = 0.6 * np.sin(i/5.0) # steering command [rad]\n",
" accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\n",
" vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation on jupyter notebook\n",
"vehicle.show_animation(interval_ms=delta_t*1000)\n",
"\n",
"# save animation as a mp4 file if necessary\n",
"# vehicle.save_animation(\"dbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Steady Input"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"sim_step = 400 # [step]\n",
"delta_t = 0.05 # [s] # Note : shorter delta_t is recommended for dynamic bicycle model\n",
"\n",
"# initialize vehicle simulator\n",
"ref_path_x = np.linspace(-25.0, 25.0, 10)\n",
"ref_path_y = np.zeros(10)\n",
"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n",
"vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 5.5, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, vx, vy, omega]\n",
"vehicle_trajectory = np.array(vehicle.get_state()[:2])\n",
"\n",
"# simulation loop\n",
"for i in range(sim_step):\n",
" steer_input = 0.3 # steering command [rad]\n",
" accel_input = 2.0 # acceleration command [m/s^2]\n",
" vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation on jupyter notebook\n",
"vehicle.show_animation(interval_ms=delta_t*1000)\n",
"\n",
"# save animation as a mp4 file if necessary\n",
"# vehicle.save_animation(\"dbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
}
],
"metadata": {
"kernelspec": {
"display_name": ".venv",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.0"
},
"orig_nbformat": 4
},
"nbformat": 4,
"nbformat_minor": 2
}
================================================
FILE: notebooks/fuzzy.ipynb
================================================
{
"cells": [
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"# Fuzzy Control"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from typing import Tuple\n",
"from matplotlib import patches\n",
"from matplotlib.animation import ArtistAnimation\n",
"from IPython import display"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Control Target : Vehicle \n",
"- Longitudinal dynamics : Point Mass Model\n",
"- Lateral dynamics : Kinematic Bicycle Model"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class Vehicle():\n",
" def __init__(\n",
" self,\n",
" l_f: float = 1.5, # [m]\n",
" l_r: float = 1.0, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n",
" delta_t: float = 0.05, # [s]\n",
" visualize: bool = True,\n",
" ) -> None:\n",
" \"\"\"initialize vehicle environment\n",
" state variables:\n",
" x: x-axis position in the global frame [m]\n",
" y: y-axis position in the global frame [m]\n",
" yaw: orientation in the global frame [rad]\n",
" v: longitudinal velocity [m/s]\n",
" control input:\n",
" steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
" accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
" Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n",
" \"\"\"\n",
" # vehicle parameters\n",
" self.l_f = l_f # [m]\n",
" self.l_r = l_r # [m]\n",
" self.wheel_base = l_f + l_r # [m]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.delta_t = delta_t #[s]\n",
" self.ref_path = ref_path\n",
"\n",
" # visualization settings\n",
" self.vehicle_w = 3.00\n",
" self.vehicle_l = 4.00\n",
" self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
" self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
"\n",
" # reset environment\n",
" self.visualize_flag = visualize\n",
" self.reset()\n",
"\n",
" def reset(\n",
" self, \n",
" init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n",
" ) -> None:\n",
" \"\"\"reset environment to initial state\"\"\"\n",
"\n",
" # reset state variables\n",
" self.state = init_state\n",
"\n",
" # clear animation frames\n",
" self.frames = []\n",
"\n",
" if self.visualize_flag:\n",
" # prepare figure\n",
" self.fig = plt.figure(figsize=(9,9))\n",
" self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
" self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
" self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
" self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
"\n",
" # graph layout settings\n",
" ## main view\n",
" self.main_ax.set_aspect('equal')\n",
" self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
" self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
" self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
" self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
" ## mini map\n",
" self.minimap_ax.set_aspect('equal')\n",
" self.minimap_ax.axis('off')\n",
" ## steering angle view\n",
" self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
" self.steer_ax.axis('off')\n",
" ## acceleration view\n",
" self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
" self.accel_ax.axis('off')\n",
" \n",
" # apply tight layout\n",
" self.fig.tight_layout()\n",
"\n",
" def update(\n",
" self, \n",
" u: np.ndarray, \n",
" delta_t: float = 0.0, \n",
" append_frame: bool = True, \n",
" vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n",
" ) -> None:\n",
" \"\"\"update state variables\"\"\"\n",
" # keep previous states\n",
" x, y, yaw, v = self.state\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" l_r = self.l_r\n",
" dt = self.delta_t if delta_t == 0.0 else delta_t\n",
"\n",
" # limit control inputs\n",
" steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
" accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
" # update state variables\n",
" beta = np.arctan(l_r / l * np.tan(steer))\n",
" new_x = x + v * np.cos(yaw + beta) * dt\n",
" new_y = y + v * np.sin(yaw + beta) * dt\n",
" new_yaw = yaw + v / l * np.sin(beta) * dt\n",
" new_v = v + accel * dt\n",
" self.state = np.array([new_x, new_y, new_yaw, new_v])\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
"\n",
" # record frame\n",
" if append_frame:\n",
" self.append_frame(steer, accel, vehicle_traj)\n",
"\n",
" def get_state(self) -> np.ndarray:\n",
" \"\"\"return state variables\"\"\"\n",
" return self.state.copy()\n",
"\n",
" def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n",
" \"\"\"draw a frame of the animation.\"\"\"\n",
" # get current states\n",
" x, y, yaw, v = self.state\n",
"\n",
" ### main view ###\n",
" # draw the vehicle shape\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
" frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
"\n",
" # draw wheels\n",
" ww, wl = 0.4, 0.7 #[m]\n",
" wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
" wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
"\n",
" ## rear-left wheel\n",
" wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
" wheel_rl_x, wheel_rl_y = \\\n",
" self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
"\n",
" ## rear-right wheel\n",
" wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
" wheel_rr_x, wheel_rr_y = \\\n",
" self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
"\n",
" ## front-left wheel\n",
" wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
" wheel_fl_x, wheel_fl_y = \\\n",
" self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
"\n",
" ## front-right wheel\n",
" wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
" wheel_fr_x, wheel_fr_y = \\\n",
" self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
"\n",
" # draw the vehicle center circle\n",
" vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n",
" frame += [self.main_ax.add_artist(vehicle_center)]\n",
"\n",
" # draw the reference path\n",
" ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
" ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
" frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
"\n",
" # draw the information text\n",
" text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
" frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
"\n",
" # draw vehicle trajectory\n",
" if vehicle_traj.any():\n",
" vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n",
" vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n",
" frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n",
"\n",
" ### mini map view ###\n",
" frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
" rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
" frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
" frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
" if vehicle_traj.any():\n",
" frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n",
"\n",
" ### control input view ###\n",
" # steering angle\n",
" MAX_STEER = self.max_steer_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" s_abs = np.abs(steer)\n",
" if steer < 0.0: # when turning right\n",
" steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else: # when turning left\n",
" steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += steer_pie_obj\n",
" frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # acceleration\n",
" MAX_ACCEL = self.max_accel_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" a_abs = np.abs(accel)\n",
" if accel > 0.0:\n",
" accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else:\n",
" accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += accel_pie_obj\n",
" frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # append frame\n",
" self.frames.append(frame)\n",
"\n",
" # rotate shape and return location on the x-y plane.\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def show_animation(self, interval_ms: int) -> None:\n",
" \"\"\"show animation of the recorded frames\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
" html = display.HTML(ani.to_jshtml())\n",
" display.display(html)\n",
" plt.close()\n",
"\n",
" def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
" \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
" ani.save(filename, writer=movie_writer)\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Longitudinal Controller : Fuzzy Controller"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class FuzzyLongitudinalController():\n",
" def __init__(\n",
" self,\n",
" slow_thresh_rate: float = +0.6, # [%]\n",
" fast_thresh_rate: float = +1.4, # [%]\n",
" target_velocity: float = 3.0, # [m/s]\n",
" ) -> None:\n",
" \"\"\"initialize fuzzy controller for keeping constant velocity\"\"\"\n",
" # fuzzy control parameters\n",
" self.target_vel = target_velocity\n",
" self.slow_thresh_vel = slow_thresh_rate * target_velocity\n",
" self.fast_thresh_vel = fast_thresh_rate * target_velocity\n",
" self.acc_when_vel_is_slow = +2.0 # [m/s^2]\n",
" self.acc_when_vel_is_mid = 0.0 # [m/s^2]\n",
" self.acc_when_vel_is_fast = -2.0 # [m/s^2]\n",
"\n",
" self.pre_e = 0.0 # previous tracking error\n",
" self.integrated_e = 0.0 # integrated tracking error\n",
"\n",
" def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n",
" \"\"\"calculate control input\"\"\"\n",
" # calculate tracking error, its integral and derivative\n",
" v = observed_vel\n",
"\n",
" # calculate control input with fuzzy control\n",
" acc_cmd = self.defuzzify(v)\n",
"\n",
" return acc_cmd\n",
"\n",
" def defuzzify(self, vel: float) -> float:\n",
" \"\"\"defuzzigy control input\"\"\"\n",
" w_slow = 1.0\n",
" w_mid = 1.0\n",
" w_fast = 1.0\n",
" acc_cmd = self.acc_when_vel_is_slow * w_slow * self.speed_slow(vel) \\\n",
" + self.acc_when_vel_is_mid * w_mid * self.speed_mid(vel) \\\n",
" + self.acc_when_vel_is_fast * w_fast * self.speed_fast(vel)\n",
"\n",
" return acc_cmd\n",
"\n",
" def speed_slow(self, vel: float):\n",
" \"\"\"fuzzy membership function for slow speed\"\"\"\n",
" if vel < self.slow_thresh_vel:\n",
" return 1.0\n",
" elif vel < self.fast_thresh_vel:\n",
" return (self.fast_thresh_vel - vel) / (self.fast_thresh_vel - self.slow_thresh_vel)\n",
" else:\n",
" return 0.0\n",
"\n",
" def speed_mid(self, vel: float):\n",
" \"\"\"fuzzy membership function for middle speed\"\"\"\n",
" if vel < self.slow_thresh_vel:\n",
" return 0.0\n",
" elif vel < self.target_vel:\n",
" return (vel - self.slow_thresh_vel) / (self.target_vel - self.slow_thresh_vel)\n",
" elif vel < self.fast_thresh_vel:\n",
" return (self.fast_thresh_vel - vel) / (self.fast_thresh_vel - self.target_vel)\n",
" else:\n",
" return 0.0\n",
"\n",
" def speed_fast(self, vel: float):\n",
" \"\"\"fuzzy membership function for fast speed\"\"\"\n",
" if vel < self.slow_thresh_vel:\n",
" return 0.0\n",
" elif vel < self.fast_thresh_vel:\n",
" return (vel - self.slow_thresh_vel) / (self.fast_thresh_vel - self.slow_thresh_vel)\n",
" else:\n",
" return 1.0\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Lateral Controller : Fuzzy Controller"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class FuzzyLateralController():\n",
" def __init__(\n",
" self,\n",
" ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n",
" ) -> None:\n",
" \"\"\"initialize fuzzy controller for path-tracking\"\"\"\n",
" # fuzzy control parameters\n",
" self.y_e_n_thresh = -1.0 # [m]\n",
" self.y_e_z_thresh = 0.0 # [m]\n",
" self.y_e_p_thresh = +1.0 # [m]\n",
" self.steer_when_y_e_is_n = +0.523 # [rad]\n",
" self.steer_when_y_e_is_z = 0.0 # [rad]\n",
" self.steer_when_y_e_is_p = -0.523 # [rad]\n",
"\n",
" self.theta_e_n_thresh = -0.523/7.0 # [rad]\n",
" self.theta_e_z_thresh = 0.0 # [rad]\n",
" self.theta_e_p_thresh = +0.523/7.0 # [rad]\n",
" self.steer_when_theta_e_is_n = +0.523 # [rad]\n",
" self.steer_when_theta_e_is_z = 0.0 # [rad]\n",
" self.steer_when_theta_e_is_p = -0.523 # [rad]\n",
"\n",
" # ref_path info\n",
" self.ref_path = ref_path\n",
" self.prev_waypoints_idx = 0\n",
"\n",
" def calc_control_input(self, observed_x: np.ndarray, delta_t: float) -> float:\n",
" \"\"\"calculate control input\"\"\"\n",
"\n",
" # set vehicle state variables from observation\n",
" x = observed_x[0]\n",
" y = observed_x[1]\n",
" yaw = observed_x[2]\n",
" v = observed_x[3]\n",
"\n",
" # get the waypoint closest to current vehicle position\n",
" _, ref_x, ref_y, ref_yaw, _ = self._get_nearest_waypoint(x, y, update_prev_idx=True)\n",
" if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n",
" print(\"[ERROR] Reached the end of the reference path.\")\n",
" raise IndexError\n",
"\n",
" # which side of the reference path is the car on, the right or the left?\n",
" ## algorithm : http://www.hptown.com/ucad/Ufb00009.htm\n",
" x1, y1 = ref_x, ref_y\n",
" x2, y2 = ref_x + 1.0 * np.cos(ref_yaw), ref_y + 1.0 * np.sin(ref_yaw)\n",
" vx, vy = x2 - x1, y2 - y1\n",
" wx, wy = x - x1, y - y1\n",
" s = vx * wy - vy * wx # s>0 : vehicle is on the left of the path, s<0 : vehicle is on the left of the path,\n",
"\n",
" # get tracking error\n",
" y_e = np.sign(s) * np.sqrt((ref_x-x)**2 + (ref_y-y)**2) # lateral error \n",
" theta_e = yaw - ref_yaw # heading error\n",
" theta_e = np.arctan2(np.sin(theta_e), np.cos(theta_e)) # normalize heading error to [-pi, pi]\n",
"\n",
" # calculate control input\n",
" steer_cmd = self.defuzzify(y_e, theta_e)\n",
" return steer_cmd\n",
"\n",
" def defuzzify(self, y_e: float, theta_e: float) -> float:\n",
" \"\"\"defuzzigy control input\"\"\"\n",
" w_p = 0.6\n",
" w_z = 0.6\n",
" w_n = 0.6\n",
"\n",
" steer_cmd_y_e = self.steer_when_y_e_is_p * w_p * self.y_e_p(y_e) \\\n",
" + self.steer_when_y_e_is_z * w_z * self.y_e_z(y_e) \\\n",
" + self.steer_when_y_e_is_n * w_n * self.y_e_n(y_e)\n",
"\n",
" steer_cmd_theta_e = self.steer_when_theta_e_is_p * (1 - w_p) * self.theta_e_p(theta_e) \\\n",
" + self.steer_when_theta_e_is_z * (1 - w_z) * self.theta_e_z(theta_e) \\\n",
" + self.steer_when_theta_e_is_n * (1 - w_n) * self.theta_e_n(theta_e)\n",
"\n",
" steer_cmd = steer_cmd_y_e + steer_cmd_theta_e\n",
" return steer_cmd\n",
"\n",
" def y_e_n(self, y_e: float):\n",
" \"\"\"fuzzy membership function for y_e negative\"\"\"\n",
" if y_e < self.y_e_n_thresh:\n",
" return 1.0\n",
" elif y_e < self.y_e_z_thresh:\n",
" return (self.y_e_z_thresh - y_e) / (self.y_e_z_thresh - self.y_e_n_thresh)\n",
" else:\n",
" return 0.0\n",
"\n",
" def y_e_z(self, y_e: float):\n",
" \"\"\"fuzzy membership function for y_e zero\"\"\"\n",
" if y_e < self.y_e_n_thresh:\n",
" return 0.0\n",
" elif y_e < self.y_e_z_thresh:\n",
" return (y_e - self.y_e_n_thresh) / (self.y_e_z_thresh - self.y_e_n_thresh)\n",
" elif y_e < self.y_e_p_thresh:\n",
" return (self.y_e_p_thresh - y_e) / (self.y_e_p_thresh - self.y_e_z_thresh)\n",
" else:\n",
" return 0.0\n",
"\n",
" def y_e_p(self, y_e: float):\n",
" \"\"\"fuzzy membership function for y_e positive\"\"\"\n",
" if y_e < self.y_e_z_thresh:\n",
" return 0.0\n",
" elif y_e < self.y_e_p_thresh:\n",
" return (y_e - self.y_e_z_thresh) / (self.y_e_p_thresh - self.y_e_z_thresh)\n",
" else:\n",
" return 1.0\n",
"\n",
" def theta_e_n(self, theta_e: float):\n",
" \"\"\"fuzzy membership function for theta_e negative\"\"\"\n",
" if theta_e < self.theta_e_n_thresh:\n",
" return 1.0\n",
" elif theta_e < self.theta_e_z_thresh:\n",
" return (self.theta_e_z_thresh - theta_e) / (self.theta_e_z_thresh - self.theta_e_n_thresh)\n",
" else:\n",
" return 0.0\n",
"\n",
" def theta_e_z(self, theta_e: float):\n",
" \"\"\"fuzzy membership function for theta_e zero\"\"\"\n",
" if theta_e < self.theta_e_n_thresh:\n",
" return 0.0\n",
" elif theta_e < self.theta_e_z_thresh:\n",
" return (theta_e - self.theta_e_n_thresh) / (self.theta_e_z_thresh - self.theta_e_n_thresh)\n",
" elif theta_e < self.theta_e_p_thresh:\n",
" return (self.theta_e_p_thresh - theta_e) / (self.theta_e_p_thresh - self.theta_e_z_thresh)\n",
" else:\n",
" return 0.0\n",
"\n",
" def theta_e_p(self, theta_e: float):\n",
" \"\"\"fuzzy membership function for theta_e positive\"\"\"\n",
" if theta_e < self.theta_e_z_thresh:\n",
" return 0.0\n",
" elif theta_e < self.theta_e_p_thresh:\n",
" return (theta_e - self.theta_e_z_thresh) / (self.theta_e_p_thresh - self.theta_e_z_thresh)\n",
" else:\n",
" return 1.0\n",
"\n",
" def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n",
" \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n",
" SEARCH_IDX_LEN = 200 # [points] forward search range\n",
" prev_idx = self.prev_waypoints_idx\n",
" dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n",
" dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n",
" d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n",
" min_d = min(d)\n",
" nearest_idx = d.index(min_d) + prev_idx\n",
"\n",
" # get reference values of the nearest waypoint\n",
" ref_x = self.ref_path[nearest_idx,0]\n",
" ref_y = self.ref_path[nearest_idx,1]\n",
" ref_yaw = self.ref_path[nearest_idx,2]\n",
" ref_v = self.ref_path[nearest_idx,3]\n",
"\n",
" # update nearest waypoint index if necessary\n",
" if update_prev_idx:\n",
" self.prev_waypoints_idx = nearest_idx \n",
"\n",
" return nearest_idx, ref_x, ref_y, ref_yaw, ref_v"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Simulation of Path-Tracking\n",
"- Longitudinal Control : Fuzzy Controller\n",
"- Lateral Control : Fuzzy Controller"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"delta_t = 0.05 # [sec]\n",
"sim_steps = 1000 # [steps]\n",
"print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n",
"\n",
"# load and visualize reference path\n",
"ref_path = np.genfromtxt('./ovalpath.csv', delimiter=',', skip_header=1)\n",
"plt.title(\"Reference Path\")\n",
"plt.plot(ref_path[:,0], ref_path[:,1])\n",
"plt.show()\n",
"\n",
"# initialize a vehicle as a control target\n",
"vehicle = Vehicle(\n",
" max_steer_abs=0.523, # [rad]\n",
" max_accel_abs=2.000, # [m/s^2]\n",
" ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\n",
")\n",
"vehicle.reset(\n",
" init_state = np.array([0.0, 1.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n",
")\n",
"vehicle_trajectory = np.array([vehicle.get_state()[:2]])\n",
"\n",
"# initialize fuzzy controller for acceleration control\n",
"fuzzy_lon_controller = FuzzyLongitudinalController(\n",
" target_velocity = +5.0 # [m/s]\n",
")\n",
"\n",
"# initialize fuzzy controller for steering control\n",
"fuzzy_lat_controller = FuzzyLateralController(\n",
" ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\n",
")\n",
"\n",
"# simulation loop\n",
"for i in range(sim_steps):\n",
"\n",
" # get current state of vehicle\n",
" current_state = vehicle.get_state()\n",
"\n",
" try:\n",
" # calculate control inputs\n",
" current_velocity = current_state[3]\n",
" accel_input = fuzzy_lon_controller.calc_control_input(observed_vel=current_velocity, delta_t=delta_t)\n",
" steer_input = fuzzy_lat_controller.calc_control_input(observed_x=current_state, delta_t=delta_t)\n",
"\n",
" except IndexError as ex:\n",
" # the vehicle has reached the end of the reference path\n",
" print(\"[ERROR] IndexError detected. Terminate simulation.\")\n",
" break\n",
"\n",
" # print current state and input force\n",
" print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={steer_input:>+6.2f}[rad], accel={accel_input:>+6.2f}[m/s]\")\n",
"\n",
" # update states of vehicle\n",
" vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation\n",
"vehicle.show_animation(interval_ms=int(delta_t * 1000))\n",
"\n",
"# save animation as a mp4 file if necessary\n",
"# vehicle.save_animation(\"fuzzy_pathtracking_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.5"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
================================================
FILE: notebooks/kinematic_bicycle_model.ipynb
================================================
{
"cells": [
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"# Kinematic Bicycle Model"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Definition of Coordinate Systems\n",
"\n",
"<img src=\"../media/def_of_frames.svg\" width=\"500px\" alt=\"definition_of_frames\">\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## State Equation\n",
"\n",
"<img src=\"../media/KBM.svg\" width=\"500px\" alt=\"KBM\">\n",
"\n",
"$$\n",
"\\begin{align}\n",
"& \\frac{\\mathrm{d}}{\\mathrm{d}t}\n",
"\\begin{bmatrix}\n",
"p^G_x \\\\\n",
"p^G_y \\\\\n",
"\\phi \\\\\n",
"V\n",
"\\end{bmatrix}\n",
"=\n",
"\\begin{bmatrix}\n",
"V \\cos(\\phi + \\beta) \\\\\n",
"V \\sin(\\phi + \\beta) \\\\\n",
"(V/l_r) \\sin\\beta \\\\\n",
"{a}\n",
"\\end{bmatrix},\\\\ \\\\\n",
"& \\beta = \\tan^{-1} \\left( \\frac{l_r}{l_f + l_r} \\tan({\\delta}) \\right).\n",
"\\end{align}\n",
"$$\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from typing import Tuple\n",
"from matplotlib import patches\n",
"from matplotlib.animation import ArtistAnimation\n",
"from IPython import display"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class Vehicle():\n",
" def __init__(\n",
" self,\n",
" l_f: float = 1.5, # [m]\n",
" l_r: float = 1.0, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n",
" delta_t: float = 0.05, # [s]\n",
" visualize: bool = True,\n",
" ) -> None:\n",
" \"\"\"initialize vehicle environment\n",
" state variables:\n",
" x: x-axis position in the global frame [m]\n",
" y: y-axis position in the global frame [m]\n",
" yaw: orientation in the global frame [rad]\n",
" v: longitudinal velocity [m/s]\n",
" control input:\n",
" steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
" accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
" Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n",
" \"\"\"\n",
" # vehicle parameters\n",
" self.l_f = l_f # [m]\n",
" self.l_r = l_r # [m]\n",
" self.wheel_base = l_f + l_r # [m]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.delta_t = delta_t #[s]\n",
" self.ref_path = ref_path\n",
"\n",
" # visualization settings\n",
" self.vehicle_w = 3.00\n",
" self.vehicle_l = 4.00\n",
" self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
" self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
"\n",
" # reset environment\n",
" self.visualize_flag = visualize\n",
" self.reset()\n",
"\n",
" def reset(\n",
" self, \n",
" init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n",
" ) -> None:\n",
" \"\"\"reset environment to initial state\"\"\"\n",
"\n",
" # reset state variables\n",
" self.state = init_state\n",
"\n",
" # clear animation frames\n",
" self.frames = []\n",
"\n",
" if self.visualize_flag:\n",
" # prepare figure\n",
" self.fig = plt.figure(figsize=(9,9))\n",
" self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
" self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
" self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
" self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
"\n",
" # graph layout settings\n",
" ## main view\n",
" self.main_ax.set_aspect('equal')\n",
" self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
" self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
" self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
" self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
" ## mini map\n",
" self.minimap_ax.set_aspect('equal')\n",
" self.minimap_ax.axis('off')\n",
" ## steering angle view\n",
" self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
" self.steer_ax.axis('off')\n",
" ## acceleration view\n",
" self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
" self.accel_ax.axis('off')\n",
" \n",
" # apply tight layout\n",
" self.fig.tight_layout()\n",
"\n",
" def update(\n",
" self, \n",
" u: np.ndarray, \n",
" delta_t: float = 0.0, \n",
" append_frame: bool = True, \n",
" vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n",
" ) -> None:\n",
" \"\"\"update state variables\"\"\"\n",
" # keep previous states\n",
" x, y, yaw, v = self.state\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" l_r = self.l_r\n",
" dt = self.delta_t if delta_t == 0.0 else delta_t\n",
"\n",
" # limit control inputs\n",
" steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
" accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
" # update state variables\n",
" beta = np.arctan(l_r / l * np.tan(steer))\n",
" new_x = x + v * np.cos(yaw + beta) * dt\n",
" new_y = y + v * np.sin(yaw + beta) * dt\n",
" new_yaw = yaw + v / l * np.sin(beta) * dt\n",
" new_v = v + accel * dt\n",
" self.state = np.array([new_x, new_y, new_yaw, new_v])\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
"\n",
" # record frame\n",
" if append_frame:\n",
" self.append_frame(steer, accel, vehicle_traj)\n",
"\n",
" def get_state(self) -> np.ndarray:\n",
" \"\"\"return state variables\"\"\"\n",
" return self.state.copy()\n",
"\n",
" def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n",
" \"\"\"draw a frame of the animation.\"\"\"\n",
" # get current states\n",
" x, y, yaw, v = self.state\n",
"\n",
" ### main view ###\n",
" # draw the vehicle shape\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
" frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
"\n",
" # draw wheels\n",
" ww, wl = 0.4, 0.7 #[m]\n",
" wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
" wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
"\n",
" ## rear-left wheel\n",
" wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
" wheel_rl_x, wheel_rl_y = \\\n",
" self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
"\n",
" ## rear-right wheel\n",
" wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
" wheel_rr_x, wheel_rr_y = \\\n",
" self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
"\n",
" ## front-left wheel\n",
" wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
" wheel_fl_x, wheel_fl_y = \\\n",
" self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
"\n",
" ## front-right wheel\n",
" wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
" wheel_fr_x, wheel_fr_y = \\\n",
" self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
"\n",
" # draw the vehicle center circle\n",
" vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n",
" frame += [self.main_ax.add_artist(vehicle_center)]\n",
"\n",
" # draw the reference path\n",
" ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
" ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
" frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
"\n",
" # draw the information text\n",
" text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
" frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
"\n",
" # draw vehicle trajectory\n",
" if vehicle_traj.any():\n",
" vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n",
" vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n",
" frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n",
"\n",
" ### mini map view ###\n",
" frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
" rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
" frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
" frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
" if vehicle_traj.any():\n",
" frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n",
"\n",
" ### control input view ###\n",
" # steering angle\n",
" MAX_STEER = self.max_steer_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" s_abs = np.abs(steer)\n",
" if steer < 0.0: # when turning right\n",
" steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else: # when turning left\n",
" steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += steer_pie_obj\n",
" frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # acceleration\n",
" MAX_ACCEL = self.max_accel_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" a_abs = np.abs(accel)\n",
" if accel > 0.0:\n",
" accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else:\n",
" accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += accel_pie_obj\n",
" frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # append frame\n",
" self.frames.append(frame)\n",
"\n",
" # rotate shape and return location on the x-y plane.\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def show_animation(self, interval_ms: int) -> None:\n",
" \"\"\"show animation of the recorded frames\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
" html = display.HTML(ani.to_jshtml())\n",
" display.display(html)\n",
" plt.close()\n",
"\n",
" def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
" \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
" ani.save(filename, writer=movie_writer)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Run Simulation"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Zig Zag Run"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"sim_step = 100 # [step]\n",
"delta_t = 0.1 # [s]\n",
"\n",
"# initialize vehicle simulator\n",
"ref_path_x = np.linspace(-3.0, 50.0, 10)\n",
"ref_path_y = np.zeros(10)\n",
"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n",
"vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 0.0])) # set initial state of the vehicle, [x, y, yaw, v]\n",
"vehicle_trajectory = np.array(vehicle.get_state()[:2])\n",
"\n",
"# simulation loop\n",
"for i in range(sim_step):\n",
" steer_input = 0.6 * np.sin(i/5.0) # steering command [rad]\n",
" accel_input = 0.5 + 0.5 * np.abs(np.sin(i/10.0)) # acceleration command [m/s^2]\n",
" vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation on jupyter notebook\n",
"vehicle.show_animation(interval_ms=delta_t*1000)\n",
"\n",
"# save animation as a mp4 file if necessary\n",
"# vehicle.save_animation(\"kbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Steady Input"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# simulation settings\n",
"sim_step = 200 # [step]\n",
"delta_t = 0.1 # [s]\n",
"\n",
"# initialize vehicle simulator\n",
"ref_path_x = np.linspace(-25.0, 25.0, 10)\n",
"ref_path_y = np.zeros(10)\n",
"vehicle = Vehicle(ref_path = np.array([ref_path_x, ref_path_y]).T, delta_t=delta_t)\n",
"vehicle.reset(init_state=np.array([0.0, 0.0, 0.0, 5.5])) # set initial state of the vehicle, [x, y, yaw, v]\n",
"vehicle_trajectory = np.array(vehicle.get_state()[:2])\n",
"\n",
"# simulation loop\n",
"for i in range(sim_step):\n",
" steer_input = 0.3 # steering command [rad]\n",
" accel_input = 2.0 # acceleration command [m/s^2]\n",
" vehicle.update(u=[steer_input, accel_input], delta_t=delta_t, vehicle_traj=vehicle_trajectory) # update vehicle state\n",
" vehicle_trajectory = np.vstack((vehicle_trajectory, vehicle.get_state()[:2])) # record vehicle trajectory\n",
"\n",
"# show animation on jupyter notebook\n",
"vehicle.show_animation(interval_ms=delta_t*1000)\n",
"\n",
"# save animation as a mp4 file if necessary\n",
"# vehicle.save_animation(\"kbm.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
]
}
],
"metadata": {
"kernelspec": {
"display_name": ".venv",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.5"
},
"orig_nbformat": 4
},
"nbformat": 4,
"nbformat_minor": 2
}
================================================
FILE: notebooks/lqr.ipynb
================================================
{
"cells": [
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"# Linear Quadratic Regulator (LQR)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from typing import Tuple\n",
"from matplotlib import patches\n",
"from matplotlib.animation import ArtistAnimation\n",
"from IPython import display"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Control Target : Vehicle \n",
"- Longitudinal dynamics : Point Mass Model\n",
"- Lateral dynamics : Kinematic Bicycle Model"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class Vehicle():\n",
" def __init__(\n",
" self,\n",
" l_f: float = 1.5, # [m]\n",
" l_r: float = 1.0, # [m]\n",
" max_steer_abs: float = 0.523, # [rad]\n",
" max_accel_abs: float = 2.000, # [m/s^2]\n",
" ref_path: np.ndarray = np.array([[-30.0, 0.0], [30.0, 0.0]]),\n",
" delta_t: float = 0.05, # [s]\n",
" visualize: bool = True,\n",
" ) -> None:\n",
" \"\"\"initialize vehicle environment\n",
" state variables:\n",
" x: x-axis position in the global frame [m]\n",
" y: y-axis position in the global frame [m]\n",
" yaw: orientation in the global frame [rad]\n",
" v: longitudinal velocity [m/s]\n",
" control input:\n",
" steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
" accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
" Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n",
" \"\"\"\n",
" # vehicle parameters\n",
" self.l_f = l_f # [m]\n",
" self.l_r = l_r # [m]\n",
" self.wheel_base = l_f + l_r # [m]\n",
" self.max_steer_abs = max_steer_abs # [rad]\n",
" self.max_accel_abs = max_accel_abs # [m/s^2]\n",
" self.delta_t = delta_t #[s]\n",
" self.ref_path = ref_path\n",
"\n",
" # visualization settings\n",
" self.vehicle_w = 3.00\n",
" self.vehicle_l = 4.00\n",
" self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
" self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
"\n",
" # reset environment\n",
" self.visualize_flag = visualize\n",
" self.reset()\n",
"\n",
" def reset(\n",
" self, \n",
" init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n",
" ) -> None:\n",
" \"\"\"reset environment to initial state\"\"\"\n",
"\n",
" # reset state variables\n",
" self.state = init_state\n",
"\n",
" # clear animation frames\n",
" self.frames = []\n",
"\n",
" if self.visualize_flag:\n",
" # prepare figure\n",
" self.fig = plt.figure(figsize=(9,9))\n",
" self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
" self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
" self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
" self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
"\n",
" # graph layout settings\n",
" ## main view\n",
" self.main_ax.set_aspect('equal')\n",
" self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
" self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
" self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
" self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
" ## mini map\n",
" self.minimap_ax.set_aspect('equal')\n",
" self.minimap_ax.axis('off')\n",
" ## steering angle view\n",
" self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
" self.steer_ax.axis('off')\n",
" ## acceleration view\n",
" self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
" self.accel_ax.axis('off')\n",
" \n",
" # apply tight layout\n",
" self.fig.tight_layout()\n",
"\n",
" def update(\n",
" self, \n",
" u: np.ndarray, \n",
" delta_t: float = 0.0, \n",
" append_frame: bool = True, \n",
" vehicle_traj: np.ndarray = np.empty(0), # vehicle trajectory\n",
" ) -> None:\n",
" \"\"\"update state variables\"\"\"\n",
" # keep previous states\n",
" x, y, yaw, v = self.state\n",
"\n",
" # prepare params\n",
" l = self.wheel_base\n",
" l_r = self.l_r\n",
" dt = self.delta_t if delta_t == 0.0 else delta_t\n",
"\n",
" # limit control inputs\n",
" steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
" accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
"\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
" # update state variables\n",
" beta = np.arctan(l_r / l * np.tan(steer))\n",
" new_x = x + v * np.cos(yaw + beta) * dt\n",
" new_y = y + v * np.sin(yaw + beta) * dt\n",
" new_yaw = yaw + v / l * np.sin(beta) * dt\n",
" new_v = v + accel * dt\n",
" self.state = np.array([new_x, new_y, new_yaw, new_v])\n",
" \"\"\"< CORE OF VEHICLE DYNAMICS >\"\"\"\n",
"\n",
" # record frame\n",
" if append_frame:\n",
" self.append_frame(steer, accel, vehicle_traj)\n",
"\n",
" def get_state(self) -> np.ndarray:\n",
" \"\"\"return state variables\"\"\"\n",
" return self.state.copy()\n",
"\n",
" def append_frame(self, steer: float, accel: float, vehicle_traj: np.ndarray) -> list:\n",
" \"\"\"draw a frame of the animation.\"\"\"\n",
" # get current states\n",
" x, y, yaw, v = self.state\n",
"\n",
" ### main view ###\n",
" # draw the vehicle shape\n",
" vw, vl = self.vehicle_w, self.vehicle_l\n",
" vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
" vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
" rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
" frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
"\n",
" # draw wheels\n",
" ww, wl = 0.4, 0.7 #[m]\n",
" wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
" wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
"\n",
" ## rear-left wheel\n",
" wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
" wheel_rl_x, wheel_rl_y = \\\n",
" self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
"\n",
" ## rear-right wheel\n",
" wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
" wheel_rr_x, wheel_rr_y = \\\n",
" self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
"\n",
" ## front-left wheel\n",
" wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
" wheel_fl_x, wheel_fl_y = \\\n",
" self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
"\n",
" ## front-right wheel\n",
" wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
" self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
" wheel_fr_x, wheel_fr_y = \\\n",
" self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
" frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
"\n",
" # draw the vehicle center circle\n",
" vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=4)\n",
" frame += [self.main_ax.add_artist(vehicle_center)]\n",
"\n",
" # draw the reference path\n",
" ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
" ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
" frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
"\n",
" # draw the information text\n",
" text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
" frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
"\n",
" # draw vehicle trajectory\n",
" if vehicle_traj.any():\n",
" vehicle_traj_x_offset = np.append(np.ravel(vehicle_traj[:, 0]) - np.full(vehicle_traj.shape[0], x), [0.0])\n",
" vehicle_traj_y_offset = np.append(np.ravel(vehicle_traj[:, 1]) - np.full(vehicle_traj.shape[0], y), [0.0])\n",
" frame += self.main_ax.plot(vehicle_traj_x_offset, vehicle_traj_y_offset, color='purple', linestyle=\"solid\", linewidth=2.0)\n",
"\n",
" ### mini map view ###\n",
" frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
" rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
" self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
" frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
" frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
" if vehicle_traj.any():\n",
" frame += self.minimap_ax.plot(vehicle_traj[:, 0], vehicle_traj[:, 1], color='purple', linestyle=\"solid\", linewidth=1.0)\n",
"\n",
" ### control input view ###\n",
" # steering angle\n",
" MAX_STEER = self.max_steer_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" s_abs = np.abs(steer)\n",
" if steer < 0.0: # when turning right\n",
" steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else: # when turning left\n",
" steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += steer_pie_obj\n",
" frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # acceleration\n",
" MAX_ACCEL = self.max_accel_abs\n",
" PIE_RATE = 3.0/4.0\n",
" PIE_STARTANGLE = 225 # [deg]\n",
" a_abs = np.abs(accel)\n",
" if accel > 0.0:\n",
" accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" else:\n",
" accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
" frame += accel_pie_obj\n",
" frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
"\n",
" # append frame\n",
" self.frames.append(frame)\n",
"\n",
" # rotate shape and return location on the x-y plane.\n",
" def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
" transformed_x = []\n",
" transformed_y = []\n",
" if len(xlist) != len(ylist):\n",
" print(\"[ERROR] xlist and ylist must have the same size.\")\n",
" raise AttributeError\n",
"\n",
" for i, xval in enumerate(xlist):\n",
" transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
" transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
" transformed_x.append(transformed_x[0])\n",
" transformed_y.append(transformed_y[0])\n",
" return transformed_x, transformed_y\n",
"\n",
" def show_animation(self, interval_ms: int) -> None:\n",
" \"\"\"show animation of the recorded frames\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
" html = display.HTML(ani.to_jshtml())\n",
" display.display(html)\n",
" plt.close()\n",
"\n",
" def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
" \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
" ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
" ani.save(filename, writer=movie_writer)\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Longitudinal Controller : PID Controller"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class PIDLongitudinalController():\n",
" def __init__(\n",
" self,\n",
" p_gain: float = +1.2, # P Gain\n",
" i_gain: float = +0.4, # I Gain\n",
" d_gain: float = +0.1, # D Gain\n",
" target_velocity: float = 3.0, # [m/s]\n",
" ) -> None:\n",
" \"\"\"initialize pid controller for keeping constant velocity\"\"\"\n",
" # pid control parameters\n",
" self.K_p = p_gain\n",
" self.K_i = i_gain\n",
" self.K_d = d_gain\n",
" self.target_vel = target_velocity\n",
" self.pre_e = 0.0 # previous tracking error\n",
" self.integrated_e = 0.0 # integrated tracking error\n",
"\n",
" def calc_control_input(self, observed_vel: float, delta_t: float) -> None:\n",
" \"\"\"calculate control input\"\"\"\n",
"\n",
" # calculate tracking error, its integral and derivative\n",
" r = self.target_vel\n",
" y = observed_vel\n",
" e = r - y # tracking error to the traget velocity\n",
" ie = self.integrated_e + (e + self.pre_e) * delta_t / 2.0 # integral of the tracking error\n",
" de = (e - self.pre_e) / delta_t # derivative of the tracking error\n",
"\n",
" # calculate control input\n",
" acc_cmd = self.K_p * e + self.K_i * ie + self.K_d * de\n",
"\n",
" # update previous tracking error\n",
" self.pre_e = e\n",
"\n",
" return acc_cmd\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Lateral Controller : Linear Quadratic Regulator (LQR)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class LQRLateralController():\n",
" def __init__(\n",
" self,\n",
" wheel_base: float = 2.5, # [m] wheel_base\n",
" Q: np.ndarray = np.diag([1.0, 1.0]), # weight matrix for state variables\n",
" R: np.ndarray = np.diag([1.0]), # weight matrix for control inputs\n",
" ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n",
" ) -> None:\n",
" \"\"\"initialize lqr controller for path-tracking\"\"\"\n",
" # vehicle parameters\n",
" self.l = wheel_base # [m] wheel base\n",
"\n",
" # weight matrices for LQR\n",
" self.Q = Q # weight matrix for state variables\n",
" self.R
gitextract_vv2ds5dc/ ├── .gitignore ├── .python-version ├── LICENSE.txt ├── README.md ├── media/ │ └── .gitkeep ├── notebooks/ │ ├── bangbang.ipynb │ ├── dwa_obstacle_avoidance.ipynb │ ├── dwa_pathtracking.ipynb │ ├── dynamic_bicycle_model.ipynb │ ├── fuzzy.ipynb │ ├── kinematic_bicycle_model.ipynb │ ├── lqr.ipynb │ ├── mppi_obstacle_avoidance.ipynb │ ├── mppi_pathtracking.ipynb │ ├── ovalpath.csv │ ├── pid.ipynb │ ├── preliminary/ │ │ ├── linear_quadratic_regulator_tutorial.ipynb │ │ └── state_feedback_control_tutorial.ipynb │ ├── purepursuit.ipynb │ ├── stanley.ipynb │ ├── state_feedback.ipynb │ └── unicycle_model.ipynb └── pyproject.toml
Condensed preview — 23 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (752K chars).
[
{
"path": ".gitignore",
"chars": 486,
"preview": "*.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*"
},
{
"path": ".python-version",
"chars": 7,
"preview": "3.10.5\n"
},
{
"path": "LICENSE.txt",
"chars": 1067,
"preview": "MIT License\n\nCopyright (c) 2024 MizuhoAOKI\n\nPermission is hereby granted, free of charge, to any person obtaining a copy"
},
{
"path": "README.md",
"chars": 7039,
"preview": "[](https://opensource.org/licenses/MIT)\n[![Poetry](htt"
},
{
"path": "media/.gitkeep",
"chars": 0,
"preview": ""
},
{
"path": "notebooks/bangbang.ipynb",
"chars": 26551,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Bang-Bang C"
},
{
"path": "notebooks/dwa_obstacle_avoidance.ipynb",
"chars": 38498,
"preview": "{\n \"cells\": [\n {\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Dynamic Window Approach\"\n ]\n }"
},
{
"path": "notebooks/dwa_pathtracking.ipynb",
"chars": 33200,
"preview": "{\n \"cells\": [\n {\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Dynamic Window Approach\"\n ]\n }"
},
{
"path": "notebooks/dynamic_bicycle_model.ipynb",
"chars": 21897,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Dynamic Bic"
},
{
"path": "notebooks/fuzzy.ipynb",
"chars": 31138,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Fuzzy Contr"
},
{
"path": "notebooks/kinematic_bicycle_model.ipynb",
"chars": 19985,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Kinematic B"
},
{
"path": "notebooks/lqr.ipynb",
"chars": 27909,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Linear Quad"
},
{
"path": "notebooks/mppi_obstacle_avoidance.ipynb",
"chars": 44076,
"preview": "{\n \"cells\": [\n {\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# MPPI (Model Predictive Path-Integ"
},
{
"path": "notebooks/mppi_pathtracking.ipynb",
"chars": 38710,
"preview": "{\n \"cells\": [\n {\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# MPPI (Model Predictive Path-Integ"
},
{
"path": "notebooks/ovalpath.csv",
"chars": 34843,
"preview": "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\n"
},
{
"path": "notebooks/pid.ipynb",
"chars": 27970,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# PID Control"
},
{
"path": "notebooks/preliminary/linear_quadratic_regulator_tutorial.ipynb",
"chars": 122612,
"preview": "{\n \"cells\": [\n {\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Stabilize A Linear System With Li"
},
{
"path": "notebooks/preliminary/state_feedback_control_tutorial.ipynb",
"chars": 133386,
"preview": "{\n \"cells\": [\n {\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Stabilize A Linear System With St"
},
{
"path": "notebooks/purepursuit.ipynb",
"chars": 25352,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Pure Pursui"
},
{
"path": "notebooks/stanley.ipynb",
"chars": 25780,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Stanley Con"
},
{
"path": "notebooks/state_feedback.ipynb",
"chars": 27018,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# State Feedb"
},
{
"path": "notebooks/unicycle_model.ipynb",
"chars": 19630,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"metadata\": {},\n \"source\": [\n \"# Unicycle Mo"
},
{
"path": "pyproject.toml",
"chars": 399,
"preview": "[tool.poetry]\nname = \"path-tracking\"\nversion = \"0.1.0\"\ndescription = \"\"\nauthors = [\"MizuhoAOKI <mizuhoaoki1998@gmail.com"
}
]
About this extraction
This page contains the full source code of the MizuhoAOKI/path_tracking_catalog GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 23 files (691.0 KB), approximately 310.6k tokens. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.