├── .gitignore ├── LICENSE ├── README.md ├── example_racecar.py ├── example_racecar.yaml ├── experiments └── ilqr_jax │ └── rollout.gif ├── iLQR ├── __init__.py ├── constraints.py ├── cost.py ├── dynamics.py ├── ellipsoid_obj.py ├── ilqr.py ├── shielding.py ├── track.py └── utils.py ├── misc ├── alter.png └── ego.png └── outerloop_center_smooth.csv /.gitignore: -------------------------------------------------------------------------------- 1 | ### Python ### 2 | # Byte-compiled / optimized / DLL files 3 | __pycache__/ 4 | *.py[cod] 5 | *$py.class 6 | *.png 7 | !misc/*.png 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | wheels/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | cover/ 56 | 57 | # Translations 58 | *.mo 59 | *.pot 60 | 61 | # Django stuff: 62 | *.log 63 | local_settings.py 64 | db.sqlite3 65 | db.sqlite3-journal 66 | 67 | # Flask stuff: 68 | instance/ 69 | .webassets-cache 70 | 71 | # Scrapy stuff: 72 | .scrapy 73 | 74 | # Sphinx documentation 75 | docs/_build/ 76 | 77 | # PyBuilder 78 | .pybuilder/ 79 | target/ 80 | 81 | # Jupyter Notebook 82 | .ipynb_checkpoints 83 | 84 | # IPython 85 | profile_default/ 86 | ipython_config.py 87 | 88 | # pyenv 89 | # For a library or package, you might want to ignore these files since the code is 90 | # intended to run in multiple environments; otherwise, check them in: 91 | # .python-version 92 | 93 | # pipenv 94 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 95 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 96 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 97 | # install all needed dependencies. 98 | #Pipfile.lock 99 | 100 | # poetry 101 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 102 | # This is especially recommended for binary packages to ensure reproducibility, and is more 103 | # commonly ignored for libraries. 104 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 105 | #poetry.lock 106 | 107 | # pdm 108 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 109 | #pdm.lock 110 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 111 | # in version control. 112 | # https://pdm.fming.dev/#use-with-ide 113 | .pdm.toml 114 | 115 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 116 | __pypackages__/ 117 | 118 | # Celery stuff 119 | celerybeat-schedule 120 | celerybeat.pid 121 | 122 | # SageMath parsed files 123 | *.sage.py 124 | 125 | # Environments 126 | .env 127 | .venv 128 | env/ 129 | venv/ 130 | ENV/ 131 | env.bak/ 132 | venv.bak/ 133 | 134 | # Spyder project settings 135 | .spyderproject 136 | .spyproject 137 | 138 | # Rope project settings 139 | .ropeproject 140 | 141 | # mkdocs documentation 142 | /site 143 | 144 | # mypy 145 | .mypy_cache/ 146 | .dmypy.json 147 | dmypy.json 148 | 149 | # Pyre type checker 150 | .pyre/ 151 | 152 | # pytype static type analyzer 153 | .pytype/ 154 | 155 | # Cython debug symbols 156 | cython_debug/ 157 | 158 | # PyCharm 159 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 160 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 161 | # and can be added to the global gitignore or merged into this file. For a more nuclear 162 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 163 | #.idea/ 164 | 165 | ### Python Patch ### 166 | # Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration 167 | poetry.toml 168 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Safe Robotics Lab 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Safe Autonomous Racing using ILQR and Rollout-based Shielding: A JAX Implementation 2 | 3 | [![License][license-shield]][license-url] 4 | [![Python 3.10](https://img.shields.io/badge/python-3.10-blue)](https://www.python.org/downloads/) 5 | [![Colab Notebook][homepage-shield]][homepage-url] 6 | 7 | 8 | 9 |
10 |

11 | 12 | Logo 13 | 14 | 15 |

16 | 17 |

18 |

19 | 20 | 21 | 22 |
23 |

Table of Contents

 24 |
    25 |
  1. About The Project
    2. 26 |
  2. Dependencies
    3. 27 |
  3. Example
    4. 28 |
  4. License
    5. 29 |
  5. Contact
    6. 30 |
  6. Paper
    7. 31 |
  7. Acknowledgements
    8. 32 |
33 |
34 | 35 | 36 | 37 | ## About The Project 38 | 39 | This repository implements a safe autonomous racing example using ILQR and rollout-based shielding, which relies on [JAX](https://github.com/google/jax) for real-time computation performance based on automatic differentiation and just-in-time (JIT) compilation. 40 | The repo is primarily developed and maintained by [Haimin Hu](https://haiminhu.org/), a PhD student in the [Safe Robotics Lab](https://saferobotics.princeton.edu). 41 | [Zixu Zhang](https://zzx9636.github.io/), [Kai-Chieh Hsu](https://kaichiehhsu.github.io/) and [Duy Nguyen](https://ece.princeton.edu/people/duy-phuong-nguyen) also contributed much to the repo (their original repo is [here](https://github.com/SafeRoboticsLab/PrincetonRaceCar_planning)). 42 | 43 | 44 | ## Dependencies 45 | 46 | This repo depends on the following packages: 47 | 1. jax=0.4.19 48 | 2. jaxlib=0.4.16 49 | 3. matplotlib=3.5.1 50 | 4. numpy=1.21.5 51 | 5. pyspline=1.5.1 52 | 6. python=3.10 53 | 7. yaml=0.2.5 54 | 55 | 56 | ## Example 57 | Please refer to the [Colab Notebook](https://colab.research.google.com/drive/1_3HgZx7LTBw69xH61Us70xI8HISUeFA7?usp=sharing) for a demo usage of this repo. 58 | 59 | Alternatively, you can directly run the main script: 60 | ```shell 61 | python3 example_racecar.py 62 | ``` 63 | 64 | 65 | 66 | ## License 67 | 68 | Distributed under the BSD 3-Clause License. See `LICENSE` for more information. 69 | 70 | 71 | 72 | ## Contact 73 | 74 | Haimin Hu - [@HaiminHu](https://twitter.com/HaiminHu) - haiminh@princeton.edu 75 | 76 | 77 | 78 | ## Paper 79 | 80 | If you found this repository helpful, please consider citing one of the following papers that turned this repository into research contributions. 81 | 82 | * ILQR-based safety filter: 83 | ```tex 84 | @article{hu2023active, 85 | title={Active uncertainty reduction for safe and efficient interaction planning: A shielding-aware dual control approach}, 86 | author={Hu, Haimin and Isele, David and Bae, Sangjae and Fisac, Jaime F}, 87 | journal={The International Journal of Robotics Research}, 88 | pages={02783649231215371}, 89 | year={2023}, 90 | publisher={SAGE Publications Sage UK: London, England} 91 | } 92 | ``` 93 | 94 | * ILQR-based Stackelberg game solver: 95 | ```tex 96 | @inproceedings{hu2024plays, 97 | title={Who Plays First? Optimizing the Order of Play in Stackelberg Games with Many Robots}, 98 | author={Hu, Haimin and Dragotto, Gabriele and Zhang, Zixu and Liang, Kaiqu and Stellato, Bartolomeo and Fisac, Jaime F}, 99 | booktitle={Proceedings of Robotics: Science and Systems}, 100 | year={2024} 101 | } 102 | ``` 103 | 104 | * ILQ-based game solver for data-aware planning: 105 | ```tex 106 | @inproceedings{lidard2024blending, 107 | title={Blending Data-Driven Priors in Dynamic Games}, 108 | author={Lidard, Justin and Hu, Haimin and Hancock, Asher and Zhang, Zixu and Contreras, Albert Gim{\'o} and Modi, Vikash and DeCastro, Jonathan and Gopinath, Deepak and Rosman, Guy and Leonard, Naomi and others}, 109 | booktitle={Proceedings of Robotics: Science and Systems}, 110 | year={2024} 111 | } 112 | ``` 113 | 114 | 115 | 116 | ## Acknowledgements 117 | 118 | This repo is inspired by the following projects: 119 | * [Princeton Race Car](https://github.com/SafeRoboticsLab/PrincetonRaceCar_planning) 120 | * [Ellipsoidal Toolbox (ET)](https://www.mathworks.com/matlabcentral/fileexchange/21936-ellipsoidal-toolbox-et) 121 | * ellReach: Ellipsoidal Reachable Set Computation for Linear Time-Varying Systems (under development) 122 | 123 | 124 | 125 | 126 | [contributors-shield]: https://img.shields.io/github/contributors/SafeRoboticsLab/repo.svg?style=for-the-badge 127 | [contributors-url]: https://github.com/SafeRoboticsLab/SHARP/contributors 128 | [forks-shield]: https://img.shields.io/github/forks/SafeRoboticsLab/repo.svg?style=for-the-badge 129 | [forks-url]: https://github.com/SafeRoboticsLab/SHARP/network/members 130 | [stars-shield]: https://img.shields.io/github/stars/SafeRoboticsLab/repo.svg?style=for-the-badge 131 | [stars-url]: https://github.com/SafeRoboticsLab/SHARP/stargazers 132 | [issues-shield]: https://img.shields.io/github/issues/SafeRoboticsLab/repo.svg?style=for-the-badge 133 | [issues-url]: https://github.com/SafeRoboticsLab/SHARP/issues 134 | [license-shield]: https://img.shields.io/badge/License-BSD%203--Clause-blue.svg 135 | [license-url]: https://opensource.org/licenses/BSD-3-Clause 136 | [linkedin-shield]: https://img.shields.io/badge/-LinkedIn-black.svg?style=for-the-badge&logo=linkedin&colorB=555 137 | [linkedin-url]: https://linkedin.com/in/SafeRoboticsLab 138 | [homepage-shield]: https://img.shields.io/badge/-Colab%20Notebook-orange 139 | [homepage-url]: https://colab.research.google.com/drive/1_3HgZx7LTBw69xH61Us70xI8HISUeFA7?usp=sharing 140 | -------------------------------------------------------------------------------- /example_racecar.py: -------------------------------------------------------------------------------- 1 | """ 2 | Please contact the author(s) of this library if you have any questions. 3 | Author: Haimin Hu (haiminh@princeton.edu) 4 | Reference: ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 5 | """ 6 | 7 | import os, jax, argparse 8 | import numpy as np 9 | from matplotlib import cm 10 | from matplotlib import pyplot as plt 11 | from matplotlib.transforms import Affine2D 12 | from IPython.display import Image 13 | import imageio.v2 as imageio 14 | 15 | jax.config.update('jax_platform_name', 'cpu') 16 | jax.config.update('jax_enable_x64', True) 17 | plt.rcParams['font.family'] = 'serif' 18 | plt.rcParams['font.serif'] = ['Times New Roman'] + plt.rcParams['font.serif'] 19 | 20 | from iLQR.utils import * 21 | from iLQR.ilqr import iLQR 22 | from iLQR.shielding import ILQshielding, NaiveSwerving 23 | from iLQR.ellipsoid_obj import EllipsoidObj 24 | 25 | 26 | def main(config_file): 27 | # Loads the config and track file. 28 | config = load_config(config_file) 29 | track = load_track(config) 30 | 31 | # Constructs static obstacles. 32 | static_obs_list = [] 33 | obs_a = config.LENGTH / 2.0 34 | obs_b = config.WIDTH / 2.0 35 | 36 | obs_q1 = np.array([0, 5.6])[:, np.newaxis] 37 | obs_Q1 = np.diag([obs_a**2, obs_b**2]) 38 | static_obs1 = EllipsoidObj(q=obs_q1, Q=obs_Q1) 39 | static_obs_list.append([static_obs1 for _ in range(config.N)]) 40 | 41 | obs_q2 = np.array([-2.15, 4.0])[:, np.newaxis] 42 | obs_Q2 = np.diag([obs_b**2, obs_a**2]) 43 | static_obs2 = EllipsoidObj(q=obs_q2, Q=obs_Q2) 44 | static_obs_list.append([static_obs2 for _ in range(config.N)]) 45 | 46 | obs_q3 = np.array([-2.4, 2.0])[:, np.newaxis] 47 | obs_Q3 = np.diag([obs_b**2, obs_a**2]) 48 | static_obs3 = EllipsoidObj(q=obs_q3, Q=obs_Q3) 49 | static_obs_list.append([static_obs3 for _ in range(config.N)]) 50 | 51 | static_obs_heading_list = [-np.pi, -np.pi / 2, -np.pi / 2] 52 | 53 | # Initialization. 54 | solver = iLQR(track, config, safety=False) # Nominal racing iLQR 55 | solver_sh = iLQR(track, config, safety=True) # Shielding safety backup iLQR 56 | 57 | # max_deacc = config.A_MIN / 3 # config.A_MIN, config.A_MIN / 2, config.A_MIN / 3 58 | # shielding = NaiveSwerving( 59 | # config, [static_obs1, static_obs2, static_obs3], solver.dynamics, 60 | # max_deacc=max_deacc, N_sh=15 61 | # ) 62 | 63 | shielding = ILQshielding( 64 | config, solver_sh, [static_obs1, static_obs2, static_obs3], static_obs_list, N_sh=15 65 | ) 66 | 67 | pos0, psi0 = track.interp([2]) # Position and yaw on the track. 68 | x_cur = np.array([3.3, 4.0, 0, np.pi / 2]) # Initial state. 69 | # x_cur = np.array([pos0[0], pos0[1], 0, psi0[-1]]) 70 | init_control = np.zeros((2, config.N)) 71 | t_total = 0. 72 | plot_cover = False 73 | 74 | itr_receding = config.MAX_ITER_RECEDING # The number of receding iterations. 75 | state_hist = np.zeros((4, itr_receding)) 76 | control_hist = np.zeros((2, itr_receding)) 77 | plan_hist = np.zeros((6, config.N, itr_receding)) 78 | K_hist = np.zeros((2, 4, config.N - 1, itr_receding)) 79 | fx_hist = np.zeros((4, 4, config.N, itr_receding)) 80 | fu_hist = np.zeros((4, 2, config.N, itr_receding)) 81 | 82 | # Specifies the folder to save figures. 83 | fig_prog_folder = os.path.join(config.OUT_FOLDER, "progress") 84 | os.makedirs(fig_prog_folder, exist_ok=True) 85 | ego = plt.imread('misc/ego.png', format="png") 86 | alter = plt.imread('misc/alter.png', format="png") 87 | 88 | # Define disturbances. 89 | sigma = np.array([config.SIGMA_X, config.SIGMA_Y, config.SIGMA_V, config.SIGMA_THETA]) 90 | 91 | # iLQR Planning. 92 | for i in range(itr_receding): 93 | # Plans the trajectory using iLQR. 94 | states, controls, t_process, status, _, K_closed_loop, fx, fu = ( 95 | solver.solve(x_cur, controls=init_control, obs_list=static_obs_list) 96 | ) 97 | 98 | # Shielding. 99 | control_sh = shielding.run(x=x_cur, u_nominal=controls[:, 0]) 100 | 101 | # Executes the control. 102 | x_cur = solver.dynamics.forward_step(x_cur, control_sh, step=1, noise=sigma)[0] 103 | print("[{}]: solver returns status {} and uses {:.3f}.".format(i, status, t_process), end='\r') 104 | if i > 0: # Excludes JAX compilation time at the first time step. 105 | t_total += t_process 106 | 107 | # Records planning history, states and controls. 108 | plan_hist[:4, :, i] = states 109 | plan_hist[4:, :, i] = controls 110 | state_hist[:, i] = states[:, 0] 111 | control_hist[:, i] = controls[:, 0] 112 | 113 | K_hist[:, :, :, i] = K_closed_loop 114 | fx_hist[:, :, :, i] = fx 115 | fu_hist[:, :, :, i] = fu 116 | 117 | # Updates the nominal control signal for warmstart of next receding horizon. 118 | init_control[:, :-1] = controls[:, 1:] 119 | 120 | # Plots the current progress. 121 | plt.clf() 122 | track.plot_track() 123 | for static_obs in static_obs_list: 124 | plot_ellipsoids( 125 | plt.gca(), static_obs[0:1], arg_list=[dict(c='k', linewidth=1.)], dims=[0, 1], N=50, 126 | plot_center=False, use_alpha=False 127 | ) 128 | if plot_cover: # plot circles that cover the footprint. 129 | static_obs[0].plot_circ(plt.gca()) 130 | solver.cost.soft_constraints.ego_ell[0].plot_circ(plt.gca()) 131 | if shielding.sh_flag: 132 | plot_ellipsoids( 133 | plt.gca(), [solver.cost.soft_constraints.ego_ell[0]], arg_list=[dict(c='r')], dims=[0, 1], 134 | N=50, plot_center=False 135 | ) 136 | else: 137 | plot_ellipsoids( 138 | plt.gca(), [solver.cost.soft_constraints.ego_ell[0]], arg_list=[dict(c='b')], dims=[0, 1], 139 | N=50, plot_center=False 140 | ) 141 | plt.plot(states[0, :], states[1, :], linewidth=2, c='b') 142 | if shielding.sh_flag: 143 | plt.plot(shielding.states[0, :], shielding.states[1, :], linewidth=2, c='r') 144 | sc = plt.scatter( 145 | state_hist[0, :i + 1], state_hist[1, :i + 1], s=24, c=state_hist[2, :i + 1], cmap=cm.jet, 146 | vmin=0, vmax=config.V_MAX, edgecolor='none', marker='o' 147 | ) 148 | # plot ego car figure 149 | transform_data = Affine2D().rotate_deg_around(*(x_cur[0], x_cur[1]), 150 | x_cur[3] / np.pi * 180) + plt.gca().transData 151 | plt.imshow( 152 | ego, transform=transform_data, interpolation='none', origin='lower', 153 | extent=[x_cur[0] - 0.25, x_cur[0] + 0.25, x_cur[1] - 0.1, 154 | x_cur[1] + 0.1], alpha=1.0, zorder=10.0, clip_on=True 155 | ) 156 | # plot alter cars figure 157 | for j in range(len(static_obs_list)): 158 | _static_obs = static_obs_list[j][0] 159 | transform_data = Affine2D().rotate_deg_around( 160 | *(_static_obs.q[0, 0], _static_obs.q[1, 0]), static_obs_heading_list[j] / np.pi * 180 161 | ) + plt.gca().transData 162 | plt.imshow( 163 | alter, transform=transform_data, interpolation='none', origin='lower', extent=[ 164 | _static_obs.q[0, 0] - 0.25, _static_obs.q[0, 0] + 0.25, _static_obs.q[1, 0] - 0.1, 165 | _static_obs.q[1, 0] + 0.1 166 | ], alpha=1.0, zorder=10.0, clip_on=True 167 | ) 168 | cbar = plt.colorbar(sc) 169 | cbar.set_label(r"velocity [$m/s$]", size=20) 170 | plt.axis('equal') 171 | plt.savefig(os.path.join(fig_prog_folder, str(i) + ".png"), dpi=200) 172 | 173 | plt.close('All') 174 | print("Planning uses {:.3f}.".format(t_total)) 175 | 176 | # Makes an animation. 177 | gif_path = os.path.join(config.OUT_FOLDER, 'rollout.gif') 178 | with imageio.get_writer(gif_path, mode='I', loop=0) as writer: 179 | for i in range(itr_receding): 180 | filename = os.path.join(fig_prog_folder, str(i) + ".png") 181 | image = imageio.imread(filename) 182 | writer.append_data(image) 183 | Image(open(gif_path, 'rb').read(), width=400) 184 | 185 | 186 | if __name__ == "__main__": 187 | parser = argparse.ArgumentParser() 188 | parser.add_argument( 189 | "-cf", "--config_file", help="config file path", type=str, 190 | default=os.path.join("", "example_racecar.yaml") 191 | ) 192 | args = parser.parse_args() 193 | main(args.config_file) 194 | -------------------------------------------------------------------------------- /example_racecar.yaml: -------------------------------------------------------------------------------- 1 | # VEHICLE: 2 | WIDTH: 0.2 3 | LENGTH: 0.5 4 | MASS : 2.99 # kg 5 | WHEELBASE: 0.257 6 | DIM_X: 4 7 | DIM_U: 2 8 | 9 | # TRACK 10 | TRACK_FILE: outerloop_center_smooth.csv 11 | TRACK_WIDTH_R: 0.3 12 | TRACK_WIDTH_L: 0.3 13 | TRACK_OFFSET: 0.15 14 | LOOP: False 15 | 16 | # CONSTRAINT PARAM 17 | Q1_V: 0. 18 | Q2_V: 0. 19 | Q1_ROAD: 2. 20 | Q2_ROAD: 5. 21 | Q1_LAT: 1. 22 | Q2_LAT: 5. 23 | Q1_OBS: 2. 24 | Q2_OBS: 5. 25 | BARRIER_THR: 20. 26 | EXP_UB: 50. 27 | 28 | # COST PARAM 29 | W_VEL: 4. 30 | W_CONTOUR: 30. 31 | W_THETA: 0.1 32 | W_ACCEL: 0.1 33 | W_DELTA: 0.1 34 | V_REF: 1. 35 | 36 | # CONSTRAINT 37 | V_MIN: 0. 38 | V_MAX: 1. 39 | A_MIN: -3.5 40 | A_MAX: 3.5 41 | DELTA_MIN: -0.4 42 | DELTA_MAX: 0.4 43 | ALAT_MIN: -5. 44 | ALAT_MAX: 5. 45 | 46 | # DISTURBANCE. 47 | SIGMA_X: 0. 48 | SIGMA_Y: 0. 49 | SIGMA_V: 0. 50 | SIGMA_THETA: 0. 51 | 52 | # SHIELDING 53 | COLL_CHECK_SLACK: 3.0 54 | 55 | # SOLVER: 56 | N: 31 57 | T: 4. 58 | MAX_ITER: 50 59 | MAX_ITER_RECEDING: 100 60 | OUT_FOLDER: experiments/ilqr_jax 61 | -------------------------------------------------------------------------------- /experiments/ilqr_jax/rollout.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/iLQR_jax_racing/bdc97019c1a3c79c46bd7166bb1be5662bb7268b/experiments/ilqr_jax/rollout.gif -------------------------------------------------------------------------------- /iLQR/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/iLQR_jax_racing/bdc97019c1a3c79c46bd7166bb1be5662bb7268b/iLQR/__init__.py -------------------------------------------------------------------------------- /iLQR/constraints.py: -------------------------------------------------------------------------------- 1 | """ 2 | Constraints. 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | Reference: ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 7 | """ 8 | 9 | import numpy as np 10 | from typing import List, Tuple 11 | 12 | from .ellipsoid_obj import EllipsoidObj 13 | 14 | 15 | class Constraints: 16 | 17 | def __init__(self, config): 18 | self.T = config.T # Planning Time Horizon 19 | self.N = config.N # number of planning steps 20 | self.dt = self.T / (self.N - 1) # time step for each planning step 21 | self.wheelbase = config.WHEELBASE # vehicle chassis length 22 | self.delta_min = config.DELTA_MIN # min steering angle rad 23 | self.delta_max = config.DELTA_MAX # max steering angle rad 24 | self.a_min = config.A_MIN # min longitudial accel 25 | self.a_max = config.A_MAX # max longitudial accel 26 | self.v_min = config.V_MIN # min velocity 27 | self.v_max = config.V_MAX # max velocity 28 | self.alat_min = config.ALAT_MIN # min lateral accel 29 | self.alat_max = config.ALAT_MAX # max lateral accel 30 | self.track_width_L = config.TRACK_WIDTH_L 31 | self.track_width_R = config.TRACK_WIDTH_R 32 | 33 | # System parameters. 34 | self.dim_x = config.DIM_X 35 | self.dim_u = config.DIM_U 36 | 37 | # Parameter for barrier functions. 38 | self.q1_v = config.Q1_V 39 | self.q2_v = config.Q2_V 40 | 41 | self.q1_road = config.Q1_ROAD 42 | self.q2_road = config.Q2_ROAD 43 | 44 | self.q1_lat = config.Q1_LAT 45 | self.q2_lat = config.Q2_LAT 46 | 47 | self.q1_obs = config.Q1_OBS 48 | self.q2_obs = config.Q2_OBS 49 | 50 | self.exp_ub = config.EXP_UB 51 | 52 | # Useful constants. 53 | self.zeros = np.zeros((self.N)) 54 | self.ones = np.ones((self.N)) 55 | 56 | self.gamma = 0.9 57 | ego_a = config.LENGTH / 2.0 58 | self.r = ego_b = config.WIDTH / 2.0 59 | wheelbase = config.WHEELBASE 60 | ego_q = np.array([wheelbase / 2, 0])[:, np.newaxis] 61 | ego_Q = np.diag([ego_a * ego_a, ego_b * ego_b]) 62 | self.ego = EllipsoidObj(q=ego_q, Q=ego_Q) 63 | self.ego_ell = None 64 | self.obs_list = None 65 | 66 | def update_obs(self, frs_list: List): 67 | """ 68 | Updates the obstacle list. 69 | 70 | Args: 71 | frs_list (List): obstacle list. 72 | """ 73 | self.obs_list = frs_list 74 | 75 | def state2ell(self, states: np.ndarray) -> List[EllipsoidObj]: 76 | """ 77 | Converts a state variable to an ellipsoid object. 78 | 79 | Args: 80 | states (np.ndarray): 4xN array of state. 81 | 82 | Returns: 83 | List[EllipsoidObj]: ellipsoid object. 84 | """ 85 | ego_ell = [] 86 | for i in range(self.N): 87 | theta = states[3, i] 88 | d = states[:2, i][:, np.newaxis] 89 | R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) 90 | temp = self.ego @ R 91 | temp.add(d) 92 | ego_ell.append(temp) 93 | return ego_ell 94 | 95 | def get_cost( 96 | self, states: np.ndarray, controls: np.ndarray, closest_pt: np.ndarray, slope: np.ndarray 97 | ) -> np.ndarray: 98 | """ 99 | Computes the total soft constraint cost. 100 | 101 | Args: 102 | states (np.ndarray): 4xN array of state. 103 | controls (np.ndarray): 2xN array of control input. 104 | closest_pts (np.ndarray): 2xN array of each state's closest point [x,y] 105 | on the track. 106 | slope (np.ndarray): 1xN array of track's slopes (rad) at closest 107 | points. 108 | 109 | Returns: 110 | np.ndarray: total soft constraint cost. 111 | """ 112 | 113 | # Road Boundary constarint. 114 | c_boundary = self._road_boundary_cost(states, closest_pt, slope) 115 | 116 | # Obstacle constraints. 117 | c_obs = np.zeros(self.N) 118 | if len(self.obs_list) > 0: 119 | # Patch footprint around state trajectory. 120 | self.ego_ell = self.state2ell(states) 121 | for i in range(self.N): 122 | ego_i = self.ego_ell[i] 123 | for obs_j in self.obs_list: # obs_j is a list of obstacles. 124 | obs_j_i = obs_j[i] # Get the ith obstacle in list obs_j. 125 | c_obs[i] += self.gamma**i * ego_i.obstacle_cost(obs_j_i, self.q1_obs, self.q2_obs) 126 | 127 | # Minimum velocity constarint. 128 | c_vel = self.q1_v * (np.exp(-states[2, :] * self.q2_v)) 129 | 130 | # Lateral Acceleration constraint. 131 | accel = states[2, :]**2 * np.tan(controls[1, :]) / self.wheelbase 132 | error_ub = accel - self.alat_max 133 | error_lb = self.alat_min - accel 134 | 135 | b_ub = self.q1_lat * (np.exp(np.clip(self.q2_lat * error_ub, None, self.exp_ub)) - 1) 136 | b_lb = self.q1_lat * (np.exp(np.clip(self.q2_lat * error_lb, None, self.exp_ub)) - 1) 137 | c_lat = b_lb + b_ub 138 | 139 | return c_vel + c_boundary + c_lat + c_obs 140 | 141 | def _road_boundary_cost( 142 | self, states: np.ndarray, closest_pt: np.ndarray, slope: np.ndarray 143 | ) -> np.ndarray: 144 | """ 145 | Computes the road boundary cost. 146 | 147 | Args: 148 | states (np.ndarray): 4xN array of state. 149 | closest_pt (np.ndarray): 2xN array of each state's closest point [x,y] 150 | on the track. 151 | slope (np.ndarray): 1xN array of track's slopes (rad) at closest 152 | points. 153 | 154 | Returns: 155 | np.ndarray: road boundary cost. 156 | """ 157 | dx = states[0, :] - closest_pt[0, :] 158 | dy = states[1, :] - closest_pt[1, :] 159 | 160 | sr = np.sin(slope) 161 | cr = np.cos(slope) 162 | dis = sr*dx - cr*dy 163 | # Right bound. 164 | b_r = dis - (self.track_width_R - self.r) 165 | 166 | c_r = self.q1_road * np.exp(np.clip(self.q2_road * b_r, -0.025 * self.q2_road, 20)) 167 | # Left Bound. 168 | b_l = -dis - (self.track_width_L - self.r) 169 | 170 | c_l = self.q1_road * np.exp(np.clip(self.q2_road * b_l, -0.025 * self.q2_road, 20)) 171 | 172 | return c_l + c_r 173 | 174 | def get_obs_derivatives(self, states: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: 175 | """ 176 | Calculates the Jacobian and Hessian of the obstacle avoidance soft 177 | constraint cost. 178 | 179 | Args: 180 | states (np.ndarray): 4xN array of state. 181 | 182 | Returns: 183 | np.ndarray: Jacobian vector. 184 | np.ndarray: Hessian matrix. 185 | """ 186 | c_x_obs = np.zeros((self.dim_x, self.N)) 187 | c_xx_obs = np.zeros((self.dim_x, self.dim_x, self.N)) 188 | 189 | if len(self.obs_list) > 0: 190 | for i in range(self.N): 191 | ego_i = self.ego_ell[i] 192 | for obs_j in self.obs_list: 193 | obs_j_i = obs_j[i] 194 | c_x_obs_temp, c_xx_obs_temp = ego_i.obstacle_derivative( 195 | states[:, i], self.wheelbase / 2, obs_j_i, self.q1_obs, self.q2_obs 196 | ) 197 | c_x_obs[:, i] += self.gamma**i * c_x_obs_temp 198 | c_xx_obs[:, :, i] += self.gamma**i * c_xx_obs_temp 199 | 200 | return c_x_obs, c_xx_obs 201 | -------------------------------------------------------------------------------- /iLQR/cost.py: -------------------------------------------------------------------------------- 1 | """ 2 | Costs, gradients, and Hessians. 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | Reference: ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 7 | """ 8 | 9 | import numpy as np 10 | from typing import Tuple, List 11 | 12 | from .constraints import Constraints 13 | 14 | from functools import partial 15 | from jax import jit, jacfwd, hessian 16 | from jaxlib.xla_extension import ArrayImpl 17 | import jax.numpy as jnp 18 | import jax 19 | 20 | 21 | class Cost: 22 | 23 | def __init__(self, config, safety=True): 24 | self.config = config 25 | self.soft_constraints = Constraints(config) 26 | self.safety = safety 27 | 28 | # Planning parameters. 29 | self.T = config.T # planning horizon. 30 | self.N = config.N # number of planning steps. 31 | self.dt = self.T / (self.N - 1) # time step for each planning step. 32 | self.v_ref = config.V_REF # reference velocity. 33 | 34 | # System parameters. 35 | self.dim_x = config.DIM_X 36 | self.dim_u = config.DIM_U 37 | 38 | # Track parameters. 39 | self.track_width_R = config.TRACK_WIDTH_R 40 | self.track_width_L = config.TRACK_WIDTH_L 41 | 42 | # Racing cost parameters. 43 | self.w_vel = config.W_VEL 44 | self.w_contour = config.W_CONTOUR 45 | self.w_theta = config.W_THETA 46 | self.w_accel = config.W_ACCEL 47 | self.w_delta = config.W_DELTA 48 | self.wheelbase = config.WHEELBASE 49 | self.track_offset = config.TRACK_OFFSET 50 | self.W_state = np.array([[self.w_contour, 0], [0, self.w_vel]]) 51 | self.W_control = np.array([[self.w_accel, 0], [0, self.w_delta]]) 52 | 53 | # Soft constraint parameters. 54 | self.q1_v = config.Q1_V 55 | self.q2_v = config.Q2_V 56 | self.q1_road = config.Q1_ROAD 57 | self.q2_road = config.Q2_ROAD 58 | self.q1_lat = config.Q1_LAT 59 | self.q2_lat = config.Q2_LAT 60 | self.q1_obs = config.Q1_OBS 61 | self.q2_obs = config.Q2_OBS 62 | self.barrier_thr = config.BARRIER_THR 63 | self.v_min = config.V_MIN 64 | self.v_max = config.V_MAX 65 | self.a_min = config.A_MIN 66 | self.a_max = config.A_MAX 67 | self.delta_min = config.DELTA_MIN 68 | self.delta_max = config.DELTA_MAX 69 | self.alat_min = config.ALAT_MIN 70 | self.alat_max = config.ALAT_MAX 71 | self.r = config.WIDTH / 2. 72 | self.road_thr = 0.025 73 | 74 | # Useful constant vectors. 75 | self.zeros = np.zeros((self.N)) 76 | self.ones = np.ones((self.N)) 77 | 78 | # Computes cost gradients using Jax. 79 | self.cx_x = jit(jacfwd(self.state_cost_stage_jitted, argnums=0)) 80 | self.cp_x = self.progress_deriv_jitted 81 | self.cu_u = jit(jacfwd(self.control_cost_stage_jitted, argnums=0)) 82 | self.rb_x = jit(jacfwd(self.road_boundary_cost_stage_jitted, argnums=0)) 83 | self.la_x = jit(jacfwd(self.lat_acc_cost_stage_jitted, argnums=0)) 84 | self.la_u_tmp = jit(jacfwd(self.lat_acc_cost_stage_jitted, argnums=1)) 85 | self.vb_x = jit(jacfwd(self.vel_bound_cost_stage_jitted, argnums=0)) 86 | 87 | # Vectorizes gradients using Jax. 88 | self.cx_x = jit(jax.vmap(self.cx_x, in_axes=(1, 1, 1), out_axes=(1))) 89 | self.cp_x = jit(jax.vmap(self.cp_x, in_axes=(1), out_axes=(1))) 90 | self.cu_u = jit(jax.vmap(self.cu_u, in_axes=(1), out_axes=(1))) 91 | self.rb_x = jit(jax.vmap(self.rb_x, in_axes=(1, 1, 1), out_axes=(1))) 92 | self.la_x = jit(jax.vmap(self.la_x, in_axes=(1, 1), out_axes=(1))) 93 | self.la_u = jit(jax.vmap(self.la_u_tmp, in_axes=(1, 1), out_axes=(1))) 94 | self.vb_x = jit(jax.vmap(self.vb_x, in_axes=(1), out_axes=(1))) 95 | 96 | # Computes cost Hessians and second-order derivatives using Jax. 97 | self.cx_xx = jit(hessian(self.state_cost_stage_jitted, argnums=0)) 98 | self.cu_uu = jit(hessian(self.control_cost_stage_jitted, argnums=0)) 99 | self.c_ux = jnp.zeros((self.dim_u, self.dim_x)) 100 | self.rb_xx = jit(hessian(self.road_boundary_cost_stage_jitted, argnums=0)) 101 | self.la_xx = jit(hessian(self.lat_acc_cost_stage_jitted, argnums=0)) 102 | self.la_uu = jit(hessian(self.lat_acc_cost_stage_jitted, argnums=1)) 103 | self.la_ux = jit(jacfwd(self.la_u_tmp, argnums=0)) 104 | self.vb_xx = jit(hessian(self.vel_bound_cost_stage_jitted, argnums=0)) 105 | 106 | # Vectorizes Hessians and second-order derivatives using Jax. 107 | self.cx_xx = jit(jax.vmap(self.cx_xx, in_axes=(1, 1, 1), out_axes=(2))) 108 | self.cu_uu = jit(jax.vmap(self.cu_uu, in_axes=(1), out_axes=(2))) 109 | self.rb_xx = jit(jax.vmap(self.rb_xx, in_axes=(1, 1, 1), out_axes=(2))) 110 | self.la_xx = jit(jax.vmap(self.la_xx, in_axes=(1, 1), out_axes=(2))) 111 | self.la_uu = jit(jax.vmap(self.la_uu, in_axes=(1, 1), out_axes=(2))) 112 | self.la_ux = jit(jax.vmap(self.la_ux, in_axes=(1, 1), out_axes=(2))) 113 | self.vb_xx = jit(jax.vmap(self.vb_xx, in_axes=(1), out_axes=(2))) 114 | 115 | # # Vectorizes soft constraint cost gradient and Hessians. 116 | # self.lat_acc_deriv_vmap = jit( 117 | # jax.vmap( 118 | # self.lat_acc_deriv_jitted, in_axes=(1, 1), 119 | # out_axes=(1, 2, 1, 2, 2) 120 | # ) 121 | # ) 122 | # self.road_boundary_deriv_vmap = jit( 123 | # jax.vmap( 124 | # self.road_boundary_deriv_jitted, in_axes=(1, 1, 1), 125 | # out_axes=(1, 2) 126 | # ) 127 | # ) 128 | # self.vel_bound_deriv_vmap = jit( 129 | # jax.vmap(self.vel_bound_deriv_jitted, in_axes=(1), out_axes=(1, 2)) 130 | # ) 131 | 132 | def update_obs(self, frs_list: List): 133 | """ 134 | Updates the obstacle list. 135 | 136 | Args: 137 | frs_list (List): obstacle list. 138 | """ 139 | self.soft_constraints.update_obs(frs_list) 140 | 141 | def get_cost( 142 | self, states: np.ndarray, controls: np.ndarray, closest_pts: np.ndarray, slope: np.ndarray, 143 | theta: np.ndarray 144 | ) -> np.ndarray: 145 | """ 146 | Calculates the cost given planned states and controls. 147 | 148 | Args: 149 | states (np.ndarray): 4xN array of planned trajectory. 150 | controls (np.ndarray): 2xN array of planned control. 151 | closest_pts (np.ndarray): 2xN array of each state's closest point [x,y] 152 | on the track. 153 | slope (np.ndarray): 1xN array of track's slopes (rad) at closest 154 | points. 155 | theta (np.ndarray): 1xN array of the progress at each state. 156 | 157 | Returns: 158 | np.ndarray: total cost. 159 | """ 160 | transform = np.array([[np.sin(slope), -np.cos(slope), self.zeros, self.zeros], 161 | [self.zeros, self.zeros, self.ones, self.zeros]]) 162 | 163 | ref_states = np.zeros_like(states) 164 | ref_states[0, :] = closest_pts[0, :] + np.sin(slope) * self.track_offset 165 | ref_states[1, :] = closest_pts[1, :] - np.cos(slope) * self.track_offset 166 | ref_states[2, :] = self.v_ref 167 | 168 | error = states - ref_states 169 | Q_trans = np.einsum( 170 | 'abn, bcn->acn', np.einsum('dan, ab -> dbn', transform.transpose(1, 0, 2), self.W_state), 171 | transform 172 | ) 173 | 174 | c_state = np.einsum('an, an->n', error, np.einsum('abn, bn->an', Q_trans, error)) 175 | c_progress = -self.w_theta * np.sum(theta) 176 | 177 | c_control = np.einsum('an, an->n', controls, np.einsum('ab, bn->an', self.W_control, controls)) 178 | 179 | c_control[-1] = 0 180 | 181 | c_constraint = self.soft_constraints.get_cost(states, controls, closest_pts, slope) 182 | 183 | if not self.safety: 184 | c_constraint = 0 185 | 186 | J = np.sum(c_state + c_constraint + c_control) + c_progress 187 | 188 | return J 189 | 190 | def get_derivatives_jax( 191 | self, states: ArrayImpl, controls: ArrayImpl, closest_pts: ArrayImpl, slopes: ArrayImpl 192 | ) -> Tuple[ArrayImpl, ArrayImpl, ArrayImpl, ArrayImpl, ArrayImpl]: 193 | """ 194 | Calculates Jacobian and Hessian of the overall cost using Jax. 195 | 196 | Args: 197 | states (ArrayImpl): current states of the shape (dim_x, N). 198 | controls (ArrayImpl): current controls of the shape (dim_u, N). 199 | closest_pts (ArrayImpl): each state's closest point [x,y] 200 | on the track (2, N). 201 | slopes (ArrayImpl): track's slopes (rad) at closest points (1, N). 202 | 203 | Returns: 204 | ArrayImpl: c_x of the shape (dim_x, N). 205 | ArrayImpl: c_xx of the shape (dim_x, dim_x, N). 206 | ArrayImpl: c_u of the shape (dim_u, N). 207 | ArrayImpl: c_uu of the shape (dim_u, dim_u, N). 208 | ArrayImpl: c_ux of the shape (dim_u, dim_x, N). 209 | """ 210 | # Obtains racing cost gradients and Hessians 211 | c_x_cost = self.cx_x(states, closest_pts, slopes) 212 | c_xx_cost = self.cx_xx(states, closest_pts, slopes) 213 | 214 | c_p_cost = self.cp_x(slopes) 215 | 216 | c_u_cost = self.cu_u(controls) 217 | c_uu_cost = self.cu_uu(controls) 218 | 219 | q = c_x_cost + c_p_cost 220 | Q = c_xx_cost 221 | r = c_u_cost 222 | R = c_uu_cost 223 | S = np.zeros((self.dim_u, self.dim_x, states.shape[1])) 224 | 225 | # Obtains soft constraint cost gradients and Hessians. 226 | # Analytical. 227 | if self.safety: 228 | c_x_obs, c_xx_obs = self.soft_constraints.get_obs_derivatives(np.asarray(states)) 229 | 230 | c_x_rb = self.rb_x(states, closest_pts, slopes) 231 | c_xx_rb = self.rb_xx(states, closest_pts, slopes) 232 | 233 | # Jax-autodiff'd. 234 | c_x_lat = self.la_x(states, controls) 235 | c_xx_lat = self.la_xx(states, controls) 236 | c_u_lat = self.la_u(states, controls) 237 | c_uu_lat = self.la_uu(states, controls) 238 | c_ux_lat = self.la_ux(states, controls) 239 | 240 | c_x_v = self.vb_x(states) 241 | c_xx_v = self.vb_xx(states) 242 | 243 | if self.safety: 244 | q += c_x_lat + c_x_v + c_x_rb + c_x_obs 245 | Q += c_xx_lat + c_xx_v + c_xx_rb + c_xx_obs 246 | else: 247 | q += c_x_lat + c_x_v + c_x_rb 248 | Q += c_xx_lat + c_xx_v + c_xx_rb 249 | r += c_u_lat 250 | R += c_uu_lat 251 | S += c_ux_lat 252 | 253 | return q, Q, r, R, S 254 | 255 | # --------------------------- Jitted racing costs ---------------------------- 256 | @partial(jit, static_argnums=(0,)) 257 | def state_cost_stage_jitted( 258 | self, state: ArrayImpl, closest_pt: ArrayImpl, slope: ArrayImpl 259 | ) -> ArrayImpl: 260 | """ 261 | Computes the stage state cost. 262 | 263 | Args: 264 | state (ArrayImpl): (dim_x,) 265 | closest_pt (ArrayImpl): (2,) 266 | slope (ArrayImpl): scalar 267 | 268 | Returns: 269 | ArrayImpl: cost (scalar) 270 | """ 271 | sr = jnp.sin(slope)[0] 272 | cr = jnp.cos(slope)[0] 273 | transform = jnp.array([[sr, -cr, 0., 0.], [0., 0., 1., 0.]]) 274 | Q = transform.T @ self.W_state @ transform 275 | ref_state = jnp.zeros((self.dim_x,)) 276 | ref_state = ref_state.at[0].set(closest_pt[0] + sr * self.track_offset) 277 | ref_state = ref_state.at[1].set(closest_pt[1] - cr * self.track_offset) 278 | ref_state = ref_state.at[2].set(self.v_ref) 279 | return (state - ref_state).T @ Q @ (state-ref_state) 280 | 281 | @partial(jit, static_argnums=(0,)) 282 | def control_cost_stage_jitted(self, control: ArrayImpl) -> ArrayImpl: 283 | """ 284 | Computes the stage control cost c(u) = u.T @ R @ u, where u is the control. 285 | 286 | Args: 287 | control (ArrayImpl): (dim_u,) 288 | 289 | Returns: 290 | ArrayImpl: cost (scalar) 291 | """ 292 | return control.T @ self.W_control @ control 293 | 294 | @partial(jit, static_argnums=(0,)) 295 | def progress_deriv_jitted(self, slope: ArrayImpl) -> ArrayImpl: 296 | """ 297 | Computes the derivative of the progress cost w.r.t. the slope. 298 | 299 | Args: 300 | slope (ArrayImpl): scalar 301 | 302 | Returns: 303 | ArrayImpl: (dim_x,) 304 | """ 305 | sr = jnp.sin(slope)[0] 306 | cr = jnp.cos(slope)[0] 307 | return -self.w_theta * jnp.hstack((cr, sr, 0, 0)) 308 | 309 | # ----------------------- Jitted soft constraint costs ----------------------- 310 | @partial(jit, static_argnums=(0,)) 311 | def lat_acc_cost_stage_jitted(self, state: ArrayImpl, control: ArrayImpl) -> ArrayImpl: 312 | ''' 313 | Calculates the lateral acceleration soft constraint cost. 314 | 315 | Args: 316 | state (ArrayImpl): (dim_x,) 317 | control (ArrayImpl): (dim_u,) 318 | 319 | Returns: 320 | ArrayImpl: dtype=float32 321 | ''' 322 | accel = state[2]**2 * jnp.tan(control[1]) / self.wheelbase 323 | error_ub = accel - self.alat_max 324 | error_lb = self.alat_min - accel 325 | 326 | c_lat_ub = self.q1_lat * (jnp.exp(self.q2_lat * error_ub) - 1.) 327 | c_lat_lb = self.q1_lat * (jnp.exp(self.q2_lat * error_lb) - 1.) 328 | return c_lat_lb + c_lat_ub 329 | 330 | @partial(jit, static_argnums=(0,)) 331 | def road_boundary_cost_stage_jitted( 332 | self, state: ArrayImpl, closest_pt: ArrayImpl, slope: ArrayImpl 333 | ) -> ArrayImpl: 334 | """ 335 | Calculates the road boundary soft constraint cost. 336 | 337 | Args: 338 | state (ArrayImpl): (dim_x,) 339 | closest_pt (ArrayImpl): (2,) 340 | slope (ArrayImpl): (1,) 341 | 342 | Returns: 343 | ArrayImpl: dtype=float32 344 | """ 345 | sr = jnp.sin(slope) 346 | cr = jnp.cos(slope) 347 | dx = state[0] - closest_pt[0] 348 | dy = state[1] - closest_pt[1] 349 | dis = (sr*dx - cr*dy)[0] 350 | 351 | _min_val = -self.road_thr * self.q2_road 352 | _max_val = self.barrier_thr 353 | 354 | # Right bound. 355 | b_r = dis - (self.track_width_R - self.r) 356 | c_r = (self.q1_road * jnp.exp(jnp.clip(self.q2_road * b_r, _min_val, _max_val))) 357 | 358 | # Left bound. 359 | b_l = -dis - (self.track_width_L - self.r) 360 | c_l = (self.q1_road * jnp.exp(jnp.clip(self.q2_road * b_l, _min_val, _max_val))) 361 | return c_l + c_r 362 | 363 | @partial(jit, static_argnums=(0,)) 364 | def vel_bound_cost_stage_jitted(self, state: ArrayImpl) -> ArrayImpl: 365 | """ 366 | Calculates the velocity bound soft constraint cost. 367 | 368 | Args: 369 | state (ArrayImpl): (dim_x,) 370 | 371 | Returns: 372 | ArrayImpl: dtype=float32 373 | """ 374 | cons_v_min = self.v_min - state[2] 375 | cons_v_max = state[2] - self.v_max 376 | barrier_v_min = self.q1_v * jnp.exp(jnp.clip(self.q2_v * cons_v_min, None, self.barrier_thr)) 377 | barrier_v_max = self.q1_v * jnp.exp(jnp.clip(self.q2_v * cons_v_max, None, self.barrier_thr)) 378 | return barrier_v_min + barrier_v_max 379 | 380 | # ----------- Jitted analytical derivatives (for comparison only) ------------ 381 | @partial(jit, static_argnums=(0,)) 382 | def lat_acc_deriv_jitted( 383 | self, state: ArrayImpl, control: ArrayImpl 384 | ) -> Tuple[ArrayImpl, ArrayImpl, ArrayImpl, ArrayImpl, ArrayImpl]: 385 | """ 386 | Calculates the Jacobian and Hessian of lateral acc. soft constraint cost. 387 | 388 | Args: 389 | state (ArrayImpl): of the shape (dim_x,). 390 | control (ArrayImpl): of the shape (dim_u,). 391 | 392 | Returns: 393 | ArrayImpl: c_x of the shape (dim_x,). 394 | ArrayImpl: c_xx of the shape (dim_x, dim_x). 395 | ArrayImpl: c_u of the shape (dim_u,). 396 | ArrayImpl: c_uu of the shape (dim_u, dim_u). 397 | ArrayImpl: c_ux of the shape (dim_u, dim_x). 398 | """ 399 | dim_x = self.dim_x 400 | wheelbase = self.wheelbase 401 | q1_lat = self.q1_lat 402 | q2_lat = self.q2_lat 403 | barrier_thr = self.barrier_thr 404 | 405 | c_x = jnp.zeros((dim_x,)) 406 | c_xx = jnp.zeros((dim_x, dim_x)) 407 | c_u = jnp.zeros((2,)) 408 | c_uu = jnp.zeros((2, 2)) 409 | c_ux = jnp.zeros((2, dim_x)) 410 | 411 | accel = state[2]**2 * jnp.tan(control[1]) / wheelbase 412 | error_ub = accel - self.alat_max 413 | error_lb = self.alat_min - accel 414 | 415 | cost_a_lat_min = q1_lat * jnp.exp(jnp.clip(q2_lat * error_lb, None, barrier_thr)) 416 | cost_a_lat_max = q1_lat * jnp.exp(jnp.clip(q2_lat * error_ub, None, barrier_thr)) 417 | 418 | da_dx = 2 * state[2] * jnp.tan(control[1]) / wheelbase 419 | da_dxx = 2 * jnp.tan(control[1]) / wheelbase 420 | 421 | da_du = state[2]**2 / (jnp.cos(control[1])**2 * wheelbase) 422 | da_duu = (state[2]**2 * jnp.sin(control[1]) / (jnp.cos(control[1])**3 * wheelbase)) 423 | 424 | da_dux = 2 * state[2] / (jnp.cos(control[1])**2 * wheelbase) 425 | 426 | c_x = c_x.at[2].set(q2_lat * (cost_a_lat_max-cost_a_lat_min) * da_dx) 427 | c_u = c_u.at[1].set(q2_lat * (cost_a_lat_max-cost_a_lat_min) * da_du) 428 | 429 | c_xx = c_xx.at[2, 2].set( 430 | q2_lat**2 * (cost_a_lat_max+cost_a_lat_min) * da_dx**2 431 | + q2_lat * (cost_a_lat_max-cost_a_lat_min) * da_dxx 432 | ) 433 | c_uu = c_uu.at[1, 1].set( 434 | q2_lat**2 * (cost_a_lat_max+cost_a_lat_min) * da_du**2 435 | + q2_lat * (cost_a_lat_max-cost_a_lat_min) * da_duu 436 | ) 437 | 438 | c_ux = c_ux.at[1, 2].set( 439 | q2_lat**2 * (cost_a_lat_max+cost_a_lat_min) * da_dx * da_du 440 | + q2_lat * (cost_a_lat_max-cost_a_lat_min) * da_dux 441 | ) 442 | return c_x, c_xx, c_u, c_uu, c_ux 443 | 444 | @partial(jit, static_argnums=(0,)) 445 | def road_boundary_deriv_jitted(self, state: ArrayImpl, closest_pt: ArrayImpl, 446 | slope: ArrayImpl) -> Tuple[ArrayImpl, ArrayImpl]: 447 | """ 448 | Calculates the Jacobian and Hessian of road boundary soft constraint cost. 449 | 450 | Args: 451 | state (ArrayImpl): (dim_x,) 452 | closest_pt (ArrayImpl): (2,) 453 | slope (ArrayImpl): (1,) 454 | 455 | Returns: 456 | ArrayImpl: (dim_x,) 457 | ArrayImpl: (dim_x, dim_x) 458 | """ 459 | dim_x = self.dim_x 460 | sr = jnp.sin(slope) 461 | cr = jnp.cos(slope) 462 | dx = state[0] - closest_pt[0] 463 | dy = state[1] - closest_pt[1] 464 | dis = sr*dx - cr*dy 465 | transform = jnp.array([sr, -cr]) 466 | 467 | cons_road_r = dis - (self.track_width_R - self.r) 468 | c_x_r = jnp.zeros((dim_x,)) 469 | c_xx_r = jnp.zeros((dim_x, dim_x)) 470 | _c_x_r, _c_xx_r = barrier_function_jitted( 471 | q1=self.q1_road, q2=self.q2_road, cons=cons_road_r, cons_dot=transform, cons_min=None, 472 | cons_max=self.barrier_thr 473 | ) 474 | c_x_r = c_x_r.at[:2].set(jnp.ravel(_c_x_r)) 475 | c_xx_r = c_xx_r.at[:2, :2].set(_c_xx_r) 476 | 477 | cons_road_l = -dis - (self.track_width_L - self.r) 478 | c_x_l = jnp.zeros((dim_x,)) 479 | c_xx_l = jnp.zeros((dim_x, dim_x)) 480 | _c_x_l, _c_xx_l = barrier_function_jitted( 481 | q1=self.q1_road, q2=self.q2_road, cons=cons_road_l, cons_dot=-transform, cons_min=None, 482 | cons_max=self.barrier_thr 483 | ) 484 | c_x_l = c_x_l.at[:2].set(jnp.ravel(_c_x_l)) 485 | c_xx_l = c_xx_l.at[:2, :2].set(_c_xx_l) 486 | 487 | return c_x_r + c_x_l, c_xx_r + c_xx_l 488 | 489 | @partial(jit, static_argnums=(0,)) 490 | def vel_bound_deriv_jitted(self, state: ArrayImpl) -> Tuple[ArrayImpl, ArrayImpl]: 491 | """ 492 | Calculates the Jacobian and Hessian of velocity soft constraint cost. 493 | 494 | Args: 495 | state (ArrayImpl): (dim_x,) 496 | 497 | Returns: 498 | ArrayImpl: (dim_x,) 499 | ArrayImpl: (dim_x, dim_x) 500 | """ 501 | dim_x = self.dim_x 502 | transform = 1. 503 | c_x = jnp.zeros((dim_x,)) 504 | c_xx = jnp.zeros((dim_x, dim_x)) 505 | 506 | cons_v_min = self.v_min - state[2] 507 | cons_v_max = state[2] - self.v_max 508 | 509 | _c_x_min, _c_xx_min = barrier_function_jitted( 510 | self.q1_v, self.q2_v, cons_v_min, -transform, cons_max=self.barrier_thr 511 | ) 512 | _c_x_max, _c_xx_max = barrier_function_jitted( 513 | self.q1_v, self.q2_v, cons_v_max, transform, cons_max=self.barrier_thr 514 | ) 515 | c_x = c_x.at[2].set(_c_x_min + _c_x_max) 516 | c_xx = c_xx.at[2, 2].set((_c_xx_min + _c_xx_max)[0, 0]) 517 | 518 | return c_x, c_xx 519 | 520 | 521 | @jit 522 | def barrier_function_jitted( 523 | q1: ArrayImpl, q2: ArrayImpl, cons: ArrayImpl, cons_dot: ArrayImpl, cons_min: ArrayImpl = None, 524 | cons_max: ArrayImpl = None 525 | ) -> Tuple[ArrayImpl, ArrayImpl]: 526 | """ 527 | Computes the barrier function with clipping. 528 | 529 | Args: 530 | q1 (ArrayImpl): scalar 531 | q2 (ArrayImpl): scalar 532 | cons (ArrayImpl): scalar 533 | cons_dot (ArrayImpl): N-by-1 534 | cons_min (ArrayImpl): scalar 535 | cons_max (ArrayImpl): scalar 536 | 537 | Returns: 538 | ArrayImpl: N-by-1 539 | ArrayImpl: N-by-N 540 | """ 541 | tmp = jnp.clip(q2 * cons, cons_min, cons_max) 542 | b = q1 * (jnp.exp(tmp)) 543 | 544 | b_dot = q2 * b * cons_dot 545 | b_ddot = (q2**2) * b * jnp.outer(cons_dot, cons_dot) 546 | 547 | return b_dot, b_ddot 548 | -------------------------------------------------------------------------------- /iLQR/dynamics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Dynamics. 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | Reference: ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 7 | TODOs: 8 | - Make state & ctrl indices global, static variables, e.g. self._px_idx = 0 9 | """ 10 | 11 | from typing import Optional, Tuple 12 | import numpy as np 13 | 14 | from functools import partial 15 | from jax import jit, jacfwd 16 | from jaxlib.xla_extension import ArrayImpl 17 | import jax.numpy as jnp 18 | import jax 19 | 20 | 21 | class Dynamics: 22 | 23 | def __init__(self, config): 24 | self.dim_x = 4 25 | self.dim_u = 2 26 | 27 | self.T = config.T # Planning Time Horizon 28 | self.N = config.N # number of planning steps 29 | self.dt = self.T / (self.N - 1) # time step for each planning step 30 | 31 | self.wheelbase = config.WHEELBASE # vehicle chassis length 32 | self.delta_min = config.DELTA_MIN # min steering angle rad 33 | self.delta_max = config.DELTA_MAX # max steering angle rad 34 | self.a_min = config.A_MIN # min longitudial accel 35 | self.a_max = config.A_MAX # max longitudial accel 36 | self.v_min = config.V_MIN # min velocity 37 | self.v_max = config.V_MAX # max velocity 38 | 39 | # Useful constants. 40 | self.zeros = np.zeros((self.N)) 41 | self.ones = np.ones((self.N)) 42 | 43 | # Computes Jacobian matrices using Jax. 44 | self.jac_f = jit(jacfwd(self.dct_time_dyn, argnums=[0, 1])) 45 | 46 | # Vectorizes Jacobians using Jax. 47 | self.jac_f = jit(jax.vmap(self.jac_f, in_axes=(1, 1), out_axes=(2, 2))) 48 | 49 | def forward_step( 50 | self, state: np.ndarray, control: np.ndarray, step: Optional[int] = 1, 51 | noise: Optional[np.ndarray] = None, noise_type: Optional[str] = 'unif' 52 | ) -> Tuple[np.ndarray, np.ndarray]: 53 | """ 54 | Finds the next state of the vehicle given the current state and 55 | control input state. 56 | 57 | Args: 58 | state (np.ndarray): (4, ) array [X, Y, V, psi]. 59 | control (np.ndarray): (2, ) array [a, delta]. 60 | step (int, optional): The number of segements to forward the 61 | dynamics. Defaults to 1. 62 | noise (np.ndarray, optional): The ball radius or standard 63 | deviation of the Gaussian noise. The magnitude should be in the 64 | sense of self.dt. Defaults to None. 65 | noise_type(str, optional): Uniform or Gaussian. Defaults to 'unif'. 66 | 67 | Returns: 68 | np.ndarray: next state. 69 | np.ndarray: clipped control. 70 | """ 71 | # Clips the controller values between min and max accel and steer values. 72 | accel = np.clip(control[0], self.a_min, self.a_max) 73 | delta = np.clip(control[1], self.delta_min, self.delta_max) 74 | control_clip = np.array([accel, delta]) 75 | next_state = state 76 | dt_step = self.dt / step 77 | 78 | for _ in range(step): 79 | # State: [x, y, v, psi] 80 | d_x = ((next_state[2] * dt_step + 0.5 * accel * dt_step**2) * np.cos(next_state[3])) 81 | d_y = ((next_state[2] * dt_step + 0.5 * accel * dt_step**2) * np.sin(next_state[3])) 82 | d_v = accel * dt_step 83 | d_psi = ((next_state[2] * dt_step + 0.5 * accel * dt_step**2) * np.tan(delta) 84 | / self.wheelbase) 85 | next_state = next_state + np.array([d_x, d_y, d_v, d_psi]) 86 | if noise is not None: 87 | T = np.array([[np.cos(next_state[-1]), np.sin(next_state[-1]), 0, 0], 88 | [-np.sin(next_state[-1]), 89 | np.cos(next_state[-1]), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) 90 | if noise_type == 'unif': 91 | rv = np.random.rand(4) - 0.5 92 | else: 93 | rv = np.random.normal(size=(4)) 94 | next_state = next_state + (T@noise) * rv / step 95 | 96 | # Clip the velocity. 97 | next_state[2] = np.clip(next_state[2], 0, None) 98 | 99 | return next_state, control_clip 100 | 101 | # ---------------------------- Jitted functions ------------------------------ 102 | @partial(jit, static_argnums=(0,)) 103 | def dct_time_dyn(self, state: ArrayImpl, control: ArrayImpl) -> ArrayImpl: 104 | """ 105 | Computes the one-step time evolution of the system. 106 | 107 | Args: 108 | state (ArrayImpl): (4,) jnp array [x, y, v, psi]. 109 | control (ArrayImpl): (2,) jnp array [a, delta]. 110 | 111 | Returns: 112 | ArrayImpl: next state. 113 | """ 114 | d_x = ((state[2] * self.dt + 0.5 * control[0] * self.dt**2) * jnp.cos(state[3])) 115 | d_y = ((state[2] * self.dt + 0.5 * control[0] * self.dt**2) * jnp.sin(state[3])) 116 | d_v = control[0] * self.dt 117 | d_psi = ((state[2] * self.dt + 0.5 * control[0] * self.dt**2) * jnp.tan(control[1]) 118 | / self.wheelbase) 119 | return state + jnp.hstack((d_x, d_y, d_v, d_psi)) 120 | 121 | @partial(jit, static_argnums=(0,)) 122 | def integrate_forward_jax(self, state: ArrayImpl, 123 | control: ArrayImpl) -> Tuple[ArrayImpl, ArrayImpl]: 124 | """ 125 | Computes the next state. 126 | 127 | Args: 128 | state (ArrayImpl): (4,) jnp array [x, y, v, psi]. 129 | control (ArrayImpl): (2,) jnp array [a, delta]. 130 | 131 | Returns: 132 | state_next: ArrayImpl 133 | control_next: ArrayImpl 134 | """ 135 | # Clips the control values with their limits. 136 | accel = jnp.clip(control[0], self.a_min, self.a_max) 137 | delta = jnp.clip(control[1], self.delta_min, self.delta_max) 138 | 139 | # Integrates the system one-step forward in time using the Euler method. 140 | control_clip = jnp.hstack((accel, delta)) 141 | state_next = self.dct_time_dyn(state, control_clip) 142 | 143 | return state_next, control_clip 144 | 145 | @partial(jit, static_argnums=(0,)) 146 | def get_AB_matrix_jax(self, nominal_states: ArrayImpl, 147 | nominal_controls: ArrayImpl) -> Tuple[ArrayImpl, ArrayImpl]: 148 | """ 149 | Returns the linearized 'A' and 'B' matrix of the ego vehicle around 150 | nominal states and controls. 151 | 152 | Args: 153 | nominal_states (ArrayImpl): (nx, N) states along the nominal traj. 154 | nominal_controls (ArrayImpl): (nu, N) controls along the traj. 155 | 156 | Returns: 157 | ArrayImpl: the Jacobian of next state w.r.t. the current state. 158 | ArrayImpl: the Jacobian of next state w.r.t. the current control. 159 | """ 160 | A, B = self.jac_f(nominal_states, nominal_controls) 161 | return A, B 162 | -------------------------------------------------------------------------------- /iLQR/ellipsoid_obj.py: -------------------------------------------------------------------------------- 1 | """ 2 | Ellipsoid object for collision avoidance. 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | Reference: 7 | - ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 8 | - Ellipsoidal Toolbox (Alex Kurzhanskiy) 9 | - ellReach (Python) Toolbox (Haimin Hu) 10 | """ 11 | 12 | from __future__ import annotations 13 | from typing import Tuple, List 14 | import numpy as np 15 | import matplotlib 16 | 17 | 18 | class EllipsoidObj(): 19 | 20 | def __init__( 21 | self, q: np.ndarray = np.array([]), Q: np.ndarray = np.array([[], []]), 22 | r=None, n_circ=None, center_L=None, major_axis=None 23 | ): 24 | 25 | def symmetricize(Q): 26 | if Q.shape[1] == 0: 27 | return Q 28 | else: 29 | return (Q + Q.T) / 2 30 | 31 | self.q = q 32 | self.Q = symmetricize(Q) 33 | self.Qinv = np.linalg.inv(self.Q) 34 | 35 | if q.shape[0] != 2: 36 | raise ValueError( 37 | "The dimension should be 2 but get {}!".format(q.shape[0]) 38 | ) 39 | 40 | # Assume 2d ellipsoid。 41 | if r is None: 42 | # print("use eigval") 43 | eigVal, eigVec = np.linalg.eig(Q) 44 | eigVal = np.sqrt(eigVal) 45 | if eigVal[0] > eigVal[1]: 46 | a = eigVal[0] 47 | b = eigVal[1] 48 | self.major_axis = eigVec[:, 0] 49 | else: 50 | a = eigVal[1] 51 | b = eigVal[0] 52 | self.major_axis = eigVec[:, 1] 53 | self.r = b # semi-minor axis 54 | self.n_circ = int(np.ceil(a / b)) #circles to cover the ellipsoid. 55 | # Assume local coordinate's origin is at the center of ellipsoid, 56 | # and x is align with major axis. Locations of circle centers are on 57 | # the local coordinate. 58 | self.center_L = np.linspace(-a + self.r, a - self.r, self.n_circ) 59 | else: 60 | self.r = r 61 | self.n_circ = n_circ 62 | self.center_L = center_L 63 | self.major_axis = major_axis 64 | 65 | # position of circle center on global coordinates 66 | self.center = np.einsum('n,a->an', self.center_L, self.major_axis) + self.q 67 | 68 | self.closest_pt = None 69 | self.slope = None 70 | self.width_L = None 71 | self.width_R = None 72 | 73 | def copy(self) -> EllipsoidObj: 74 | """ 75 | Creats an identical ellipsoid object. 76 | 77 | Returns: 78 | EllipsoidObj: an ellipsoid object. 79 | """ 80 | return EllipsoidObj(q=self.q, Q=self.Q) 81 | 82 | def is_degenerate(self) -> bool: 83 | """ 84 | Checks if the ellipsoid is degenerate. 85 | Returns True if the ellipsoid is degenerate, False - otherwise. 86 | 87 | Returns: 88 | bool: flag indicating if the ellipsoid is degenerate. 89 | """ 90 | return self.rank() < self.dim() 91 | 92 | def is_empty(self) -> bool: 93 | """ 94 | Checks if the ellipsoid object is empty. 95 | Returns True if the ellipsoid is empty, False - otherwise. 96 | 97 | Returns: 98 | bool: flag indicating if the ellipsoid is empty. 99 | """ 100 | return self.dim() == 0 101 | 102 | def dim(self) -> int: 103 | """ 104 | Returns the dimension of the ellipsoid. 105 | 106 | Returns: 107 | int: dimension of the ellipsoid. 108 | """ 109 | return self.q.size 110 | 111 | def rank(self) -> int: 112 | """ 113 | Returns the rank of the shape matrix. 114 | 115 | Returns: 116 | int: rank of the shape matrix. 117 | """ 118 | if self.dim() == 0: 119 | return 0 120 | else: 121 | return np.linalg.matrix_rank(self.Q) 122 | 123 | def parameters(self) -> Tuple[np.ndarray, np.ndarray]: 124 | """ 125 | Returns parameters of the ellipsoid. 126 | 127 | Returns: 128 | np.ndarray: center of the ellipsoid. 129 | np.ndarray: shape matrix of the ellipsoid. 130 | """ 131 | return self.q, self.Q 132 | 133 | def rho(self, L: np.ndarray, 134 | eps: float = 1e-10) -> Tuple[np.ndarray, np.ndarray]: 135 | """ 136 | Computes the support function of the ellipsoid in directions specified by 137 | the columns of matrix L, and boundary points X of this ellipsoid that 138 | correspond to directions in L. 139 | 140 | Args: 141 | L (np.ndarray): a direction matrix. 142 | eps (float): tolerance number. 143 | 144 | Returns: 145 | res (np.ndarray): the values of the support function for the specified 146 | ellipsoid and directions L. 147 | x (np.ndarray): boundary points of the ellipsoid that correspond to 148 | directions in L. 149 | """ 150 | d = L.shape[1] 151 | res = np.array([]).reshape((1, 0)) 152 | x = np.array([]).reshape((self.dim(), 0)) 153 | for i in range(d): 154 | l = L[:, i].reshape(self.dim(), 1) 155 | sr = np.maximum(np.sqrt(l.T @ self.Q @ l)[0, 0], eps) 156 | res = np.hstack((res, self.q.T @ l + sr)) 157 | x = np.hstack((x, self.Q @ l / sr + self.q)) 158 | return res, x 159 | 160 | def __matmul__(self, A: np.ndarray) -> EllipsoidObj: 161 | """ 162 | Multiplies the ellipsoid with a matrix or a scalar. 163 | If E(q,Q) is an ellipsoid, and A - matrix of suitable dimensions, then 164 | E(q, Q) @ A = E(Aq, AQA'). 165 | 166 | Args: 167 | A (np.ndarray): a matrix. 168 | 169 | Returns: 170 | EllipsoidObj: an ellipsoid object. 171 | """ 172 | q = A @ self.q 173 | Q = A @ self.Q @ A.T 174 | new_major_axis = A @ self.major_axis 175 | return EllipsoidObj( 176 | q=q, Q=Q, r=self.r, n_circ=self.n_circ, center_L=self.center_L, 177 | major_axis=new_major_axis 178 | ) 179 | 180 | def __add__(self, b: np.ndarray) -> EllipsoidObj: 181 | """ 182 | Computes vector addition with an ellipsoid: 183 | E + b = E(q + b, Q). 184 | 185 | Args: 186 | b (np.ndarray): a vector. 187 | """ 188 | if b.shape != self.q.shape: 189 | raise ValueError("Dimensions of b and q of the ellipsoid do not match.") 190 | return EllipsoidObj( 191 | q=self.q + b, Q=self.Q, r=self.r, n_circ=self.n_circ, 192 | center_L=self.center_L, major_axis=self.major_axis 193 | ) 194 | 195 | def add(self, b: np.ndarray): 196 | """ 197 | Computes vector addition with an ellipsoid: 198 | E + b = E(q + b, Q). 199 | 200 | Args: 201 | b (np.ndarray): a vector. 202 | """ 203 | self.q = self.q + b 204 | self.center = self.center + b 205 | 206 | def __sub__(self, b: np.ndarray) -> EllipsoidObj: 207 | """ 208 | Computes vector subtraction from an ellipsoid: 209 | E - b = E(q - b, Q). 210 | 211 | Args: 212 | b (np.ndarray): a vector. 213 | """ 214 | if b.shape != self.q.shape: 215 | raise ValueError("Dimensions of b and q of the ellipsoid do not match.") 216 | return EllipsoidObj( 217 | q=self.q - b, Q=self.Q, r=self.r, n_circ=self.n_circ, 218 | center_L=self.center_L, major_axis=self.major_axis 219 | ) 220 | 221 | def plot_circ(self, ax: matplotlib.axes.Axes): 222 | """ 223 | Plots a circle. 224 | 225 | Args: 226 | ax (matplotlib.axes.Axes): matplotlib axe object. 227 | """ 228 | theta = np.linspace(0, 2 * np.pi, 100) 229 | x0 = self.r * np.cos(theta) 230 | y0 = self.r * np.sin(theta) 231 | for i in range(self.n_circ): 232 | x = self.center[0, i] + x0 233 | y = self.center[1, i] + y0 234 | ax.plot(x, y) 235 | 236 | def projection(self, dims: List) -> EllipsoidObj: 237 | """ 238 | Computes projection of the ellipsoid onto the given dimensions. 239 | 240 | Args: 241 | dims (List): dimension to be preserved. 242 | 243 | Returns: 244 | EllipsoidObj: projected ellipsoid (of lower dimension). 245 | """ 246 | if not isinstance(dims, list): 247 | raise ValueError("Dims must be a list.") 248 | n = self.dim() 249 | m = len(dims) 250 | B = np.zeros((n, m)) 251 | for i in range(m): 252 | B[dims[i], i] = 1 253 | return self @ B.T 254 | 255 | def is_internal( 256 | self, x: np.ndarray, abs_tol: float = 1e-7, slack: float = 0. 257 | ) -> bool: 258 | """ 259 | Checks if the given point belongs to the ellipsoid, i.e. if inequality 260 | <(x - q), Q^(-1)(x - q)> <= 1 261 | holds. 262 | 263 | Args: 264 | x (np.ndarray): a column vector with the same dimension as the 265 | ellipsoid object. 266 | abs_tol (float, optional): tolerance number. Defaults to 1e-7. 267 | slack (float, optional): slack variable. Defaults to 0.. 268 | 269 | Returns: 270 | bool: flag indicating if the given point belongs to the ellipsoid. 271 | """ 272 | q = x - self.q 273 | r = (q.T @ self.Qinv @ q)[0, 0] 274 | if r < 1 + slack or np.abs(r - 1 + slack) < abs_tol: 275 | return True 276 | else: 277 | return False 278 | 279 | def plot( 280 | self, ax: matplotlib.axes.Axes, color: str = 'r', dims: List = None, 281 | N: int = 200, plot_center: bool = True 282 | ): 283 | """ 284 | Plots the ellipsoid object in 1D, 2D or 3D. 285 | 286 | Args: 287 | ax (matplotlib.axes.Axes): matplotlib axe object. 288 | color (str): color scheme. 289 | dims (List): dimension to be preserved. 290 | N (int): number of boundary points. 291 | plot_center (bool): flag indicating if to plot the ellipsoid center. 292 | """ 293 | if dims: 294 | E = self.projection(dims) 295 | else: 296 | E = self.copy() 297 | if E.dim() > 3: 298 | raise ValueError("Ellipsoid dimension can be 1, 2 or 3.") 299 | if E.dim() == 1: 300 | x = np.array([(E.q - np.sqrt(E.Q))[0, 0], (E.q + np.sqrt(E.Q))[0, 0]]) 301 | ax.plot(x, np.zeros_like(x), color) 302 | ax.plot(E.q, 0, color + '*') 303 | if E.dim() == 2: 304 | phi = np.array(np.linspace(0., 2. * np.pi, N)) 305 | L = np.concatenate( 306 | (np.cos(phi).reshape(1, N), np.sin(phi).reshape(1, N)), axis=0 307 | ) 308 | _, x = E.rho(L) 309 | ax.plot(x[0, :], x[1, :], color) 310 | if plot_center: 311 | ax.plot(E.q[0, 0], E.q[1, 0], color + '.') 312 | ax.set_aspect('equal') 313 | if E.dim() == 3: 314 | raise NotImplementedError 315 | 316 | def obstacle_cost(self, obs: EllipsoidObj, q1: float, q2: float) -> float: 317 | """ 318 | Computes the barrier cost given an obstacle and hyperparameters. 319 | 320 | Args: 321 | obs (EllipsoidObj): obstacle. 322 | q1 (float): shape parameter. 323 | q2 (float): scale parameter. 324 | 325 | Returns: 326 | float: barrier cost. 327 | """ 328 | # Broadcasts to (2, self.n_circ, obs.n_circ). 329 | d = (self.center.T[:, np.newaxis, :] - obs.center.T).T 330 | # Reshapes to (2, self.n_circ*obs.n_circ). 331 | d = np.reshape(d, (2, self.n_circ * obs.n_circ), "F") 332 | # Ignores and sets to -0.2 when self is far away from the obstacle. 333 | g = np.clip(self.r + obs.r - np.linalg.norm(d, axis=0), -0.2, None) 334 | b = np.sum(q1 * np.exp(q2 * g)) # barrier 335 | return b 336 | 337 | def obstacle_derivative( 338 | self, state: np.ndarray, center_L: float, obs: EllipsoidObj, q1: float, 339 | q2: float 340 | ) -> Tuple(np.ndarray, np.ndarray): 341 | """ 342 | Computes ellipsoidal obstacle soft constraint cost Jacobian and Hessian. 343 | 344 | Args: 345 | state (np.ndarray): nominal state trajectory, of the shape 346 | (x_dim, N). 347 | center_L (float): displacement of vehicle origin along the major 348 | axis of ellipsoid. 349 | obs (EllipsoidObj): obstacle (or its FRS) represented by ellipsoid. 350 | q1 (float): barrier function parameters. 351 | q2 (float): barrier function parameters. 352 | 353 | Returns: 354 | np.ndarray: Jacobian of softconstraint of shape (x_dim, N). 355 | np.ndarray: Hessian of softconstraint of shape (x_dim, x_dim, N). 356 | """ 357 | x = state[0] 358 | y = state[1] 359 | theta = state[3] 360 | ct = np.cos(theta) 361 | st = np.sin(theta) 362 | center_L = self.center_L + center_L 363 | 364 | c_x = np.zeros(4) 365 | c_xx = np.zeros((4, 4)) 366 | 367 | for i in range(self.n_circ): 368 | c_x_temp = np.zeros((4, obs.n_circ)) 369 | c_xx_temp = np.zeros((4, 4, obs.n_circ)) 370 | center_i = self.center[:, i][:, np.newaxis] 371 | dxy = center_i - obs.center 372 | dx = dxy[0, :] 373 | dy = dxy[1, :] 374 | d = np.linalg.norm(dxy, axis=0) 375 | 376 | b = self.r + obs.r - d 377 | 378 | idx_ignore = b < -0.2 379 | c = q1 * np.exp(q2 * b) 380 | 381 | # Jacobian 382 | c_x_temp[:2, :] = (-(q2 * c * dxy) / d) 383 | 384 | c_x_temp[3, :] = -( 385 | center_L[i] * q2 * c * 386 | (y*ct - obs.center[1, :] * ct - x*st + obs.center[0, :] * st) 387 | ) / d 388 | 389 | # Hessian 390 | c_xx_temp[0, 0, :] = ((q2 * c * dx**2) / (d**3) - (q2*c) / d + 391 | (q2**2 * c * dx**2) / (d**2)) 392 | c_xx_temp[1, 1, :] = ((q2 * c * dy**2) / (d**3) - (q2*c) / d + 393 | (q2**2 * c * dy**2) / (d**2)) 394 | c_xx_temp[0, 1, :] = ((q2**2 * c * dx * dy) / (d**2) + (q2*c*dx*dy) / 395 | (d**3)) 396 | c_xx_temp[1, 0, :] = ((q2**2 * c * dx * dy) / (d**2) + (q2*c*dx*dy) / 397 | (d**3)) 398 | 399 | c_x_temp[:, idx_ignore] = 0 400 | c_xx_temp[:, :, idx_ignore] = 0 401 | 402 | c_x += c_x_temp.sum(axis=-1) 403 | c_xx += c_xx_temp.sum(axis=-1) 404 | 405 | return c_x, c_xx 406 | -------------------------------------------------------------------------------- /iLQR/ilqr.py: -------------------------------------------------------------------------------- 1 | """ 2 | Jaxified iterative Linear Quadrative Regulator (iLQR). 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | Reference: ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 7 | """ 8 | 9 | import time 10 | import numpy as np 11 | from typing import Tuple 12 | 13 | from .cost import Cost 14 | from .dynamics import Dynamics 15 | 16 | from functools import partial 17 | from jax import jit, lax 18 | from jaxlib.xla_extension import ArrayImpl 19 | import jax.numpy as jnp 20 | 21 | 22 | class iLQR(): 23 | 24 | def __init__(self, ref_path, config, safety=True): 25 | 26 | self.T = config.T 27 | self.N = config.N 28 | 29 | self.ref_path = ref_path 30 | 31 | self.steps = config.MAX_ITER 32 | 33 | self.tol = 1e-2 34 | self.lambad = 10 35 | self.lambad_max = 100 36 | self.lambad_min = 1e-3 37 | 38 | self.dynamics = Dynamics(config) 39 | self.alphas = 1.1**(-np.arange(10)**2) 40 | 41 | self.dim_x = self.dynamics.dim_x 42 | self.dim_u = self.dynamics.dim_u 43 | 44 | self.cost = Cost(config, safety) 45 | 46 | def forward_pass( 47 | self, nominal_states: ArrayImpl, nominal_controls: ArrayImpl, Ks: ArrayImpl, ks: ArrayImpl, 48 | alpha: float 49 | ) -> Tuple[np.ndarray, np.ndarray, float, np.ndarray, np.ndarray, np.ndarray]: 50 | """ 51 | Forward pass wrapper. 52 | Calls forward_pass_jax to speed up computation. 53 | 54 | Args: 55 | nominal_states (ArrayImpl): (dim_x, N) 56 | nominal_controls (ArrayImpl): (dim_u, N) 57 | Ks (ArrayImpl): gain matrices (dim_u, dim_x, N - 1) 58 | ks (ArrayImpl): gain vectors (dim_u, N - 1) 59 | alpha (float): scalar parameter 60 | 61 | Returns: 62 | X (np.ndarray): (dim_x, N) 63 | U (np.ndarray): (dim_u, N) 64 | J (float): total cost. 65 | closest_pts (np.ndarray): 2xN array of each state's closest point [x,y] 66 | on the track. 67 | slope (np.ndarray): 1xN array of track's slopes (rad) at closest 68 | points. 69 | theta (np.ndarray): 1xN array of the progress at each state. 70 | """ 71 | Xs, Us = self.forward_pass_jax(nominal_states, nominal_controls, Ks, ks, alpha) 72 | Xs = np.asarray(Xs) 73 | Us = np.asarray(Us) 74 | 75 | closest_pt, slope, theta = self.ref_path.get_closest_pts(Xs[:2, :]) 76 | J = self.cost.get_cost(Xs, Us, closest_pt, slope, theta) 77 | return Xs, Us, J, closest_pt, slope, theta 78 | 79 | def backward_pass( 80 | self, nominal_states: ArrayImpl, nominal_controls: ArrayImpl, closest_pts: ArrayImpl, 81 | slopes: ArrayImpl 82 | ) -> Tuple[np.ndarray, np.ndarray]: 83 | """ 84 | Backward pass wrapper. 85 | Calls get_AB_matrix_jax and backward_pass_jax to speed up computation. 86 | 87 | Args: 88 | nominal_states (ArrayImpl): (dim_x, N) 89 | nominal_controls (ArrayImpl): (dim_u, N) 90 | closest_pts (ArrayImpl): (2, N) 91 | slopes (ArrayImpl): (1, N) 92 | 93 | Returns: 94 | Ks (np.ndarray): gain matrices (dim_u, dim_x, N - 1) 95 | ks (np.ndarray): gain vectors (dim_u, N - 1) 96 | """ 97 | # t0 = time.time() 98 | L_x, L_xx, L_u, L_uu, L_ux = self.cost.get_derivatives_jax( 99 | nominal_states, nominal_controls, closest_pts, slopes 100 | ) 101 | fx, fu = self.dynamics.get_AB_matrix_jax(nominal_states, nominal_controls) 102 | Ks, ks = self.backward_pass_jax(L_x, L_xx, L_u, L_uu, L_ux, fx, fu) 103 | # print("backward pass use: ", time.time()-t0) 104 | return np.asarray(Ks), np.asarray(ks) 105 | 106 | def solve( 107 | self, cur_state: np.ndarray, controls: np.ndarray = None, obs_list: list = [], 108 | record: bool = False 109 | ) -> Tuple[np.ndarray, np.ndarray, float, int, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: 110 | """ 111 | Solves the iLQR problem. 112 | 113 | Args: 114 | cur_state (np.ndarray): (dim_x,) 115 | controls (np.ndarray, optional): (self.dim_u,). Defaults to None. 116 | obs_list (list, optional): obstacle list. Defaults to []. 117 | record (bool, optional): Defaults to False. 118 | 119 | Returns: 120 | states: np.ndarray 121 | controls: np.ndarray 122 | t_process: float 123 | status: int 124 | theta: np.ndarray 125 | Ks: np.ndarray 126 | fx: np.ndarray 127 | fu: np.ndarray 128 | """ 129 | status = 0 130 | 131 | time0 = time.time() 132 | 133 | if controls is None: 134 | controls = np.zeros((self.dim_u, self.N)) 135 | states = np.zeros((self.dim_x, self.N)) 136 | states[:, 0] = cur_state 137 | 138 | for i in range(1, self.N): 139 | states[:, i], _ = self.dynamics.forward_step(states[:, i - 1], controls[:, i - 1]) 140 | closest_pt, slope, theta = self.ref_path.get_closest_pts(states[:2, :]) 141 | 142 | self.cost.update_obs(obs_list) 143 | 144 | J = self.cost.get_cost(states, controls, closest_pt, slope, theta) 145 | 146 | converged = False 147 | 148 | for i in range(self.steps): 149 | slope = slope[np.newaxis, :] 150 | Ks, ks = self.backward_pass(states, controls, closest_pt, slope) 151 | 152 | updated = False 153 | for alpha in self.alphas: 154 | X_new, U_new, J_new, closest_pt_new, slope_new, theta_new = ( 155 | self.forward_pass(states, controls, Ks, ks, alpha) 156 | ) 157 | if J_new <= J: 158 | if np.abs((J-J_new) / J) < self.tol: 159 | converged = True 160 | J = J_new 161 | states = X_new 162 | controls = U_new 163 | closest_pt = closest_pt_new 164 | slope = slope_new 165 | theta = theta_new 166 | updated = True 167 | break 168 | if updated: 169 | self.lambad *= 0.7 170 | else: 171 | status = 2 172 | break 173 | self.lambad = max(self.lambad_min, self.lambad) 174 | 175 | if converged: 176 | status = 1 177 | break 178 | t_process = time.time() - time0 179 | 180 | if record: 181 | Ks, _ = self.backward_pass_jax(states, controls, closest_pt, slope) 182 | fx, fu = self.dynamics.get_AB_matrix_jax(states, controls) 183 | else: 184 | Ks = None 185 | fx = None 186 | fu = None 187 | return states, controls, t_process, status, theta, Ks, fx, fu 188 | 189 | # ----------------------------- Jitted functions ----------------------------- 190 | @partial(jit, static_argnums=(0,)) 191 | def forward_pass_jax( 192 | self, nominal_states: ArrayImpl, nominal_controls: ArrayImpl, Ks: ArrayImpl, ks: ArrayImpl, 193 | alpha: float 194 | ) -> Tuple[ArrayImpl, ArrayImpl]: 195 | """ 196 | Jitted forward pass looped computation. 197 | 198 | Args: 199 | nominal_states (ArrayImpl): (dim_x, N) 200 | nominal_controls (ArrayImpl): (dim_u, N) 201 | Ks (ArrayImpl): gain matrices (dim_u, dim_x, N - 1) 202 | ks (ArrayImpl): gain vectors (dim_u, N - 1) 203 | alpha (float): scalar parameter 204 | 205 | Returns: 206 | Xs (ArrayImpl): (dim_x, N) 207 | Us (ArrayImpl): (dim_u, N) 208 | """ 209 | 210 | @jit 211 | def forward_pass_looper(i, _carry): 212 | Xs, Us = _carry 213 | u = ( 214 | nominal_controls[:, i] + alpha * ks[:, i] 215 | + Ks[:, :, i] @ (Xs[:, i] - nominal_states[:, i]) 216 | ) 217 | X_next, U_next = self.dynamics.integrate_forward_jax(Xs[:, i], u) 218 | Xs = Xs.at[:, i + 1].set(X_next) 219 | Us = Us.at[:, i].set(U_next) 220 | return Xs, Us 221 | 222 | Xs = jnp.zeros((self.dim_x, self.N)) 223 | Us = jnp.zeros((self.dim_u, self.N)) 224 | Xs = Xs.at[:, 0].set(nominal_states[:, 0]) 225 | Xs, Us = lax.fori_loop(0, self.N - 1, forward_pass_looper, (Xs, Us)) 226 | return Xs, Us 227 | 228 | @partial(jit, static_argnums=(0,)) 229 | def backward_pass_jax( 230 | self, L_x: ArrayImpl, L_xx: ArrayImpl, L_u: ArrayImpl, L_uu: ArrayImpl, L_ux: ArrayImpl, 231 | fx: ArrayImpl, fu: ArrayImpl 232 | ) -> Tuple[ArrayImpl, ArrayImpl]: 233 | """ 234 | Jitted backward pass looped computation. 235 | 236 | Args: 237 | L_x (ArrayImpl): (dim_x, N) 238 | L_xx (ArrayImpl): (dim_x, dim_x, N) 239 | L_u (ArrayImpl): (dim_u, N) 240 | L_uu (ArrayImpl): (dim_u, dim_u, N) 241 | L_ux (ArrayImpl): (dim_u, dim_x, N) 242 | fx (ArrayImpl): (dim_x, dim_x, N) 243 | fu (ArrayImpl): (dim_x, dim_u, N) 244 | 245 | Returns: 246 | Ks (ArrayImpl): gain matrices (dim_u, dim_x, N - 1) 247 | ks (ArrayImpl): gain vectors (dim_u, N - 1) 248 | """ 249 | 250 | @jit 251 | def backward_pass_looper(i, _carry): 252 | V_x, V_xx, ks, Ks = _carry 253 | n = self.N - 2 - i 254 | 255 | Q_x = L_x[:, n] + fx[:, :, n].T @ V_x 256 | Q_u = L_u[:, n] + fu[:, :, n].T @ V_x 257 | Q_xx = L_xx[:, :, n] + fx[:, :, n].T @ V_xx @ fx[:, :, n] 258 | Q_ux = L_ux[:, :, n] + fu[:, :, n].T @ V_xx @ fx[:, :, n] 259 | Q_uu = L_uu[:, :, n] + fu[:, :, n].T @ V_xx @ fu[:, :, n] 260 | 261 | Q_uu_inv = jnp.linalg.inv(Q_uu + reg_mat) 262 | 263 | Ks = Ks.at[:, :, n].set(-Q_uu_inv @ Q_ux) 264 | ks = ks.at[:, n].set(-Q_uu_inv @ Q_u) 265 | 266 | V_x = Q_x - Ks[:, :, n].T @ Q_uu @ ks[:, n] 267 | V_xx = Q_xx - Ks[:, :, n].T @ Q_uu @ Ks[:, :, n] 268 | 269 | return V_x, V_xx, ks, Ks 270 | 271 | Ks = jnp.zeros((self.dim_u, self.dim_x, self.N - 1)) 272 | ks = jnp.zeros((self.dim_u, self.N - 1)) 273 | 274 | V_x = L_x[:, -1] 275 | V_xx = L_xx[:, :, -1] 276 | 277 | reg_mat = self.lambad * jnp.eye(self.dim_u) 278 | 279 | V_x, V_xx, ks, Ks = lax.fori_loop(0, self.N - 1, backward_pass_looper, (V_x, V_xx, ks, Ks)) 280 | return Ks, ks 281 | -------------------------------------------------------------------------------- /iLQR/shielding.py: -------------------------------------------------------------------------------- 1 | """ 2 | iLQR-based shielding. 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | """ 7 | 8 | import numpy as np 9 | 10 | from jaxlib.xla_extension import ArrayImpl 11 | 12 | 13 | class Shielding(object): 14 | 15 | def __init__(self, obs_list, config, N_sh=None): 16 | """ 17 | Base class for all shielding mechanisms. 18 | 19 | Args: 20 | obs_list (list): list of ellipsoidal obstacles. 21 | config (Struct): parameter data. 22 | N_sh (int): shielding rollout horizon. 23 | """ 24 | 25 | self.obs_list = obs_list 26 | self.config = config 27 | self.coll_check_slack = config.COLL_CHECK_SLACK 28 | self.N_sh = N_sh 29 | self.sh_flag = False 30 | 31 | def is_collision(self, states: ArrayImpl) -> bool: 32 | """ 33 | Checks collision with the static obstacles. 34 | 35 | Args: 36 | states (ArrayImpl): state trajectory of the ego agent. 37 | 38 | Returns: 39 | bool: True if there is a collision. 40 | """ 41 | states = np.asarray(states) 42 | 43 | if len(states.shape) < 2: 44 | states = states[:, np.newaxis] 45 | 46 | if self.N_sh is None: 47 | N_sh = states.shape[1] 48 | else: 49 | N_sh = min(states.shape[1], self.N_sh) 50 | 51 | res = False 52 | for obs in self.obs_list: 53 | for k in range(N_sh): 54 | pos_k = states[:2, k] 55 | pos_k = pos_k[:, np.newaxis] 56 | if obs.is_internal(pos_k, slack=self.coll_check_slack): 57 | return True 58 | return res 59 | 60 | 61 | class NaiveSwerving(Shielding): 62 | 63 | def __init__(self, config, obs_list, dynamics, max_deacc, N_sh=None): 64 | """ 65 | Naive swerving shielding strategy: maximum steering and deacceleration. 66 | 67 | Args: 68 | obs_list (list): list of ellipsoidal obstacles. 69 | config (Struct): parameter data. 70 | dynamics (Dynamics): dynamical system. 71 | max_deacc (float): maximum deacceleration. 72 | N_sh (int): shielding rollout horizon. 73 | """ 74 | 75 | self.states = None 76 | self.controls_init = np.zeros((2, config.N)) 77 | self.max_deacc = max_deacc 78 | self.delta_min = config.DELTA_MIN 79 | self.delta_max = config.DELTA_MAX 80 | self.dynamics = dynamics 81 | super(NaiveSwerving, self).__init__(obs_list, config, N_sh) 82 | 83 | def run(self, x: ArrayImpl, u_nominal: ArrayImpl) -> np.ndarray: 84 | """ 85 | Runs the naive swerving shielding. 86 | 87 | Args: 88 | x (ArrayImpl): current state. 89 | u_nominal (ArrayImpl): nominal control. 90 | 91 | Returns: 92 | np.ndarray: shielded control. 93 | """ 94 | 95 | x = np.asarray(x) 96 | u_nominal = np.asarray(u_nominal) 97 | 98 | # Resets the shielding flag. 99 | self.sh_flag = False 100 | 101 | # One-step simulation with robot's nominal policy. 102 | x_next_nom = self.dynamics.forward_step(x, u_nominal)[0] 103 | 104 | success_delta_min = True 105 | 106 | if self.is_collision(x_next_nom): 107 | self.sh_flag = True 108 | else: 109 | # Computes iLQR shielding policies starting with x_next_nom. 110 | # 1. First try delta_min. 111 | u_sh = np.array([self.max_deacc, self.delta_min]) 112 | x_sh = x_next_nom 113 | for _ in range(self.N_sh - 1): 114 | x_sh = self.dynamics.forward_step(x_sh, u_sh)[0] 115 | if self.is_collision(x_sh): 116 | success_delta_min = False 117 | break 118 | 119 | # 2. If delta_min fails, then try delta_max. 120 | if not success_delta_min: 121 | u_sh = np.array([self.max_deacc, self.delta_max]) 122 | x_sh = x_next_nom 123 | for _ in range(self.N_sh - 1): 124 | x_sh = self.dynamics.forward_step(x_sh, u_sh)[0] 125 | if self.is_collision(x_sh): 126 | self.sh_flag = True 127 | break 128 | 129 | # Shielding is needed. 130 | if self.sh_flag: 131 | # print("Shielding override.") 132 | if success_delta_min: 133 | u_sh = np.array([self.max_deacc, self.delta_min]) 134 | 135 | else: 136 | u_sh = np.array([self.max_deacc, self.delta_max]) 137 | 138 | states_sh = x[:, np.newaxis] 139 | for _ in range(self.N_sh - 1): 140 | x = self.dynamics.forward_step(x, u_sh)[0] 141 | states_sh = np.hstack((states_sh, x[:, np.newaxis])) 142 | 143 | self.states = states_sh 144 | 145 | return u_sh 146 | 147 | # Shielding is not needed. 148 | else: 149 | return u_nominal 150 | 151 | 152 | class ILQshielding(Shielding): 153 | 154 | def __init__(self, config, solver, obs_list, obs_list_timed, N_sh=None): 155 | """ 156 | ILQR-based shielding policy. 157 | 158 | Args: 159 | config (Struct): parameter data. 160 | solver (iLQR): iLQR solver. 161 | obs_list (list): list of ellipsoidal obstacles. 162 | obs_list_timed (list): list of time-varying obstacles. 163 | N_sh (int): shielding rollout horizon. 164 | """ 165 | 166 | self.solver = solver 167 | self.obs_list_timed = obs_list_timed 168 | self.states = None 169 | self.controls_init = np.zeros((2, config.N)) 170 | super(ILQshielding, self).__init__(obs_list, config, N_sh) 171 | 172 | def run(self, x: ArrayImpl, u_nominal: ArrayImpl) -> np.ndarray: 173 | """ 174 | Runs the iLQR-based shielding. 175 | 176 | Args: 177 | x (ArrayImpl): current state. 178 | u_nominal (ArrayImpl): nominal control. 179 | 180 | Returns: 181 | np.ndarray: shielded control. 182 | """ 183 | 184 | # Resets the shielding flag. 185 | self.sh_flag = False 186 | 187 | # Computes iLQR shielding policies starting with x. 188 | states_sh, controls_sh, _, _, _, _, _, _ = ( 189 | self.solver.solve(x, controls=self.controls_init, obs_list=self.obs_list_timed) 190 | ) 191 | self.states = states_sh 192 | 193 | # One-step simulation with robot's nominal policy. 194 | x_next_nom = self.solver.dynamics.forward_step(x, u_nominal)[0] 195 | if self.is_collision(x_next_nom): 196 | self.sh_flag = True 197 | else: 198 | # Computes iLQR shielding policies starting with x_next_nom. 199 | states, _, _, _, _, _, _, _ = ( 200 | self.solver.solve(x_next_nom, controls=self.controls_init, obs_list=self.obs_list_timed) 201 | ) 202 | if self.is_collision(states): 203 | self.sh_flag = True 204 | 205 | # Updates the init. control signal for warmstart of next receding horizon. 206 | self.controls_init[:, :-1] = controls_sh[:, 1:] 207 | 208 | # Shielding is needed. 209 | if self.sh_flag: 210 | # print("Shielding override.") 211 | controls_sh = np.asarray(controls_sh[:, 0]) 212 | if controls_sh[1] <= 0: 213 | u_sh = np.array([controls_sh[0], self.config.DELTA_MIN]) 214 | else: 215 | u_sh = np.array([controls_sh[0], self.config.DELTA_MAX]) 216 | return u_sh 217 | 218 | # Shielding is not needed. 219 | else: 220 | return np.asarray(u_nominal) 221 | -------------------------------------------------------------------------------- /iLQR/track.py: -------------------------------------------------------------------------------- 1 | """ 2 | Racing track. 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | Reference: ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 7 | """ 8 | 9 | import numpy as np 10 | from typing import Tuple 11 | from matplotlib import pyplot as plt 12 | from pyspline.pyCurve import Curve 13 | 14 | 15 | class Track: 16 | 17 | def __init__( 18 | self, center_line: np.ndarray = None, width_left: float = None, 19 | width_right: float = None, loop: bool = True 20 | ): 21 | """ 22 | Constructs a track with a fixed width. 23 | 24 | Args: 25 | center_line (np.ndarray, optional): 2D numpy array containing samples 26 | of track center line [[x1,x2,...], [y1,y2,...]]. Defaults to None. 27 | width_left (float, optional): width of the track on the left side. 28 | Defaults to None. 29 | width_right (float, optional): width of the track on the right side. 30 | Defaults to None. 31 | loop (bool, optional): flag indicating if the track has loop. 32 | Defaults to True. 33 | """ 34 | self.width_left = width_left 35 | self.width_right = width_right 36 | self.loop = loop 37 | 38 | if center_line is not None: 39 | self.center_line = Curve(x=center_line[0, :], y=center_line[1, :], k=3) 40 | self.length = self.center_line.getLength() 41 | else: 42 | self.length = None 43 | self.center_line = None 44 | 45 | self.track_bound = None 46 | self.track_center = None 47 | 48 | def _interp_s(self, s: list) -> Tuple[np.ndarray, np.ndarray]: 49 | """ 50 | Given a list of s (progress since start), returns corresponing (x,y) points 51 | on the track. In addition, return slope of trangent line on those points. 52 | 53 | Args: 54 | s (list): progress since start. 55 | 56 | Returns: 57 | np.ndarray: (x,y) points on the track. 58 | np.ndarray: slopes of trangent line. 59 | """ 60 | try: 61 | n = len(s) 62 | except: 63 | s = np.array([s]) 64 | n = len(s) 65 | 66 | interp_pt = self.center_line.getValue(s) 67 | slope = np.zeros(n) 68 | 69 | for i in range(n): 70 | deri = self.center_line.getDerivative(s[i]) 71 | slope[i] = np.arctan2(deri[1], deri[0]) 72 | return interp_pt.T, slope 73 | 74 | def interp(self, theta_list: list) -> Tuple[np.ndarray, np.ndarray]: 75 | """ 76 | Given a list of theta (progress since start), return corresponing (x,y) 77 | points on the track. In addition, return slope of trangent line on those 78 | points. 79 | 80 | Args: 81 | theta_list (list): progress since start 82 | 83 | Returns: 84 | np.ndarray: (x,y) points on the track. 85 | np.ndarray: slopes of trangent line. 86 | """ 87 | if self.loop: 88 | s = np.remainder(theta_list, self.length) / self.length 89 | else: 90 | s = np.array(theta_list) / self.length 91 | s[s > 1] = 1 92 | return self._interp_s(s) 93 | 94 | def get_closest_pts( 95 | self, points: np.ndarray 96 | ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: 97 | """ 98 | Gets the closest points. 99 | 100 | Args: 101 | points (np.ndarray): points on the track of [2xn] shape. 102 | 103 | Returns: 104 | np.ndarray: (x,y) points on the track. 105 | np.ndarray: slopes of trangent line. 106 | np.ndarray: projected progress 107 | """ 108 | s, _ = self.center_line.projectPoint(points.T, eps=1e-3) 109 | closest_pt, slope = self._interp_s(s) 110 | return closest_pt, slope, s * self.length 111 | 112 | def project_point(self, point: np.ndarray) -> np.ndarray: 113 | """ 114 | Projects a point. 115 | 116 | Args: 117 | point (np.ndarray): points on the track of [2xn] shape. 118 | 119 | Returns: 120 | np.ndarray: projected points. 121 | """ 122 | s, _ = self.center_line.projectPoint(point, eps=1e-3) 123 | return s * self.length 124 | 125 | def get_track_width(self, 126 | theta: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: 127 | """ 128 | Gets the width of the track. 129 | 130 | Args: 131 | theta (np.ndarray): progress since start. 132 | 133 | Returns: 134 | np.ndarray: left width array. 135 | np.ndarray: right width array. 136 | """ 137 | temp = np.ones_like(theta) 138 | return self.width_left * temp, self.width_right * temp 139 | 140 | def plot_track(self): 141 | """ 142 | Plots the track. 143 | """ 144 | N = 500 145 | if self.track_bound is None: 146 | theta_sample = np.linspace(0, 1, N, endpoint=False) * self.length 147 | interp_pt, slope = self.interp(theta_sample) 148 | 149 | if self.loop: 150 | self.track_bound = np.zeros((4, N + 1)) 151 | else: 152 | self.track_bound = np.zeros((4, N)) 153 | 154 | self.track_bound[ 155 | 0, :N] = interp_pt[0, :] - np.sin(slope) * self.width_left 156 | self.track_bound[ 157 | 1, :N] = interp_pt[1, :] + np.cos(slope) * self.width_left 158 | 159 | self.track_bound[ 160 | 2, :N] = interp_pt[0, :] + np.sin(slope) * self.width_right 161 | self.track_bound[ 162 | 3, :N] = interp_pt[1, :] - np.cos(slope) * self.width_right 163 | 164 | if self.loop: 165 | self.track_bound[:, -1] = self.track_bound[:, 0] 166 | 167 | plt.plot(self.track_bound[0, :], self.track_bound[1, :], 'k-') 168 | plt.plot(self.track_bound[2, :], self.track_bound[3, :], 'k-') 169 | 170 | def plot_track_center(self): 171 | """ 172 | Plots the center of the track. 173 | """ 174 | N = 500 175 | if self.track_center is None: 176 | theta_sample = np.linspace(0, 1, N, endpoint=False) * self.length 177 | interp_pt, slope = self.interp(theta_sample) 178 | self.track_center = interp_pt 179 | print(len(slope)) 180 | 181 | plt.plot(self.track_center[0, :], self.track_center[1, :], 'r--') 182 | 183 | 184 | if __name__ == '__main__': 185 | import csv 186 | track_file = 'outerloop_center_smooth.csv' 187 | x = [] 188 | y = [] 189 | with open(track_file, newline='') as f: 190 | spamreader = csv.reader(f, delimiter=',') 191 | for i, row in enumerate(spamreader): 192 | if i > 0: 193 | x.append(float(row[0])) 194 | y.append(float(row[1])) 195 | 196 | center_line = np.array([x, y]) 197 | track = Track( 198 | center_line=center_line, width_left=0.3, width_right=0.3, loop=True 199 | ) 200 | 201 | track.plot_track() 202 | track.plot_track_center() 203 | plt.show() 204 | -------------------------------------------------------------------------------- /iLQR/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | Util functions. 3 | 4 | Please contact the author(s) of this library if you have any questions. 5 | Author: Haimin Hu (haiminh@princeton.edu) 6 | Reference: ECE346@Princeton (Zixu Zhang, Kai-Chieh Hsu, Duy P. Nguyen) 7 | """ 8 | 9 | import yaml 10 | import csv 11 | import numpy as np 12 | 13 | from .track import Track 14 | 15 | 16 | class Struct: 17 | """ 18 | Struct for managing parameters. 19 | """ 20 | 21 | def __init__(self, data): 22 | for key, value in data.items(): 23 | setattr(self, key, value) 24 | 25 | 26 | def load_config(file_path): 27 | """ 28 | Loads the config file. 29 | 30 | Args: 31 | file_path (string): path to the parameter file. 32 | 33 | Returns: 34 | Struct: parameters. 35 | """ 36 | with open(file_path) as f: 37 | data = yaml.safe_load(f) 38 | config = Struct(data) 39 | return config 40 | 41 | 42 | def load_track(config): 43 | """ 44 | Loads the track. 45 | 46 | Args: 47 | config (Struct): problem parameters. 48 | 49 | Returns: 50 | Track: the track object. 51 | """ 52 | x_center_line = [] 53 | y_center_line = [] 54 | with open(config.TRACK_FILE) as f: 55 | spamreader = csv.reader(f, delimiter=',') 56 | for i, row in enumerate(spamreader): 57 | if i > 0: 58 | x_center_line.append(float(row[0])) 59 | y_center_line.append(float(row[1])) 60 | 61 | center_line = np.array([x_center_line, y_center_line]) 62 | track = Track( 63 | center_line=center_line, width_left=config.TRACK_WIDTH_L, 64 | width_right=config.TRACK_WIDTH_R, loop=config.LOOP 65 | ) 66 | return track 67 | 68 | 69 | def plot_ellipsoids( 70 | ax, E_list, arg_list=[], dims=None, N=200, plot_center=True, 71 | use_alpha=False 72 | ): 73 | """ 74 | Plot a list of ellipsoids 75 | Args: 76 | E_list (list): list of ellipsoids 77 | arg_list (list): list of args dictionary. 78 | dims (list): dimension to be preserved. 79 | N (int): number of boundary points. 80 | plot_center (bool): plot the center of the ellipsoid if True. Defaults to 81 | True. 82 | use_alpha (bool): make later ellipsoids more transparent if True. 83 | Defaults to False. 84 | """ 85 | if len(arg_list) == 0: 86 | arg_list = [dict(c='r')] * len(E_list) 87 | elif len(arg_list) == 1: 88 | arg_list = arg_list * len(E_list) 89 | else: 90 | assert len(arg_list) == len(E_list), "The length does not match." 91 | if use_alpha: 92 | alpha_list = np.linspace(1., 0.1, len(E_list)) 93 | else: 94 | alpha_list = np.ones(len(E_list)) 95 | 96 | for E0, arg, alpha in zip(E_list, arg_list, alpha_list): 97 | if dims: 98 | E = E0.projection(dims) 99 | else: 100 | E = E0.copy() 101 | if E.dim() > 3: 102 | raise ValueError("[ellReach-plot] ellipsoid dimension can be 1, 2 or 3.") 103 | if E.dim() == 1: 104 | x = np.array([(E.q - np.sqrt(E.Q))[0, 0], (E.q + np.sqrt(E.Q))[0, 0]]) 105 | ax.plot(x, np.zeros_like(x), color=arg) 106 | ax.plot(E.q, 0, color=arg, marker='*') 107 | if E.dim() == 2: 108 | if E.is_degenerate(): 109 | print("degenerate!") 110 | phi = np.array(np.linspace(0., 2. * np.pi, N)) 111 | L = np.concatenate( 112 | (np.cos(phi).reshape(1, N), np.sin(phi).reshape(1, N)), axis=0 113 | ) 114 | _, x = E.rho(L) 115 | # x = np.concatenate((x, x[:, 0].reshape(2, 1)), axis=1) 116 | ax.plot(x[0, :], x[1, :], alpha=alpha, **arg) 117 | if plot_center: 118 | ax.plot(E.q[0, 0], E.q[1, 0], marker='.', alpha=alpha, **arg) 119 | ax.set_aspect('equal') 120 | if E.dim() == 3: 121 | raise NotImplementedError 122 | -------------------------------------------------------------------------------- /misc/alter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/iLQR_jax_racing/bdc97019c1a3c79c46bd7166bb1be5662bb7268b/misc/alter.png -------------------------------------------------------------------------------- /misc/ego.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/iLQR_jax_racing/bdc97019c1a3c79c46bd7166bb1be5662bb7268b/misc/ego.png -------------------------------------------------------------------------------- /outerloop_center_smooth.csv: -------------------------------------------------------------------------------- 1 | -0.618804608073334,-0.0356295937401568 2 | -0.615545566529325,-0.0354481224108462 3 | -0.612284641734617,-0.0352667471001334 4 | -0.609019950438511,-0.0350855638266163 5 | -0.605749609390307,-0.0349046686088927 6 | -0.602471735339307,-0.0347241574655604 7 | -0.59918444503481,-0.0345441264152173 8 | -0.595885855226119,-0.0343646714764611 9 | -0.592574082662533,-0.0341858886678898 10 | -0.589247244093355,-0.034007874008101 11 | -0.585903456267883,-0.0338307235156928 12 | -0.58254083593542,-0.0336545332092628 13 | -0.579157499845266,-0.033479399107409 14 | -0.575751564746722,-0.0333054172287291 15 | -0.572321147389089,-0.033132683591821 16 | -0.568864364521668,-0.0329612942152825 17 | -0.565379332893759,-0.0327913451177114 18 | -0.561864169254663,-0.0326229323177056 19 | -0.558316990353682,-0.032456151833863 20 | -0.554735912940115,-0.0322910996847812 21 | -0.551119053763264,-0.0321278718890582 22 | -0.54746452957243,-0.0319665644652918 23 | -0.543770457116914,-0.0318072734320798 24 | -0.540034953146015,-0.0316500948080201 25 | -0.536256134409036,-0.0314951246117104 26 | -0.532432117655277,-0.0313424588617486 27 | -0.528561019634038,-0.0311921935767326 28 | -0.524640957094621,-0.0310444247752601 29 | -0.520670046786327,-0.0308992484759291 30 | -0.516646405458456,-0.0307567606973372 31 | -0.512568149860309,-0.0306170574580824 32 | -0.508433396741187,-0.0304802347767625 33 | -0.504240262850391,-0.0303463886719753 34 | -0.499986864937221,-0.0302156151623186 35 | -0.495671319750979,-0.0300880102663903 36 | -0.491291744040965,-0.0299636700027883 37 | -0.486846254556481,-0.0298426903901102 38 | -0.482332968046826,-0.029725167446954 39 | -0.477750001261302,-0.0296111971919175 40 | -0.473095470949209,-0.0295008756435985 41 | -0.468367493859849,-0.0293942988205948 42 | -0.463564186742523,-0.0292915627415043 43 | -0.45868366634653,-0.0291927634249249 44 | -0.453724049421173,-0.0290979968894542 45 | -0.448683452715751,-0.0290073591536903 46 | -0.443559992979565,-0.0289209462362308 47 | -0.438351786961918,-0.0288388541556736 48 | -0.433056951412108,-0.0287611789306166 49 | -0.427673603079438,-0.0286880165796576 50 | -0.422199858713207,-0.0286194631213944 51 | -0.416633835062717,-0.0285556145744248 52 | -0.41097425706643,-0.0284965344613146 53 | -0.405222282419454,-0.028442156320501 54 | -0.399379677006056,-0.0283923811943892 55 | -0.393448206710507,-0.0283471101253842 56 | -0.387429637417073,-0.0283062441558912 57 | -0.381325735010025,-0.0282696843283154 58 | -0.375138265373631,-0.0282373316850619 59 | -0.36886899439216,-0.0282090872685358 60 | -0.36251968794988,-0.0281848521211423 61 | -0.35609211193106,-0.0281645272852864 62 | -0.349588032219968,-0.0281480138033734 63 | -0.343009214700875,-0.0281352127178084 64 | -0.336357425258048,-0.0281260250709965 65 | -0.329634429775755,-0.0281203519053429 66 | -0.322841994138267,-0.0281180942632526 67 | -0.31598188422985,-0.0281191531871309 68 | -0.309055865934775,-0.0281234297193828 69 | -0.30206570513731,-0.0281308249024135 70 | -0.295013167721724,-0.0281412397786282 71 | -0.287900019572285,-0.0281545753904319 72 | -0.280728026573262,-0.0281707327802298 73 | -0.273498954608923,-0.0281896129904271 74 | -0.266214569563538,-0.0282111170634288 75 | -0.258876637321376,-0.0282351460416402 76 | -0.251486923766704,-0.0282616009674663 77 | -0.244047194783792,-0.0282903828833123 78 | -0.236559216256909,-0.0283213928315834 79 | -0.229024754070322,-0.0283545318546846 80 | -0.221445574108302,-0.0283897009950211 81 | -0.213823442255116,-0.028426801294998 82 | -0.206160124395033,-0.0284657337970206 83 | -0.198457386412322,-0.0285063995434938 84 | -0.190716994191252,-0.0285486995768229 85 | -0.182940713616092,-0.0285925349394129 86 | -0.175130310571109,-0.0286378066736691 87 | -0.167287550940574,-0.0286844158219966 88 | -0.159414200608754,-0.0287322634268004 89 | -0.151512025459919,-0.0287812505304858 90 | -0.143582791378337,-0.0288312781754579 91 | -0.135628264248276,-0.0288822474041217 92 | -0.127650209954006,-0.0289340592588825 93 | -0.119650394379795,-0.0289866147821454 94 | -0.111630583409912,-0.0290398150163155 95 | -0.103592542928626,-0.029093561003798 96 | -0.0955380388202057,-0.0291477537869979 97 | -0.0874688369689192,-0.0292022944083205 98 | -0.0793867032590355,-0.0292570839101709 99 | -0.0712934035748234,-0.0293120233349541 100 | -0.0631907038005517,-0.0293670137250754 101 | -0.0550803698204889,-0.0294219561229398 102 | -0.046963823022465,-0.0294767578970855 103 | -0.0388411068085542,-0.0295313517205818 104 | -0.0307119200843916,-0.0295856765926312 105 | -0.0225759617556125,-0.0296396715124359 106 | -0.0144329307278521,-0.0296932754791984 107 | -0.0062825259067458,-0.0297464274921211 108 | 0.00187555380207126,-0.0297990665504063 109 | 0.0100416094929638,-0.0298511316532563 110 | 0.0182159422602966,-0.0299025617998735 111 | 0.0263988531984344,-0.0299532959894603 112 | 0.0345906434017419,-0.0300032732212191 113 | 0.0427916139645839,-0.0300524324943522 114 | 0.0510020659813251,-0.0301007128080619 115 | 0.0592223005463303,-0.0301480531615507 116 | 0.0674526187539643,-0.0301943925540209 117 | 0.0756933216985917,-0.0302396699846749 118 | 0.0839447104745773,-0.030283824452715 119 | 0.092207086176286,-0.0303267949573436 120 | 0.100480749898082,-0.030368520497763 121 | 0.108766002734331,-0.0304089400731757 122 | 0.117063145779397,-0.0304479926827839 123 | 0.125372480127645,-0.0304856173257901 124 | 0.13369430687344,-0.0305217530013966 125 | 0.142028927111147,-0.0305563387088058 126 | 0.150376641935129,-0.03058931344722 127 | 0.158737752439753,-0.0306206162158416 128 | 0.167112559719382,-0.030650186013873 129 | 0.175501364868381,-0.0306779618405166 130 | 0.183904468981116,-0.0307038826949746 131 | 0.192322173151951,-0.0307278875764494 132 | 0.200754778475251,-0.0307499154841435 133 | 0.20920258604538,-0.0307699054172592 134 | 0.217665896956703,-0.0307877963749988 135 | 0.226145012303586,-0.0308035273565648 136 | 0.234640233180392,-0.0308170373611594 137 | 0.243151860681486,-0.030828265387985 138 | 0.251680195901234,-0.0308371504362441 139 | 0.260225539934,-0.0308436315051389 140 | 0.268788193874148,-0.0308476475938718 141 | 0.277368458816044,-0.0308491377016453 142 | 0.285966635854052,-0.0308480408276616 143 | 0.294583026082537,-0.0308442959711231 144 | 0.303217930595864,-0.0308378421312322 145 | 0.311871650488397,-0.0308286183071912 146 | 0.320544486854502,-0.0308165634982026 147 | 0.329236740788543,-0.0308016167034686 148 | 0.337948713384884,-0.0307837169221917 149 | 0.34668070573789,-0.0307628031535741 150 | 0.355433018941927,-0.0307388143968184 151 | 0.364205954091359,-0.0307116896511267 152 | 0.372999835272426,-0.030681387462656 153 | 0.381815078538866,-0.0306479445653808 154 | 0.390652122936292,-0.03061141724023 155 | 0.39951140751032,-0.0305718617681327 156 | 0.408393371306562,-0.0305293344300178 157 | 0.417298453370633,-0.0304838915068144 158 | 0.426227092748145,-0.0304355892794513 159 | 0.435179728484713,-0.0303844840288576 160 | 0.44415679962595,-0.0303306320359623 161 | 0.45315874521747,-0.0302740895816944 162 | 0.462186004304887,-0.0302149129469828 163 | 0.471239015933814,-0.0301531584127565 164 | 0.480318219149865,-0.0300888822599446 165 | 0.489424052998653,-0.0300221407694759 166 | 0.498556956525794,-0.0299529902222796 167 | 0.507717368776899,-0.0298814868992845 168 | 0.516905728797583,-0.0298076870814196 169 | 0.526122475633459,-0.029731647049614 170 | 0.535368048330142,-0.0296534230847967 171 | 0.544642885933245,-0.0295730714678965 172 | 0.553947427488381,-0.0294906484798426 173 | 0.563282112041164,-0.0294062104015638 174 | 0.572647378637209,-0.0293198135139892 175 | 0.582043666322128,-0.0292315140980477 176 | 0.591471414141535,-0.0291413684346684 177 | 0.600931061141044,-0.0290494328047802 178 | 0.610423046366269,-0.0289557634893122 179 | 0.619947808862824,-0.0288604167691932 180 | 0.629505787676321,-0.0287634489253523 181 | 0.639097421852376,-0.0286649162387184 182 | 0.6487231504366,-0.0285648749902206 183 | 0.658383412474609,-0.0284633814607879 184 | 0.668078647012016,-0.0283604919313491 185 | 0.677809293094434,-0.0282562626828334 186 | 0.687575789767478,-0.0281507499961696 187 | 0.697378576076761,-0.0280440101522869 188 | 0.707218091067896,-0.0279360994321141 189 | 0.717094773786497,-0.0278270741165802 190 | 0.727009063278179,-0.0277169904866143 191 | 0.736961398588554,-0.0276059048231453 192 | 0.746952218763237,-0.0274938734071022 193 | 0.756981962847841,-0.027380952519414 194 | 0.767051069887979,-0.0272671984410096 195 | 0.777159978929266,-0.0271526674528181 196 | 0.787309129017316,-0.0270374158357685 197 | 0.797498959197741,-0.0269214998707897 198 | 0.807729908516155,-0.0268049758388107 199 | 0.818002416018173,-0.0266879000207605 200 | 0.828316920749408,-0.0265703286975681 201 | 0.838673861755473,-0.0264523181501624 202 | 0.849073496887066,-0.0263339136073332 203 | 0.859515359215216,-0.026215116089313 204 | 0.869998800616036,-0.0260959155641948 205 | 0.880523172965639,-0.025976302000072 206 | 0.891087828140138,-0.0258562653650377 207 | 0.901692118015645,-0.025735795627185 208 | 0.912335394468273,-0.0256148827546071 209 | 0.923017009374135,-0.0254935167153973 210 | 0.933736314609345,-0.0253716874776487 211 | 0.944492662050014,-0.0252493850094546 212 | 0.955285403572256,-0.0251265992789079 213 | 0.966113891052183,-0.0250033202541021 214 | 0.976977476365908,-0.0248795379031302 215 | 0.987875511389545,-0.0247552421940854 216 | 0.998807347999205,-0.0246304230950609 217 | 1.009772338071,-0.0245050705741499 218 | 1.02076983348105,-0.0243791745994455 219 | 1.03179918610546,-0.0242527251390411 220 | 1.04285974782034,-0.0241257121610296 221 | 1.05395087050181,-0.0239981256335044 222 | 1.06507190602599,-0.0238699555245586 223 | 1.07622220626897,-0.0237411918022853 224 | 1.08740112310689,-0.0236118244347779 225 | 1.09860800841584,-0.0234818433901293 226 | 1.10984221407195,-0.0233512386364329 227 | 1.12110309195132,-0.0232200001417818 228 | 1.13238999393006,-0.0230881178742692 229 | 1.1437022718843,-0.0229555818019883 230 | 1.15503927769015,-0.0228223818930322 231 | 1.1664003632237,-0.0226885081154942 232 | 1.17778488036109,-0.0225539504374674 233 | 1.18919218097842,-0.022418698827045 234 | 1.20062161695181,-0.0222827432523202 235 | 1.21207254015736,-0.0221460736813861 236 | 1.22354430247119,-0.022008680082336 237 | 1.23503625576942,-0.0218705524232631 238 | 1.24654775192815,-0.0217316806722604 239 | 1.2580781428235,-0.0215920547974213 240 | 1.26962678033158,-0.0214516647668388 241 | 1.28119301632851,-0.0213105005486062 242 | 1.29277620269039,-0.0211685521108166 243 | 1.30437569129335,-0.0210258094215633 244 | 1.31599083401349,-0.0208822624489393 245 | 1.32762098272692,-0.020737901161038 246 | 1.33926548930976,-0.0205927155259524 247 | 1.35092370563813,-0.0204466955117758 248 | 1.36259498358813,-0.0202998310866013 249 | 1.37427867503587,-0.0201521122185222 250 | 1.38597413185748,-0.0200035288756315 251 | 1.39768070592906,-0.0198540710260225 252 | 1.40939762383543,-0.019703732037419 253 | 1.42112361099622,-0.0195525188760672 254 | 1.43285726753976,-0.0194004419078438 255 | 1.4445971935944,-0.0192475114986257 256 | 1.45634198928847,-0.0190937380142897 257 | 1.4680902547503,-0.0189391318207125 258 | 1.47984059010823,-0.0187837032837711 259 | 1.49159159549059,-0.018627462769342 260 | 1.50334187102573,-0.0184704206433023 261 | 1.51509001684197,-0.0183125872715286 262 | 1.52683463306765,-0.0181539730198978 263 | 1.53857431983111,-0.0179945882542867 264 | 1.55030767726068,-0.0178344433405721 265 | 1.5620333054847,-0.0176735486446307 266 | 1.57374980463151,-0.0175119145323395 267 | 1.58545577482943,-0.0173495513695751 268 | 1.59714981620681,-0.0171864695222143 269 | 1.60883052889198,-0.0170226793561341 270 | 1.62049651301328,-0.0168581912372112 271 | 1.63214636869903,-0.0166930155313223 272 | 1.64377869607759,-0.0165271626043443 273 | 1.65539209527727,-0.016360642822154 274 | 1.66698516642643,-0.0161934665506282 275 | 1.67855650965339,-0.0160256441556437 276 | 1.69010472508649,-0.0158571860030772 277 | 1.70162841285406,-0.0156881024588057 278 | 1.71312617308445,-0.0155184038887058 279 | 1.72459660590598,-0.0153481006586545 280 | 1.73603831144699,-0.0151772031345284 281 | 1.74744988983582,-0.0150057216822044 282 | 1.75882994120081,-0.0148336666675593 283 | 1.77017706567028,-0.0146610484564699 284 | 1.78148986337257,-0.014487877414813 285 | 1.79276693443603,-0.0143141639084654 286 | 1.80400687898898,-0.0141399183033039 287 | 1.81520829715976,-0.0139651509652053 288 | 1.82636978907671,-0.0137898722600464 289 | 1.83748995486816,-0.013614092553704 290 | 1.84856739466245,-0.0134378222120549 291 | 1.85960070858792,-0.0132610716009759 292 | 1.87058849677289,-0.0130838510863438 293 | 1.8815293593457,-0.0129061710340354 294 | 1.8924218964347,-0.0127280418099276 295 | 1.90326470816821,-0.012549473779897 296 | 1.91405639467457,-0.0123704773098205 297 | 1.92479555608212,-0.012191062765575 298 | 1.93548079251919,-0.0120112405130371 299 | 1.94611070411412,-0.0118310209180838 300 | 1.95668389099524,-0.0116504143465917 301 | 1.96719895329089,-0.0114694311644378 302 | 1.97765503657423,-0.0112879838978037 303 | 1.98805346819767,-0.0111055937140909 304 | 1.99839612095847,-0.0109216839410056 305 | 2.00868486765388,-0.0107356779062542 306 | 2.01892158108114,-0.010546998937543 307 | 2.02910813403749,-0.0103550703625784 308 | 2.03924639932019,-0.0101593155090666 309 | 2.04933824972648,-0.00995915770471409 310 | 2.0593855580536,-0.00975402027722706 311 | 2.06939019709881,-0.00954332655431188 312 | 2.07935403965935,-0.00932649986367488 313 | 2.08927895853246,-0.00910296353302238 314 | 2.0991668265154,-0.00887214089006072 315 | 2.1090195164054,-0.00863345526249621 316 | 2.11883890099972,-0.00838632997803519 317 | 2.1286268530956,-0.00813018836438397 318 | 2.1383852454903,-0.0078644537492489 319 | 2.14811595098104,-0.00758854946033628 320 | 2.15782084236509,-0.00730189882535246 321 | 2.16750179243968,-0.00700392517200376 322 | 2.17716067400207,-0.0066940518279965 323 | 2.1867993598495,-0.006371702121037 324 | 2.19641972277922,-0.00603629937883161 325 | 2.20602363558847,-0.00568726692908664 326 | 2.2156129710745,-0.00532402809950841 327 | 2.22518960203456,-0.00494600621780327 328 | 2.23475540126589,-0.00455262461167752 329 | 2.24431224156575,-0.0041433066088375 330 | 2.25386199573136,-0.00371747553698954 331 | 2.26340653655999,-0.00327455472383996 332 | 2.27294773684889,-0.00281396749709509 333 | 2.28248746939528,-0.00233513718446126 334 | 2.29202760699643,-0.00183748711364479 335 | 2.30157002244958,-0.001320440612352 336 | 2.31111658855197,-0.000783421008289232 337 | 2.32066917810085,-0.000225851629162803 338 | 2.33022966389348,0.000352844197320958 339 | 2.33979991872708,0.000953243143455724 340 | 2.34938181539892,0.00157592188153517 341 | 2.35897722670624,0.00222145708385297 342 | 2.36858802544628,0.00289042542270279 343 | 2.37821608441629,0.00358340357037832 344 | 2.38786327641352,0.00430096819917322 345 | 2.39753147423521,0.00504369598138116 346 | 2.40722255067861,0.00581216358929582 347 | 2.41693837854097,0.00660694769521089 348 | 2.42668083061954,0.00742862497142001 349 | 2.43645177971155,0.00827777209021687 350 | 2.44625309861425,0.00915496572389515 351 | 2.45608666012491,0.0100607825447485 352 | 2.46595342646491,0.0109957710863912 353 | 2.47585071755235,0.0119603673277193 354 | 2.48577494272946,0.0129549791089498 355 | 2.49572251133849,0.0139800142702993 356 | 2.50568983272169,0.0150358806519847 357 | 2.51567331622129,0.0161229860942226 358 | 2.52566937117953,0.0172417384372299 359 | 2.53567440693867,0.0183925455212233 360 | 2.54568483284094,0.0195758151864196 361 | 2.55569705822859,0.0207919552730355 362 | 2.56570749244386,0.0220413736212879 363 | 2.57571254482899,0.0233244780713935 364 | 2.58570862472622,0.024641676463569 365 | 2.59569214147781,0.0259933766380312 366 | 2.60565950442599,0.0273799864349969 367 | 2.61560712291301,0.0288019136946829 368 | 2.6255314062811,0.0302595662573059 369 | 2.63542876387252,0.0317533519630826 370 | 2.6452956050295,0.0332836786522299 371 | 2.65512833909429,0.0348509541649645 372 | 2.66492337540913,0.0364555863415032 373 | 2.67467712331626,0.0380979830220627 374 | 2.68438599215793,0.0397785520468599 375 | 2.69404639127639,0.0414977012561114 376 | 2.70365473001386,0.043255838490034 377 | 2.71320741771261,0.0450533715888446 378 | 2.72270086371486,0.0468907083927598 379 | 2.73213147736287,0.0487682567419965 380 | 2.74149566799887,0.0506864244767714 381 | 2.75078984496511,0.0526456194373012 382 | 2.76001041760383,0.0546462494638028 383 | 2.76915379525728,0.0566887223964929 384 | 2.7782163872677,0.0587734460755883 385 | 2.78719460297733,0.0609008283413057 386 | 2.79608485172841,0.0630712770338618 387 | 2.80488354286319,0.0652851999934736 388 | 2.81358708572392,0.0675430050603577 389 | 2.82219188965282,0.0698451000747309 390 | 2.83069436399216,0.0721918928768099 391 | 2.83909091808416,0.0745837913068116 392 | 2.84737796127108,0.0770212032049526 393 | 2.85555190289515,0.0795045364114499 394 | 2.86360915229862,0.08203419876652 395 | 2.87154611882374,0.0846105981103799 396 | 2.87935921181274,0.0872341422832462 397 | 2.88704484060787,0.0899052391253357 398 | 2.89459941455137,0.0926242964768652 399 | 2.90201934298549,0.0953917221780515 400 | 2.90930103525247,0.0982079240691113 401 | 2.91644090069454,0.101073309990261 402 | 2.92343641946037,0.103988213502409 403 | 2.93028935492426,0.106952671049223 404 | 2.93700254126692,0.109966644795061 405 | 2.94357881266905,0.113030096904285 406 | 2.95002100331137,0.116142989541251 407 | 2.9563319473746,0.11930528487032 408 | 2.96251447903943,0.122516945055851 409 | 2.9685714324866,0.125777932262202 410 | 2.97450564189679,0.129088208653733 411 | 2.98031994145073,0.132447736394803 412 | 2.98601716532913,0.135856477649771 413 | 2.9916001477127,0.139314394582996 414 | 2.99707172278215,0.142821449358836 415 | 3.00243472471819,0.146377604141652 416 | 3.00769198770154,0.149982821095803 417 | 3.01284634591289,0.153637062385646 418 | 3.01790063353298,0.157340290175542 419 | 3.0228576847425,0.16109246662985 420 | 3.02772033372217,0.164893553912928 421 | 3.0324914146527,0.168743514189136 422 | 3.0371737617148,0.172642309622832 423 | 3.04177020908918,0.176589902378376 424 | 3.04628359095656,0.180586254620128 425 | 3.05071674149764,0.184631328512445 426 | 3.05507249489314,0.188725086219687 427 | 3.05935368532377,0.192867489906214 428 | 3.06356314697024,0.197058501736384 429 | 3.06770371401325,0.201298083874556 430 | 3.07177822063353,0.20558619848509 431 | 3.07578950101178,0.209922807732344 432 | 3.07974038932872,0.214307873780678 433 | 3.08363371976505,0.218741358794451 434 | 3.08747232650149,0.223223224938021 435 | 3.09125904371875,0.227753434375748 436 | 3.09499670559753,0.232331949271991 437 | 3.09868814631856,0.236958731791109 438 | 3.10233620006254,0.241633744097462 439 | 3.10594370101019,0.246356948355407 440 | 3.10951348334221,0.251128306729305 441 | 3.11304838123932,0.255947781383514 442 | 3.11655122888222,0.260815334482394 443 | 3.12002486045164,0.265730928190303 444 | 3.12347211012827,0.270694524671601 445 | 3.12689581209284,0.275706086090647 446 | 3.13029880052605,0.280765574611799 447 | 3.13368390960861,0.285872952399418 448 | 3.13705397352124,0.291028181617861 449 | 3.14041182644465,0.296231224431488 450 | 3.14376030255955,0.301482043004659 451 | 3.14710223604665,0.306780599501732 452 | 3.15043989771599,0.31212708433995 453 | 3.15377330489494,0.317522600948096 454 | 3.15710191154022,0.322968481007834 455 | 3.16042517160853,0.328466056200832 456 | 3.16374253905656,0.334016658208753 457 | 3.16705346784104,0.339621618713265 458 | 3.17035741191865,0.345282269396033 459 | 3.17365382524611,0.350999941938723 460 | 3.17694216178013,0.356775968023 461 | 3.1802218754774,0.36261167933053 462 | 3.18349242029464,0.36850840754298 463 | 3.18675325018854,0.374467484342014 464 | 3.19000381911581,0.3804902414093 465 | 3.19324358103317,0.386578010426501 466 | 3.1964719898973,0.392732123075285 467 | 3.19968849966493,0.398953911037317 468 | 3.20289256429274,0.405244705994263 469 | 3.20608363773746,0.411605839627788 470 | 3.20926117395578,0.418038643619559 471 | 3.21242462690441,0.424544449651241 472 | 3.21557345054005,0.431124589404499 473 | 3.21870709881941,0.437780394561001 474 | 3.22182502569919,0.444513196802411 475 | 3.2249266851361,0.451324327810395 476 | 3.22801153108685,0.458215119266619 477 | 3.23107901750813,0.465186902852749 478 | 3.23412859835666,0.472241010250451 479 | 3.23715972758914,0.479378773141389 480 | 3.24017185916228,0.486601523207231 481 | 3.24316444703277,0.493910592129643 482 | 3.24613694515733,0.501307311590288 483 | 3.24908880749266,0.508793013270834 484 | 3.25201948799546,0.516369028852947 485 | 3.25492844062244,0.524036690018291 486 | 3.25781511933031,0.531797328448534 487 | 3.26067897807577,0.539652275825339 488 | 3.26351947081553,0.547602863830375 489 | 3.26633605150628,0.555650424145305 490 | 3.26912817410474,0.563796288451797 491 | 3.27189529256761,0.572041788431515 492 | 3.2746368608516,0.580388255766125 493 | 3.27735233291341,0.588837022137294 494 | 3.28004116270974,0.597389419226687 495 | 3.28270280419731,0.60604677871597 496 | 3.28533671133281,0.614810432286808 497 | 3.28794233807296,0.623681711620868 498 | 3.29051913837445,0.632661948399815 499 | 3.29306656619399,0.641752474305315 500 | 3.29558407548829,0.650954621019034 501 | 3.29807112021405,0.660269720222637 502 | 3.30052716949759,0.669698803932208 503 | 3.30295175314369,0.679241705501502 504 | 3.30534441612674,0.688897958618691 505 | 3.30770470342113,0.698667096971947 506 | 3.31003216000126,0.708548654249443 507 | 3.31232633084151,0.718542164139351 508 | 3.31458676091627,0.728647160329844 509 | 3.31681299519994,0.738863176509093 510 | 3.3190045786669,0.749189746365272 511 | 3.32116105629156,0.759626403586552 512 | 3.32328197304829,0.770172681861106 513 | 3.3253668739115,0.780828114877106 514 | 3.32741530385557,0.791592236322725 515 | 3.32942680785489,0.802464579886135 516 | 3.33140093088386,0.813444679255508 517 | 3.33333721791687,0.824532068119017 518 | 3.3352352139283,0.835726280164834 519 | 3.33709446389256,0.847026849081131 520 | 3.33891451278402,0.858433308556082 521 | 3.34069490557709,0.869945192277857 522 | 3.34243518724615,0.88156203393463 523 | 3.34413490276559,0.893283367214572 524 | 3.34579359710981,0.905108725805857 525 | 3.3474108152532,0.917037643396657 526 | 3.34898610217014,0.929069653675143 527 | 3.35051900283504,0.941204290329489 528 | 3.35200906222228,0.953441087047866 529 | 3.35345582530624,0.965779577518448 530 | 3.35485883706134,0.978219295429406 531 | 3.35621764246195,0.990759774468912 532 | 3.35753178648246,1.00340054832514 533 | 3.35880081409727,1.01614115068626 534 | 3.36002427028077,1.02898111524045 535 | 3.36120170000735,1.04191997567587 536 | 3.36233264825141,1.05495726568071 537 | 3.36341665998732,1.06809251894313 538 | 3.36445328018949,1.0813252691513 539 | 3.36544205383231,1.0946550499934 540 | 3.36638252589016,1.1080813951576 541 | 3.36727424133744,1.12160383833208 542 | 3.36811674514855,1.135221913205 543 | 3.36890958229786,1.14893515346453 544 | 3.36965229775977,1.16274309279886 545 | 3.37034443650868,1.17664526489615 546 | 3.37098554351897,1.19064120344457 547 | 3.37157516376504,1.20473044213229 548 | 3.37211284222127,1.2189125146475 549 | 3.37259812386207,1.23318695467836 550 | 3.37303055366181,1.24755329591304 551 | 3.3734096765949,1.26201107203972 552 | 3.37373537049325,1.27655959071791 553 | 3.37400884461897,1.29119725549252 554 | 3.37423164109169,1.30592224387979 555 | 3.37440530203103,1.32073273339598 556 | 3.37453136955662,1.33562690155734 557 | 3.37461138578809,1.3506029258801 558 | 3.37464689284509,1.36565898388053 559 | 3.37463943284722,1.38079325307487 560 | 3.37459054791413,1.39600391097936 561 | 3.37450178016545,1.41128913511027 562 | 3.3743746717208,1.42664710298383 563 | 3.37421076469982,1.44207599211629 564 | 3.37401160122214,1.45757398002392 565 | 3.37377872340738,1.47313924422294 566 | 3.37351367337518,1.48876996222962 567 | 3.37321799324517,1.50446431156019 568 | 3.37289322513698,1.52022046973092 569 | 3.37254091117023,1.53603661425805 570 | 3.37216259346457,1.55191092265782 571 | 3.37175981413961,1.56784157244649 572 | 3.37133411531499,1.5838267411403 573 | 3.37088703911034,1.59986460625551 574 | 3.3704201276453,1.61595334530836 575 | 3.36993492303948,1.63209113581511 576 | 3.36943296741252,1.64827615529199 577 | 3.36891580288406,1.66450658125526 578 | 3.36838497157372,1.68078059122118 579 | 3.36784201560112,1.69709636270597 580 | 3.36728847708592,1.71345207322591 581 | 3.36672589814772,1.72984590029723 582 | 3.36615582090617,1.74627602143618 583 | 3.36557978748089,1.76274061415901 584 | 3.36499933999151,1.77923785598198 585 | 3.36441602055767,1.79576592442133 586 | 3.36383137129899,1.8123229969933 587 | 3.3632469343351,1.82890725121415 588 | 3.36266425178565,1.84551686460013 589 | 3.36208486577024,1.86215001466749 590 | 3.36151031840852,1.87880487893247 591 | 3.36094215182012,1.89547963491132 592 | 3.36038190812467,1.91217246012029 593 | 3.35983112944179,1.92888153207564 594 | 3.35929135789112,1.9456050282936 595 | 3.35876413559228,1.96234112629044 596 | 3.35825100466492,1.97908800358239 597 | 3.35775350722865,1.9958438376857 598 | 3.35727318540311,2.01260680611663 599 | 3.35681158130793,2.02937508639143 600 | 3.35637023706274,2.04614685602634 601 | 3.35595069478716,2.06292029253761 602 | 3.35555408724519,2.07969379909861 603 | 3.35517990977821,2.09646668151122 604 | 3.35482724837195,2.11323847123443 605 | 3.35449518901214,2.13000869972724 606 | 3.35418281768454,2.14677689844863 607 | 3.35388922037487,2.16354259885762 608 | 3.35361348306887,2.18030533241318 609 | 3.35335469175227,2.19706463057433 610 | 3.35311193241083,2.21382002480004 611 | 3.35288429103026,2.23057104654932 612 | 3.35267085359631,2.24731722728116 613 | 3.35247070609471,2.26405809845456 614 | 3.35228293451121,2.28079319152852 615 | 3.35210662483153,2.29752203796202 616 | 3.35194086304142,2.31424416921406 617 | 3.35178473512661,2.33095911674364 618 | 3.35163732707283,2.34766641200976 619 | 3.35149772486584,2.3643655864714 620 | 3.35136501449135,2.38105617158757 621 | 3.35123828193511,2.39773769881725 622 | 3.35111661318286,2.41440969961946 623 | 3.35099909422033,2.43107170545316 624 | 3.35088481103326,2.44772324777738 625 | 3.35077284960739,2.46436385805109 626 | 3.35066229592845,2.4809930677333 627 | 3.35055223598218,2.497610408283 628 | 3.35044175575431,2.51421541115918 629 | 3.35032994123059,2.53080760782085 630 | 3.35021587839674,2.54738652972699 631 | 3.35009865323852,2.5639517083366 632 | 3.34997735174164,2.58050267510867 633 | 3.34985105989186,2.59703896150221 634 | 3.3497188636749,2.6135600989762 635 | 3.34957984907651,2.63006561898965 636 | 3.34943310208242,2.64655505300154 637 | 3.34927770867836,2.66302793247087 638 | 3.34911275485008,2.67948378885664 639 | 3.34893732658331,2.69592215361785 640 | 3.34875050986379,2.71234255821348 641 | 3.34855139067725,2.72874453410253 642 | 3.34833905500943,2.745127612744 643 | 3.34811258884607,2.76149132559688 644 | 3.3478710781729,2.77783520412018 645 | 3.34761360897567,2.79415877977287 646 | 3.3473392672401,2.81046158401396 647 | 3.34704713895194,2.82674314830245 648 | 3.34673631009693,2.84300300409733 649 | 3.34640586666079,2.85924068285759 650 | 3.34605489462926,2.87545571604223 651 | 3.34568247998809,2.89164763511024 652 | 3.34528794318179,2.90781609590446 653 | 3.34487154249002,2.92396125180303 654 | 3.34443377065123,2.94008338056795 655 | 3.34397512040386,2.9561827599612 656 | 3.34349608448634,2.97225966774477 657 | 3.34299715563714,2.98831438168066 658 | 3.34247882659468,3.00434717953084 659 | 3.34194159009741,3.02035833905731 660 | 3.34138593888377,3.03634813802205 661 | 3.34081236569221,3.05231685418706 662 | 3.34022136326118,3.06826476531431 663 | 3.3396134243291,3.0841921491658 664 | 3.33898904163444,3.10009928350352 665 | 3.33834870791562,3.11598644608945 666 | 3.33769291591109,3.13185391468559 667 | 3.33702215835931,3.14770196705391 668 | 3.3363369279987,3.16353088095641 669 | 3.33563771756771,3.17934093415508 670 | 3.33492501980479,3.19513240441191 671 | 3.33419932744838,3.21090556948887 672 | 3.33346113323692,3.22666070714797 673 | 3.33271092990885,3.24239809515118 674 | 3.33194921020262,3.2581180112605 675 | 3.33117646685668,3.27382073323792 676 | 3.33039319260945,3.28950653884542 677 | 3.3295998801994,3.30517570584499 678 | 3.32879702236495,3.32082851199861 679 | 3.32798511184456,3.33646523506829 680 | 3.32716464137667,3.352086152816 681 | 3.32633610369972,3.36769154300373 682 | 3.32549999155215,3.38328168339347 683 | 3.3246567976724,3.39885685174721 684 | 3.32380701479893,3.41441732582694 685 | 3.32295113567017,3.42996338339465 686 | 3.32208965302456,3.44549530221231 687 | 3.32122305960056,3.46101336004193 688 | 3.32035184813659,3.47651783464549 689 | 3.31947651137111,3.49200900378497 690 | 3.31859754204256,3.50748714522237 691 | 3.31771543288938,3.52295253671967 692 | 3.31683067665002,3.53840545603886 693 | 3.31594376606291,3.55384618094194 694 | 3.3150551938665,3.56927498919087 695 | 3.31416545279924,3.58469215854767 696 | 3.31327503559957,3.60009796677431 697 | 3.31238443500592,3.61549269163278 698 | 3.31149414375675,3.63087661088507 699 | 3.3106046545905,3.64625000229316 700 | 3.3097164602456,3.66161314361906 701 | 3.30883005346051,3.67696631262473 702 | 3.30794528422591,3.69230961972738 703 | 3.30705943154147,3.70764250596499 704 | 3.30616913165912,3.72296424503075 705 | 3.30527102083076,3.73827411061784 706 | 3.30436173530831,3.75357137641947 707 | 3.3034379113437,3.76885531612882 708 | 3.30249618518884,3.78412520343907 709 | 3.30153319309564,3.79938031204342 710 | 3.30054557131603,3.81461991563505 711 | 3.29952995610192,3.82984328790716 712 | 3.29848298370523,3.84504970255294 713 | 3.29740129037787,3.86023843326556 714 | 3.29628151237176,3.87540875373823 715 | 3.29512028593882,3.89055993766414 716 | 3.29391424733097,3.90569125873646 717 | 3.29266003280013,3.9208019906484 718 | 3.2913542785982,3.93589140709314 719 | 3.28999362097712,3.95095878176386 720 | 3.28857469618878,3.96600338835376 721 | 3.28709414048513,3.98102450055604 722 | 3.28554859011806,3.99602139206387 723 | 3.28393468133949,4.01099333657044 724 | 3.28224905040136,4.02593960776895 725 | 3.28048833355556,4.04085947935259 726 | 3.27864916705402,4.05575222501454 727 | 3.27672818714866,4.070617118448 728 | 3.27472203009139,4.08545343334615 729 | 3.27262733213413,4.10026044340218 730 | 3.2704407295288,4.11503742230928 731 | 3.26815885852731,4.12978364376064 732 | 3.26577835538159,4.14449838144945 733 | 3.26329585634354,4.15918090906891 734 | 3.26070799766509,4.17383050031219 735 | 3.25801141559815,4.18844642887249 736 | 3.25520274639464,4.20302796844299 737 | 3.25227862630648,4.2175743927169 738 | 3.24923569158559,4.23208497538738 739 | 3.24607057848388,4.24655899014765 740 | 3.24277992325326,4.26099571069088 741 | 3.23936036214567,4.27539441071026 742 | 3.235808531413,4.28975436389898 743 | 3.23212106730719,4.30407484395024 744 | 3.22829460608014,4.31835512455722 745 | 3.22432578398378,4.33259447941311 746 | 3.22021123727003,4.3467921822111 747 | 3.21594760219079,4.36094750664438 748 | 3.21153151499799,4.37505972640613 749 | 3.20695961194354,4.38912811518956 750 | 3.20222852927936,4.40315194668784 751 | 3.19733490325737,4.41713049459417 752 | 3.1922764071782,4.43106278106866 753 | 3.18705486253734,4.44494682213909 754 | 3.181673127879,4.4587803823002 755 | 3.17613406174738,4.47256122604668 756 | 3.17044052268668,4.48628711787327 757 | 3.16459536924111,4.49995582227466 758 | 3.15860145995487,4.51356510374559 759 | 3.15246165337217,4.52711272678076 760 | 3.14617880803722,4.54059645587489 761 | 3.13975578249421,4.55401405552269 762 | 3.13319543528736,4.56736329021888 763 | 3.12650062496086,4.58064192445818 764 | 3.11967421005893,4.5938477227353 765 | 3.11271904912576,4.60697844954496 766 | 3.10563800070556,4.62003186938186 767 | 3.09843392334255,4.63300574674074 768 | 3.09110967558091,4.6458978461163 769 | 3.08366811596486,4.65870593200325 770 | 3.07611210303859,4.67142776889632 771 | 3.06844449534633,4.68406112129022 772 | 3.06066815143226,4.69660375367967 773 | 3.0527859298406,4.70905343055937 774 | 3.04480068911555,4.72140791642405 775 | 3.03671528780132,4.73366497576842 776 | 3.0285325844421,4.7458223730872 777 | 3.02025543758211,4.75787787287509 778 | 3.01188670576554,4.76982923962683 779 | 3.00342924753661,4.78167423783712 780 | 2.99488592143952,4.79341063200067 781 | 2.98625958601846,4.80503618661221 782 | 2.97755309981766,4.81654866616645 783 | 2.96876932138131,4.8279458351581 784 | 2.95991110925362,4.83922545808188 785 | 2.95098132197879,4.85038529943251 786 | 2.94198281810102,4.8614231237047 787 | 2.93291845616453,4.87233669539316 788 | 2.92379109471351,4.88312377899262 789 | 2.91460359229217,4.89378213899778 790 | 2.90535880744472,4.90430953990337 791 | 2.89605959871536,4.91470374620409 792 | 2.88670882464829,4.92496252239467 793 | 2.87730934378772,4.93508363296981 794 | 2.86786401467786,4.94506484242424 795 | 2.85837569586291,4.95490391525267 796 | 2.84884724588707,4.96459861594981 797 | 2.83928152329455,4.97414670901038 798 | 2.82968138662955,4.9835459589291 799 | 2.82004969443628,4.99279413020069 800 | 2.81038930525895,5.00188898731984 801 | 2.80070307764175,5.01082829478129 802 | 2.79099327881149,5.01961049186245 803 | 2.7812598107254,5.02823671697152 804 | 2.77150198402327,5.0367087832994 805 | 2.76171910934494,5.04502850403699 806 | 2.7519104973302,5.05319769237521 807 | 2.74207545861889,5.06121816150495 808 | 2.7322133038508,5.06909172461711 809 | 2.72232334366577,5.0768201949026 810 | 2.7124048887036,5.08440538555232 811 | 2.70245724960411,5.09184910975718 812 | 2.69247973700711,5.09915318070808 813 | 2.68247166155242,5.10631941159591 814 | 2.67243233387985,5.11334961561159 815 | 2.66236106462923,5.12024560594601 816 | 2.65225716444036,5.12700919579008 817 | 2.64211994395305,5.13364219833471 818 | 2.63194871380714,5.14014642677079 819 | 2.62174278464242,5.14652369428923 820 | 2.61150146709872,5.15277581408093 821 | 2.60122407181584,5.15890459933679 822 | 2.59090990943362,5.16491186324772 823 | 2.58055829059185,5.17079941900461 824 | 2.57016852593036,5.17656907979839 825 | 2.55973992608896,5.18222265881993 826 | 2.54927180170746,5.18776196926016 827 | 2.53876346342569,5.19318882430997 828 | 2.52821422188345,5.19850503716026 829 | 2.51762338772056,5.20371242100194 830 | 2.50699027157684,5.20881278902591 831 | 2.4963141840921,5.21380795442307 832 | 2.48559443590616,5.21869973038433 833 | 2.47483033765883,5.22348993010058 834 | 2.46402119998993,5.22818036676274 835 | 2.45316633353926,5.23277285356171 836 | 2.44226504894666,5.23726920368838 837 | 2.43131665685193,5.24167123033367 838 | 2.42032046789488,5.24598074668847 839 | 2.40927579271534,5.25019956594368 840 | 2.39818194195312,5.25432950129022 841 | 2.38703822624803,5.25837236591898 842 | 2.37584395623988,5.26232997302086 843 | 2.3645984425685,5.26620413578677 844 | 2.3533009958737,5.26999666740762 845 | 2.34195092679529,5.2737093810743 846 | 2.33054754597309,5.27734408997772 847 | 2.31909016404692,5.28090260730877 848 | 2.30757809165658,5.28438674625837 849 | 2.2960106394419,5.28779832001742 850 | 2.28438711804268,5.29113914177682 851 | 2.27270683809876,5.29441102472747 852 | 2.26096926064213,5.29761556233972 853 | 2.24917444827363,5.30075346920172 854 | 2.2373226139863,5.30382524018104 855 | 2.22541397077317,5.30683137014528 856 | 2.21344873162727,5.30977235396202 857 | 2.20142710954163,5.31264868649884 858 | 2.18934931750929,5.31546086262334 859 | 2.17721556852328,5.31820937720311 860 | 2.16502607557663,5.32089472510571 861 | 2.15278105166237,5.32351740119876 862 | 2.14048070977354,5.32607790034982 863 | 2.12812526290316,5.32857671742649 864 | 2.11571492404428,5.33101434729635 865 | 2.10324990618993,5.33339128482699 866 | 2.09073042233313,5.335708024886 867 | 2.07815668546693,5.33796506234096 868 | 2.06552890858434,5.34016289205946 869 | 2.05284730467842,5.34230200890909 870 | 2.04011208674218,5.34438290775743 871 | 2.02732346776866,5.34640608347206 872 | 2.0144816607509,5.34837203092058 873 | 2.00158687868192,5.35028124497058 874 | 1.98863933455476,5.35213422048963 875 | 1.97563924136246,5.35393145234532 876 | 1.96258681209804,5.35567343540525 877 | 1.94948225975454,5.35736066453699 878 | 1.93632579732499,5.35899363460814 879 | 1.92311763780242,5.36057284048627 880 | 1.90985799417987,5.36209877703899 881 | 1.89654707945037,5.36357193913387 882 | 1.88318510660695,5.36499282163849 883 | 1.86977228864264,5.36636191942046 884 | 1.85630883855048,5.36767972734734 885 | 1.8427949693235,5.36894674028674 886 | 1.82923089395473,5.37016345310623 887 | 1.81561682543721,5.3713303606734 888 | 1.80195297676396,5.37244795785585 889 | 1.78823956092803,5.37351673952114 890 | 1.77447679092243,5.37453720053688 891 | 1.76066487974022,5.37550983577065 892 | 1.74680404037441,5.37643514009003 893 | 1.73289448581805,5.37731360836261 894 | 1.71893642906416,5.37814573545598 895 | 1.70493008310577,5.37893201623773 896 | 1.69087566093593,5.37967294557543 897 | 1.67677337554766,5.38036901833668 898 | 1.66262343993399,5.38102072938907 899 | 1.64842606708796,5.38162857360017 900 | 1.6341814700026,5.38219304583758 901 | 1.61988986167095,5.38271464096889 902 | 1.60555142338741,5.38319396768934 903 | 1.5911662096519,5.38363209000484 904 | 1.57673424326571,5.38403018574898 905 | 1.56225554703015,5.38438943275532 906 | 1.54773014374651,5.38471100885746 907 | 1.53315805621608,5.38499609188895 908 | 1.51853930724017,5.38524585968339 909 | 1.50387391962005,5.38546149007434 910 | 1.48916191615704,5.38564416089539 911 | 1.47440331965242,5.3857950499801 912 | 1.4595981529075,5.38591533516206 913 | 1.44474643872356,5.38600619427485 914 | 1.42984819990191,5.38606880515203 915 | 1.41490345924383,5.38610434562719 916 | 1.39991223955063,5.38611399353391 917 | 1.3848745636236,5.38609892670575 918 | 1.36979045426403,5.38606032297631 919 | 1.35465993427322,5.38599936017914 920 | 1.33948302645247,5.38591721614784 921 | 1.32425975360307,5.38581506871597 922 | 1.30899013852632,5.38569409571712 923 | 1.29367420402351,5.38555547498485 924 | 1.27831197289594,5.38540038435275 925 | 1.26290346794491,5.3852300016544 926 | 1.2474487119717,5.38504550472337 927 | 1.23194772777762,5.38484807139324 928 | 1.21640053816396,5.38463887949758 929 | 1.20080716593202,5.38441910686996 930 | 1.18516763388309,5.38418993134398 931 | 1.16948196481846,5.3839525307532 932 | 1.15375018153944,5.38370808293121 933 | 1.13797230684732,5.38345776571157 934 | 1.12214836354339,5.38320275692786 935 | 1.10627837442895,5.38294423441366 936 | 1.09036236230529,5.38268337600256 937 | 1.07440034997372,5.38242135952811 938 | 1.05839236023552,5.38215936282391 939 | 1.042338415892,5.38189856372353 940 | 1.02623853974444,5.38164014006054 941 | 1.01009275459415,5.38138526966852 942 | 0.993901083242412,5.38113513038105 943 | 0.97766354849053,5.38089090003171 944 | 0.961380173139799,5.38065375645406 945 | 0.945050979991512,5.3804248774817 946 | 0.928675991846967,5.38020544094819 947 | 0.912255231507458,5.37999662468711 948 | 0.895788721774281,5.37979960653204 949 | 0.879276485448732,5.37961556431655 950 | 0.862718545332106,5.37944567587423 951 | 0.846114924225698,5.37929111903864 952 | 0.829465896284255,5.37915286449969 953 | 0.812772741076322,5.37903105437251 954 | 0.796036989523893,5.3789256236286 955 | 0.779260172548964,5.3788365072394 956 | 0.762443821073531,5.37876364017639 957 | 0.745589466019588,5.37870695741105 958 | 0.728698638309131,5.37866639391483 959 | 0.711772868864156,5.37864188465921 960 | 0.694813688606657,5.37863336461566 961 | 0.677822628458629,5.37864076875565 962 | 0.660801219342069,5.37866403205063 963 | 0.643750992178971,5.3787030894721 964 | 0.62667347789133,5.3787578759915 965 | 0.609570207401142,5.37882832658032 966 | 0.592442711630403,5.37891437621002 967 | 0.575292521501107,5.37901595985206 968 | 0.558121167935249,5.37913301247793 969 | 0.540930181854825,5.37926546905908 970 | 0.523721094181831,5.37941326456699 971 | 0.506495435838261,5.37957633397312 972 | 0.48925473774611,5.37975461224895 973 | 0.472000530827375,5.37994803436594 974 | 0.45473434600405,5.38015653529556 975 | 0.43745771419813,5.38038005000928 976 | 0.420172166331612,5.38061851347858 977 | 0.402879233326489,5.38087186067491 978 | 0.385580446104758,5.38114002656975 979 | 0.368277335588413,5.38142294613457 980 | 0.350971432699451,5.38172055434083 981 | 0.333664268359865,5.38203278616001 982 | 0.316357373491652,5.38235957656357 983 | 0.299052279016807,5.38270086052299 984 | 0.281750515857324,5.38305657300972 985 | 0.2644536149352,5.38342664899525 986 | 0.24716310717243,5.38381102345103 987 | 0.229880523491008,5.38420963134855 988 | 0.212607394812931,5.38462240765926 989 | 0.195345252060192,5.38504928735464 990 | 0.178095626154789,5.38549020540615 991 | 0.160860048018715,5.38594509678527 992 | 0.143640048573967,5.38641389646346 993 | 0.126437158742539,5.3868965394122 994 | 0.109252909446426,5.38739296060294 995 | 0.0920888316076249,5.38790309500716 996 | 0.0749464561481298,5.38842687759634 997 | 0.0578273139899362,5.38896424334193 998 | 0.0407329360550396,5.38951512721541 999 | 0.0236648532654349,5.39007946418824 1000 | 0.00662459654311764,5.3906571892319 1001 | -0.010386303189917,5.39124823731785 1002 | -0.0273667088018218,5.39185221351505 1003 | -0.0443170583213421,5.39246740328237 1004 | -0.061238183567371,5.39309176217616 1005 | -0.0781309163588019,5.39372324575279 1006 | -0.0949960885145282,5.39435980956862 1007 | -0.111834531853443,5.39499940917999 1008 | -0.12864707819444,5.39564000014328 1009 | -0.145434559356412,5.39627953801483 1010 | -0.162197807158252,5.396915978351 1011 | -0.178937653418855,5.39754727670816 1012 | -0.195654929957112,5.39817138864265 1013 | -0.212350468591918,5.39878626971084 1014 | -0.229025101142165,5.39938987546909 1015 | -0.245679659426748,5.39998016147375 1016 | -0.262314975264559,5.40055508328118 1017 | -0.278931880474491,5.40111259644773 1018 | -0.295531206875438,5.40165065652977 1019 | -0.312113786286294,5.40216721908366 1020 | -0.328680450525951,5.40266023966574 1021 | -0.345232031413303,5.40312767383238 1022 | -0.361769360767243,5.40356747713994 1023 | -0.378293270406665,5.40397760514477 1024 | -0.394804592150461,5.40435601340323 1025 | -0.411304157817525,5.40470065747168 1026 | -0.427792799226751,5.40500949290648 1027 | -0.444271348197031,5.40528047526398 1028 | -0.460740636547259,5.40551156010054 1029 | -0.477201496096328,5.40570070297252 1030 | -0.493654758663132,5.40584585943628 1031 | -0.510101256066564,5.40594498504817 1032 | -0.526541820125517,5.40599603536455 1033 | -0.542977282658884,5.40599696594179 1034 | -0.559408475485559,5.40594573233623 1035 | -0.575836230424435,5.40584029010423 1036 | -0.592261379294405,5.40567859480216 1037 | -0.608684753914363,5.40545860198637 1038 | -0.625107186103202,5.40517826721322 1039 | -0.641529507679816,5.40483554603906 1040 | -0.657952550463096,5.40442839402026 1041 | -0.674377146271938,5.40395476671316 1042 | -0.690804126925234,5.40341261967414 1043 | -0.707234324241877,5.40279990845954 1044 | -0.723668570040761,5.40211458862573 1045 | -0.740107696140779,5.40135461572905 1046 | -0.756552534360824,5.40051794532588 1047 | -0.77300391651979,5.39960253297256 1048 | -0.78946267443657,5.39860633422546 1049 | -0.805929639930058,5.39752730464093 1050 | -0.822405644819145,5.39636339977533 1051 | -0.838891520922727,5.39511257518502 1052 | -0.855387204695287,5.39377314947443 1053 | -0.87188905113367,5.39234489344031 1054 | -0.888392519870315,5.3908279409275 1055 | -0.904893070537659,5.38922242578082 1056 | -0.921386162768139,5.3875284818451 1057 | -0.937867256194191,5.38574624296516 1058 | -0.954331810448254,5.38387584298585 1059 | -0.970775285162764,5.38191741575199 1060 | -0.987193139970158,5.3798710951084 1061 | -1.00358083450287,5.37773701489991 1062 | -1.01993382839335,5.37551530897136 1063 | -1.03624758127402,5.37320611116757 1064 | -1.05251755277732,5.37080955533338 1065 | -1.06873920253569,5.3683257753136 1066 | -1.08490799018157,5.36575490495308 1067 | -1.1010193753474,5.36309707809663 1068 | -1.1170688176656,5.36035242858909 1069 | -1.13305177676863,5.35752109027529 1070 | -1.14896371228891,5.35460319700005 1071 | -1.16480008385888,5.35159888260821 1072 | -1.18055635111099,5.34850828094458 1073 | -1.19622797367766,5.34533152585402 1074 | -1.21181041119134,5.34206875118133 1075 | -1.22729912328445,5.33872009077135 1076 | -1.24268956958945,5.33528567846891 1077 | -1.25797720973876,5.33176564811884 1078 | -1.27315750336482,5.32816013356596 1079 | -1.28822591010008,5.32446926865511 1080 | -1.30317788957696,5.32069318723111 1081 | -1.31800890142791,5.3168320231388 1082 | -1.33271440528536,5.312885910223 1083 | -1.34728986078174,5.30885498232854 1084 | -1.36173072754951,5.30473937330024 1085 | -1.37603246522108,5.30053921698295 1086 | -1.39019053342891,5.29625464722149 1087 | -1.40420039180542,5.29188579786068 1088 | -1.41805749998306,5.28743280274536 1089 | -1.43175731759426,5.28289579572035 1090 | -1.44529530427146,5.27827491063048 1091 | -1.4586669196471,5.27357028132059 1092 | -1.4718676233536,5.26878204163549 1093 | -1.48489287502342,5.26391032542003 1094 | -1.49773813428899,5.25895526651903 1095 | -1.51039886078274,5.25391699877731 1096 | -1.52287051413711,5.24879565603971 1097 | -1.53514855398454,5.24359137215106 1098 | -1.54722843995747,5.23830428095618 1099 | -1.55910563168833,5.2329345162999 1100 | -1.57077558880956,5.22748221202705 1101 | -1.58223377095359,5.22194750198247 1102 | -1.59347696155705,5.21633076357887 1103 | -1.60450723927327,5.21063334850052 1104 | -1.61532800655975,5.20485685199962 1105 | -1.62594266587399,5.19900286932831 1106 | -1.63635461967352,5.19307299573878 1107 | -1.64656727041584,5.1870688264832 1108 | -1.65658402055845,5.18099195681374 1109 | -1.66640827255887,5.17484398198257 1110 | -1.6760434288746,5.16862649724186 1111 | -1.68549289196316,5.16234109784378 1112 | -1.69476006428205,5.15598937904051 1113 | -1.70384834828878,5.14957293608422 1114 | -1.71276114644086,5.14309336422707 1115 | -1.72150186119581,5.13655225872124 1116 | -1.73007389501111,5.12995121481891 1117 | -1.7384806503443,5.12329182777223 1118 | -1.74672552965287,5.11657569283339 1119 | -1.75481193539433,5.10980440525456 1120 | -1.7627432700262,5.1029795602879 1121 | -1.77052293600598,5.0961027531856 1122 | -1.77815433579118,5.08917557919981 1123 | -1.78564087183931,5.08219963358271 1124 | -1.79298594660788,5.07517651158648 1125 | -1.80019296255439,5.06810780846328 1126 | -1.80726532213636,5.06099511946529 1127 | -1.81420642781129,5.05384003984467 1128 | -1.8210196820367,5.04664416485361 1129 | -1.82770848727009,5.03940908974426 1130 | -1.83427624596897,5.03213640976881 1131 | -1.84072636059085,5.02482772017942 1132 | -1.84706223359324,5.01748461622827 1133 | -1.85328726743364,5.01010869316752 1134 | -1.85940486456958,5.00270154624935 1135 | -1.86541842745854,4.99526477072593 1136 | -1.87133135855805,4.98779996184944 1137 | -1.87714706032561,4.98030871487203 1138 | -1.88286893521874,4.97279262504589 1139 | -1.88850038569493,4.96525328762318 1140 | -1.8940448142117,4.95769229785609 1141 | -1.89950562322656,4.95011125099677 1142 | -1.90488621519702,4.9425117422974 1143 | -1.91018999258058,4.93489536701015 1144 | -1.91542035783476,4.9272637203872 1145 | -1.92058071341706,4.91961839768071 1146 | -1.92567446178499,4.91196099414286 1147 | -1.93070500539606,4.90429310502581 1148 | -1.93567574670778,4.89661632558175 1149 | -1.94059008817765,4.88893225106284 1150 | -1.9454514322632,4.88124247672124 1151 | -1.95026318142192,4.87354859780914 1152 | -1.95502819492936,4.8658516237501 1153 | -1.9597471593332,4.85815022065318 1154 | -1.96442021799918,4.85044246879887 1155 | -1.96904751429301,4.84272644846762 1156 | -1.97362919158041,4.83500023993992 1157 | -1.97816539322711,4.82726192349622 1158 | -1.98265626259884,4.819509579417 1159 | -1.9871019430613,4.81174128798273 1160 | -1.99150257798024,4.80395512947386 1161 | -1.99585831072136,4.79614918417088 1162 | -2.00016928465039,4.78832153235425 1163 | -2.00443564313306,4.78047025430444 1164 | -2.00865752953508,4.77259343030191 1165 | -2.01283508722219,4.76468914062714 1166 | -2.01696845956009,4.75675546556059 1167 | -2.02105778991453,4.74879048538273 1168 | -2.02510322165121,4.74079228037404 1169 | -2.02910489813586,4.73275893081497 1170 | -2.0330629627342,4.724688516986 1171 | -2.03697755881197,4.7165791191676 1172 | -2.04084882973487,4.70842881764023 1173 | -2.04467691886863,4.70023569268436 1174 | -2.04846196957897,4.69199782458046 1175 | -2.05220412523163,4.68371329360901 1176 | -2.05590352919231,4.67538018005046 1177 | -2.05956032482675,4.66699656418529 1178 | -2.06317465550066,4.65856052629396 1179 | -2.06674666457977,4.65007014665695 1180 | -2.0702764954298,4.64152350555471 1181 | -2.07376429141647,4.63291868326773 1182 | -2.07721019590551,4.62425376007647 1183 | -2.08061435226264,4.61552681626139 1184 | -2.08397690385359,4.60673593210297 1185 | -2.08729799404406,4.59787918788167 1186 | -2.0905777661998,4.58895466387797 1187 | -2.09381636368651,4.57996044037232 1188 | -2.09701392986993,4.57089459764521 1189 | -2.10017060811577,4.56175521597709 1190 | -2.10328654178976,4.55254037564844 1191 | -2.10636187425762,4.54324815693972 1192 | -2.10939674888507,4.53387664013141 1193 | -2.11239130903784,4.52442390550396 1194 | -2.11534569808165,4.51488803333786 1195 | -2.11826005938222,4.50526710391356 1196 | -2.12113453630528,4.49555919751155 1197 | -2.12396927221654,4.48576239441227 1198 | -2.12676441048174,4.47587477489621 1199 | -2.12952009446659,4.46589441924384 1200 | -2.13223646753681,4.45581940773561 1201 | -2.13491367305813,4.445647820652 1202 | -2.13755176558402,4.43537809125174 1203 | -2.14015044441897,4.42501006470656 1204 | -2.14270932005521,4.41454393916646 1205 | -2.14522800298498,4.40397991278144 1206 | -2.14770610370051,4.3933181837015 1207 | -2.15014323269404,4.38255895007664 1208 | -2.15253900045781,4.37170241005686 1209 | -2.15489301748404,4.36074876179214 1210 | -2.15720489426497,4.3496982034325 1211 | -2.15947424129285,4.33855093312793 1212 | -2.1617006690599,4.32730714902842 1213 | -2.16388378805836,4.31596704928398 1214 | -2.16602320878047,4.3045308320446 1215 | -2.16811854171845,4.29299869546028 1216 | -2.17016939736456,4.28137083768101 1217 | -2.17217538621101,4.26964745685681 1218 | -2.17413611875005,4.25782875113765 1219 | -2.17605120547392,4.24591491867355 1220 | -2.17792025687484,4.2339061576145 1221 | -2.17974288344506,4.2218026661105 1222 | -2.1815186956768,4.20960464231154 1223 | -2.18324730406231,4.19731228436762 1224 | -2.18492831909382,4.18492579042875 1225 | -2.18656135126356,4.17244535864491 1226 | -2.18814601106378,4.15987118716612 1227 | -2.1896819089867,4.14720347414235 1228 | -2.19116865552456,4.13444241772362 1229 | -2.1926058611696,4.12158821605992 1230 | -2.19399313641404,4.10864106730125 1231 | -2.19533009175014,4.09560116959761 1232 | -2.19661633767012,4.08246872109899 1233 | -2.19785148466621,4.0692439199554 1234 | -2.19903514323066,4.05592696431682 1235 | -2.2001669238557,4.04251805233326 1236 | -2.20124643703357,4.02901738215472 1237 | -2.20227329325649,4.01542515193119 1238 | -2.2032471030167,4.00174155981268 1239 | -2.20416747680645,3.98796680394917 1240 | -2.20503402511796,3.97410108249067 1241 | -2.20584635844348,3.96014459358718 1242 | -2.20660408727523,3.94609753538869 1243 | -2.20730682210545,3.93196010604521 1244 | -2.20795417342638,3.91773250370672 1245 | -2.20854575173025,3.90341492652323 1246 | -2.2090811675093,3.88900757264473 1247 | -2.20956003125577,3.87451064022123 1248 | -2.20998195346188,3.85992432740272 1249 | -2.21034654461988,3.8452488323392 1250 | -2.21065341522199,3.83048435318066 1251 | -2.21090217576046,3.81563108807711 1252 | -2.2110928130739,3.80068938326507 1253 | -2.21122681938647,3.78566017732716 1254 | -2.21130606326867,3.77054455693252 1255 | -2.21133241329105,3.75534360875031 1256 | -2.21130773802413,3.74005841944967 1257 | -2.21123390603844,3.72469007569975 1258 | -2.21111278590451,3.70923966416969 1259 | -2.21094624619286,3.69370827152864 1260 | -2.21073615547402,3.67809698444576 1261 | -2.21048438231852,3.66240688959018 1262 | -2.21019279529689,3.64663907363106 1263 | -2.20986326297965,3.63079462323753 1264 | -2.20949765393734,3.61487462507876 1265 | -2.20909783674047,3.59888016582387 1266 | -2.20866567995959,3.58281233214204 1267 | -2.20820305216521,3.56667221070239 1268 | -2.20771182192786,3.55046088817407 1269 | -2.20719385781807,3.53417945122625 1270 | -2.20665102840637,3.51782898652805 1271 | -2.20608520226329,3.50141058074863 1272 | -2.20549824795935,3.48492532055713 1273 | -2.20489203406508,3.46837429262271 1274 | -2.20426842915102,3.4517585836145 1275 | -2.20362930178767,3.43507928020166 1276 | -2.20297652054559,3.41833746905334 1277 | -2.20231195399528,3.40153423683867 1278 | -2.20163747070729,3.38467067022682 1279 | -2.20095493925213,3.36774785588692 1280 | -2.20026622820034,3.35076688048811 1281 | -2.19957320612244,3.33372883069956 1282 | -2.19887774158897,3.31663479319041 1283 | -2.19818170317044,3.29948585462979 1284 | -2.19748695943738,3.28228310168687 1285 | -2.19679537896033,3.26502762103079 1286 | -2.19610883030981,3.24772049933069 1287 | -2.19542918205635,3.23036282325572 1288 | -2.19475830277048,3.21295567947503 1289 | -2.19409806102272,3.19550015465776 1290 | -2.19345032538361,3.17799733547307 1291 | -2.19281696442366,3.1604483085901 1292 | -2.19219984671341,3.14285416067799 1293 | -2.19160084082339,3.1252159784059 1294 | -2.19102181532412,3.10753484844297 1295 | -2.19046463878613,3.08981185745835 1296 | -2.18993117977994,3.07204809212119 1297 | -2.1894233068761,3.05424463910063 1298 | -2.18894288864511,3.03640258506581 1299 | -2.18849179365752,3.0185230166859 1300 | -2.18807189048385,3.00060702063003 1301 | -2.18768504769462,2.98265568356735 1302 | -2.18733262403744,2.96467005592319 1303 | -2.18701393896818,2.94665104314766 1304 | -2.1867278021198,2.92859951444703 1305 | -2.18647302312524,2.9105163390276 1306 | -2.18624841161746,2.89240238609563 1307 | -2.18605277722941,2.87425852485742 1308 | -2.18588492959403,2.85608562451925 1309 | -2.18574367834428,2.8378845542874 1310 | -2.1856278331131,2.81965618336816 1311 | -2.18553620353346,2.80140138096781 1312 | -2.18546759923829,2.78312101629263 1313 | -2.18542082986055,2.7648159585489 1314 | -2.18539470503319,2.74648707694292 1315 | -2.18538803438916,2.72813524068096 1316 | -2.18539962756141,2.70976131896931 1317 | -2.18542829418289,2.69136618101424 1318 | -2.18547284388655,2.67295069602206 1319 | -2.18553208630534,2.65451573319902 1320 | -2.18560483107221,2.63606216175143 1321 | -2.18568988782011,2.61759085088557 1322 | -2.185786066182,2.59910266980771 1323 | -2.18589217579082,2.58059848772414 1324 | -2.18600702627952,2.56207917384115 1325 | -2.18612942728105,2.54354559736501 1326 | -2.18625818842837,2.52499862750202 1327 | -2.18639211935442,2.50643913345845 1328 | -2.18653002969216,2.48786798444059 1329 | -2.18667072907453,2.46928604965473 1330 | -2.18681302713449,2.45069419830713 1331 | -2.18695573350498,2.4320932996041 1332 | -2.18709765781896,2.41348422275191 1333 | -2.18723760970938,2.39486783695685 1334 | -2.18737439880918,2.37624501142519 1335 | -2.18750683475132,2.35761661536323 1336 | -2.18763372716875,2.33898351797725 1337 | -2.18775388569442,2.32034658847352 1338 | -2.18786611996127,2.30170669605834 1339 | -2.18796923960227,2.28306470993798 1340 | -2.18806205425035,2.26442149931874 1341 | -2.18814337353847,2.24577793340689 1342 | -2.18821200709958,2.22713488140872 1343 | -2.18826676456664,2.2084932125305 1344 | -2.18830645557258,2.18985379597853 1345 | -2.18832988975037,2.17121750095909 1346 | -2.18833587673295,2.15258519667846 1347 | -2.18832322615327,2.13395775234293 1348 | -2.18829074764428,2.11533603715877 1349 | -2.18823725083893,2.09672092033228 1350 | -2.18816154537018,2.07811327106973 1351 | -2.18806244087097,2.05951395857741 1352 | -2.18793889360071,2.04092390448276 1353 | -2.18779044632464,2.0223442400979 1354 | -2.18761678843443,2.00377614915609 1355 | -2.18741760932177,1.98522081539061 1356 | -2.18719259837835,1.96667942253471 1357 | -2.18694144499586,1.94815315432168 1358 | -2.18666383856598,1.92964319448479 1359 | -2.18635946848039,1.9111507267573 1360 | -2.1860280241308,1.89267693487249 1361 | -2.18566919490887,1.87422300256362 1362 | -2.1852826702063,1.85579011356397 1363 | -2.18486813941478,1.8373794516068 1364 | -2.18442529192598,1.8189922004254 1365 | -2.1839538171316,1.80062954375301 1366 | -2.18345340442332,1.78229266532293 1367 | -2.18292374319284,1.76398274886842 1368 | -2.18236452283182,1.74570097812274 1369 | -2.18177543273197,1.72744853681917 1370 | -2.18115616228497,1.70922660869098 1371 | -2.1805064008825,1.69103637747144 1372 | -2.17982583791625,1.67287902689383 1373 | -2.17911416277791,1.6547557406914 1374 | -2.17837106485916,1.63666770259743 1375 | -2.17759623355169,1.61861609634519 1376 | -2.17678935824719,1.60060210566796 1377 | -2.17595012833734,1.582626914299 1378 | -2.17507823321383,1.56469170597158 1379 | -2.17417336226834,1.54679766441897 1380 | -2.17323520489256,1.52894597337445 1381 | -2.17226345047818,1.51113781657128 1382 | -2.17125778841689,1.49337437774274 1383 | -2.17021790810036,1.47565684062209 1384 | -2.16914349892029,1.4579863889426 1385 | -2.16803425026837,1.44036420643755 1386 | -2.16688985153627,1.4227914768402 1387 | -2.16570999211569,1.40526938388384 1388 | -2.16449436139831,1.38779911130171 1389 | -2.16324264877582,1.37038184282711 1390 | -2.1619545436399,1.35301876219329 1391 | -2.16062973538224,1.33571105313353 1392 | -2.15926791339453,1.3184598993811 1393 | -2.15786876706845,1.30126648466927 1394 | -2.1564319857957,1.2841319927313 1395 | -2.15495725896794,1.26705760730048 1396 | -2.15344427597688,1.25004451211007 1397 | -2.1518927262142,1.23309389089333 1398 | -2.15030229907159,1.21620692738355 1399 | -2.14867268394072,1.19938480531398 1400 | -2.1470035702133,1.18262870841791 1401 | -2.14529464728099,1.1659398204286 1402 | -2.1435456018318,1.14931956426808 1403 | -2.14175610973888,1.13277031961336 1404 | -2.13992584417171,1.11629470533022 1405 | -2.13805447829977,1.09989534028443 1406 | -2.1361416852925,1.08357484334178 1407 | -2.1341871383194,1.06733583336803 1408 | -2.13219051054992,1.05118092922895 1409 | -2.13015147515354,1.03511274979033 1410 | -2.12806970529973,1.01913391391794 1411 | -2.12594487415795,1.00324704047754 1412 | -2.12377665489767,0.987454748334925 1413 | -2.12156472068837,0.971759656355855 1414 | -2.11930874469951,0.95616438340611 1415 | -2.11700840010056,0.940671548351463 1416 | -2.114663360061,0.925283770057689 1417 | -2.11227329775028,0.91000366739056 1418 | -2.10983788633789,0.894833859215853 1419 | -2.10735679899329,0.879776964399342 1420 | -2.10482970888595,0.864835601806799 1421 | -2.10225628918534,0.850012390304001 1422 | -2.09963621306092,0.835309948756721 1423 | -2.09696915368217,0.820730896030733 1424 | -2.09425478421857,0.806277850991812 1425 | -2.09149277783956,0.791953432505732 1426 | -2.08868280771464,0.777760259438267 1427 | -2.08582454701326,0.763700950655192 1428 | -2.08291766890489,0.749778125022281 1429 | -2.07996184655901,0.735994401405308 1430 | -2.07695675314508,0.722352398670048 1431 | -2.07390206183258,0.708854735682275 1432 | -2.07079744579097,0.695504031307762 1433 | -2.06764257818972,0.682302904412285 1434 | -2.0644371321983,0.669253973861618 1435 | -2.06118078098618,0.656359858521535 1436 | -2.05787319772284,0.64362317725781 1437 | -2.05451405557773,0.631046548936218 1438 | -2.05110302772034,0.618632592422533 1439 | -2.04763978732012,0.606383926582529 1440 | -2.04412400754655,0.59430317028198 1441 | -2.04055536156909,0.582392942386662 1442 | -2.03693352255723,0.570655861762347 1443 | -2.03325816368042,0.559094547274811 1444 | -2.02952895810814,0.547711617789828 1445 | -2.02574557900985,0.536509692173171 1446 | -2.02190769955502,0.525491389290616 1447 | -2.01801499291313,0.514659328007936 1448 | -2.01406713225365,0.504016127190907 1449 | -2.01006379074603,0.493564405705301 1450 | -2.00600464155976,0.483306782416894 1451 | -2.0018893578643,0.47324587619146 1452 | -1.9977178154328,0.46338337931648 1453 | -1.99349070045315,0.453717277766267 1454 | -1.9892089017169,0.444244630936839 1455 | -1.9848733080156,0.434962498224216 1456 | -1.98048480814083,0.425867939024416 1457 | -1.97604429088414,0.41695801273346 1458 | -1.9715526450371,0.408229778747366 1459 | -1.96701075939126,0.399680296462153 1460 | -1.96241952273818,0.391306625273841 1461 | -1.95777982386942,0.383105824578449 1462 | -1.95309255157654,0.375074953771996 1463 | -1.94835859465111,0.367211072250501 1464 | -1.94357884188469,0.359511239409983 1465 | -1.93875418206883,0.351972514646462 1466 | -1.93388550399509,0.344591957355957 1467 | -1.92897369645503,0.337366626934487 1468 | -1.92401964824022,0.330293582778071 1469 | -1.91902424814222,0.323369884282728 1470 | -1.91398838495258,0.316592590844478 1471 | -1.90891294746286,0.309958761859339 1472 | -1.90379882446463,0.303465456723332 1473 | -1.89864690474945,0.297109734832474 1474 | -1.89345807710887,0.290888655582786 1475 | -1.88823323033446,0.284799278370287 1476 | -1.88297325321778,0.278838662590995 1477 | -1.87767903455038,0.27300386764093 1478 | -1.87235146312382,0.267291952916111 1479 | -1.86699142772968,0.261699977812557 1480 | -1.8615998171595,0.256225001726288 1481 | -1.85617752020485,0.250864084053323 1482 | -1.85072542565729,0.24561428418968 1483 | -1.84524442230837,0.24047266153138 1484 | -1.83973539894967,0.23543627547444 1485 | -1.83419924437273,0.230502185414881 1486 | -1.82863684736912,0.225667450748722 1487 | -1.82304909673039,0.220929130871982 1488 | -1.81743688124812,0.216284285180679 1489 | -1.81180108971386,0.211729973070834 1490 | -1.80614261091916,0.207263253938465 1491 | -1.8004623336556,0.202881187179592 1492 | -1.79476114671472,0.198580832190233 1493 | -1.7890399388881,0.194359248366409 1494 | -1.78329959896728,0.190213495104138 1495 | -1.77754101574384,0.186140631799439 1496 | -1.77176507800933,0.182137717848331 1497 | -1.7659726745553,0.178201812646835 1498 | -1.76016469417333,0.174329975590968 1499 | -1.75434202565497,0.17051926607675 1500 | -1.74850555779178,0.166766743500201 1501 | -1.74265617937532,0.163069467257339 1502 | -1.73679414051082,0.159424903158788 1503 | -1.73091713655816,0.155832142673587 1504 | -1.7250222241909,0.152290683685379 1505 | -1.71910646008257,0.148800024077809 1506 | -1.71316690090674,0.145359661734518 1507 | -1.70720060333696,0.14196909453915 1508 | -1.70120462404676,0.138627820375349 1509 | -1.69517601970971,0.135335337126759 1510 | -1.68911184699935,0.132091142677021 1511 | -1.68300916258924,0.128894734909781 1512 | -1.67686502315291,0.12574561170868 1513 | -1.67067648536394,0.122643270957363 1514 | -1.66444060589585,0.119587210539472 1515 | -1.65815444142222,0.116576928338652 1516 | -1.65181504861657,0.113611922238545 1517 | -1.64541948415247,0.110691690122795 1518 | -1.63896480470347,0.107815729875044 1519 | -1.63244806694311,0.104983539378938 1520 | -1.62586632754494,0.102194616518118 1521 | -1.61921664318252,0.0994484591762276 1522 | -1.6124960705294,0.096744565236911 1523 | -1.60570166625912,0.0940824325838111 1524 | -1.59883048704524,0.0914615591005714 1525 | -1.5918795895613,0.088881442670835 1526 | -1.58484603048086,0.0863415811782454 1527 | -1.57772686647747,0.0838414725064459 1528 | -1.57051915422467,0.0813806145390798 1529 | -1.56321995039602,0.0789585051597905 1530 | -1.55582631166507,0.0765746422522213 1531 | -1.54833529470537,0.0742285237000154 1532 | -1.54074395619046,0.0719196473868164 1533 | -1.5330493527939,0.0696475111962674 1534 | -1.52524854118924,0.0674116130120119 1535 | -1.51733857805002,0.0652114507176931 1536 | -1.5093165200498,0.0630465221969544 1537 | -1.50117942386214,0.0609163253334392 1538 | -1.49292434616057,0.0588203580107908 1539 | -1.48454834361864,0.0567581181126524 1540 | -1.47604847290992,0.0547291035226675 1541 | -1.46742179070795,0.0527328121244794 1542 | -1.45866535368627,0.0507687418017314 1543 | -1.44977621851845,0.0488363904380668 1544 | -1.44075144187802,0.046935255917129 1545 | -1.43158808043855,0.0450648361225614 1546 | -1.42228319087357,0.0432246289380072 1547 | -1.41283382985665,0.0414141322471098 1548 | -1.40323705406132,0.0396328439335125 1549 | -1.39348992016115,0.0378802618808586 1550 | -1.38358948482967,0.0361558839727916 1551 | -1.37353280474045,0.0344592080929547 1552 | -1.36331795774838,0.0327897251446778 1553 | -1.3529471064338,0.0311468981100369 1554 | -1.34242343455839,0.0295301829907945 1555 | -1.33175012588383,0.0279390357887132 1556 | -1.32093036417181,0.0263729125055555 1557 | -1.309967333184,0.0248312691430838 1558 | -1.29886421668211,0.0233135617030609 1559 | -1.2876241984278,0.0218192461872491 1560 | -1.27625046218276,0.020347778597411 1561 | -1.26474619170868,0.0188986149353092 1562 | -1.25311457076725,0.0174712112027061 1563 | -1.24135878312013,0.0160650234013644 1564 | -1.22948201252903,0.0146795075330465 1565 | -1.21748744275562,0.0133141195995149 1566 | -1.20537825756158,0.0119683156025322 1567 | -1.19315764070861,0.010641551543861 1568 | -1.18082877595838,0.00933328342526373 1569 | -1.16839484707258,0.00804296724850294 1570 | -1.15585903781289,0.00677005901534117 1571 | -1.143224531941,0.00551401472754094 1572 | -1.13049451321859,0.0042742903868648 1573 | -1.11767216540734,0.00305034199507526 1574 | -1.10476067226895,0.00184162555393487 1575 | -1.09176321756508,0.000647597065206153 1576 | -1.07868298505743,-0.000532287469348372 1577 | -1.06552315850768,-0.00169857204796615 1578 | -1.05228692167752,-0.00285180066888468 1579 | -1.03897745832863,-0.00399251733034141 1580 | -1.02559795222268,-0.00512126603057381 1581 | -1.01215158712138,-0.00623859076781936 1582 | -0.99864154678639,-0.00734503554031552 1583 | -0.985071014979409,-0.00844114434629976 1584 | -0.971443175462116,-0.00952746118400956 1585 | -0.957761211996195,-0.0106045300516824 1586 | -0.944028308343331,-0.0116728949475557 1587 | -0.930247648265207,-0.012733099869867 1588 | -0.916422415523507,-0.0137856888168537 1589 | -0.902555793879916,-0.0148312057867533 1590 | -0.888650967096116,-0.0158701947778032 1591 | -0.874711118933793,-0.016903199788241 1592 | -0.86073943315463,-0.0179307648163041 1593 | -0.846739093520312,-0.01895343386023 1594 | -0.832713283792521,-0.0199717509182561 1595 | -0.818665187732943,-0.02098625998862 1596 | -0.80459798910326,-0.021997505069559 1597 | -0.790514871665158,-0.0230060301593107 1598 | -0.77641901918032,-0.0240123792561125 1599 | -0.76231361541043,-0.0250170963582019 1600 | -0.748201844117172,-0.0260207254638164 1601 | -0.73408688906223,-0.0270238105711934 1602 | -0.719971934007288,-0.0280268956785704 1603 | -0.70586016271403,-0.0290305247841848 1604 | -0.69175475894414,-0.0300352418862742 1605 | -0.677658906459302,-0.031041590983076 1606 | -0.6635757890212,-0.0320501160728277 1607 | -0.649508590391518,-0.0330613611537667 1608 | -0.635460494331939,-0.0340758702241306 1609 | -0.621434684604149,-0.0350941872821567 1610 | --------------------------------------------------------------------------------