Repository: mlab-upenn/LearningMPC
Branch: master
Commit: 50fcc219037c
Files: 14
Total size: 177.1 KB
Directory structure:
gitextract_5zinre62/
├── CMakeLists.txt
├── Lmpc_params.yaml
├── README.md
├── data/
│ ├── centerline_waypoints.csv
│ └── initial_safe_set.csv
├── include/
│ └── LearningMPC/
│ ├── CSVReader.h
│ ├── car_params.h
│ ├── occupancy_grid.h
│ ├── spline.h
│ └── track.h
├── launch/
│ └── lmpc.launch
├── package.xml
└── src/
├── LMPC.cpp
└── record_initial_safe_set.cpp
================================================
FILE CONTENTS
================================================
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(LearningMPC)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
find_package(OsqpEigen REQUIRED)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES LearningMPC
CATKIN_DEPENDS roscpp rospy
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_executable(LMPC src/LMPC.cpp)
target_link_libraries(LMPC ${catkin_LIBRARIES})
add_dependencies(LMPC ${catkin_EXPORTED_TARGETS})
add_executable(record_init_ss src/record_initial_safe_set.cpp)
target_link_libraries(record_init_ss ${catkin_LIBRARIES})
add_dependencies(record_init_ss ${catkin_EXPORTED_TARGETS})
target_link_libraries(LMPC OsqpEigen::OsqpEigen osqp::osqp)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/LearningMPC.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/LearningMPC_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
#)
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_LearningMPC.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
================================================
FILE: Lmpc_params.yaml
================================================
pose_topic : "odom"
drive_topic: "nav"
wp_file_name: "/home/yuwei/yuwei_ws/src/LearningMPC/data/centerline_waypoints.csv"
r_accel : 1.5
r_steer: 18.0
q_s: 3000.0
q_s_terminal: 800
N: 25
Ts: 0.05
K_NEAR: 16
SPEED_MAX: 10.00
STEER_MAX: 0.41
ACCELERATION_MAX: 4.0
DECELERATION_MAX: 5.0
VEL_THRESHOLD: 0.8
MAP_MARGIN: 0.32
WAYPOINT_SPACE: 0.2
wheelbase: 0.3302 # meters
friction_coeff: 1.2 # - (complete estimate)
height_cg: 0.08255 # m (roughly measured to be 3.25 in)
l_cg2rear: 0.17145 # m (decently measured to be 6.75 in)
l_cg2front: 0.15875 # m (decently measured to be 6.25 in)
C_S_front: 2.3 #.79 # 1/rad ? (estimated weight/4)
C_S_rear: 2.3 #.79 # 1/rad ? (estimated weight/4)
mass: 3.17 # kg (measured on car 'lidart')
moment_inertia: .0398378 # kg m^2 (estimated as a rectangle with width and height of car and evenly distributed mass, then shifted to account for center of mass location)
random: 1.0
================================================
FILE: README.md
================================================
# LearningMPC
Learning based Model Predictive Control for online iterative trajectory optimization for F1/10 autonomous racing.
The F1/10 hardware used for experiments:
<p align="center">
<img src="https://storage.googleapis.com/groundai-web-prod/media/users/user_211599/project_333357/images/introduction/images/Fig_1.png" width="350" title="The F1/10 hardware used for experiments">
</p>
The car is learning to improve its racing policy iteratively online as it completes more and more laps, until the policy converges to a minimum lap time.
Initial Sample Safe Set is collected using a path following controller. Starting from Lap 3, control policies are generated by learning MPC. Every time a lap is completed, cost-to-go values are updated for all collected samples. Stability and recursive feasibility of MPC is gauranteed by enforcing the terminal state to land within a convex hull of a selected subset of collected samples from previous laps. The MPC manages to minimize the cost-to-go for the terminal state at each run, and is solved with OSQP solver at 20Hz. The sample safe set continues to grow as more and more data is being collected along the way.
The improvement of racing performance over laps is shown here below. The black one is the initial lap with path following controller. The colored ones are the improved trajectories given by LMPC
### Dependencies
1. Install the f1/10 racecar simulator:
https://github.com/mlab-upenn/f110-fall2019-skeletons/tree/master/racecar_simulator
2. Install QSQP Solver:
https://osqp.org/docs/get_started/sources.html
3. Install the C++ Eigen wrapper for OSQP:
https://robotology.github.io/osqp-eigen/doxygen/doc/html/index.html
### How to Run
Build everything with ```catkin_make``` from the root directory of your catkin workspace
First launch the simulator by entering the command below in terminal:
roslaunch racecar_simulator simulator.launch
Position the car as shown below (The car must be placed ahead of the starting line) using interactive markers in Rviz
Then launch LearningMPC node:
roslaunch LearningMPC lmpc.launch
Enter ```n``` in the simulator terminal to unlock the car
### Results
Lap 5 (Top speed 1.2 m/s)
Lap 25 (Top speed 3 m/s)
After Lap 60, it converges to an optimal policy (Top speed 6 m/s) with precisely controlled high speed drifting.
.
With low frictions, it converges to a policy with lower top speed but with more drifting.
.
Obstacle Avoidance 1:
.
Obstacle Avoidance 2 (low friction):
.
Lap time vs number of iterations completed:
Converged optimal policy recorded:
Currently working on Gaussian Process and local linear regression for system identification to update the dynamic model online.
The project is inspired by "Learning How to Autonomously Race a Car: a Predictive Control Approach, Ugo Rosolia and Francesco Borrelli"
================================================
FILE: data/centerline_waypoints.csv
================================================
0.688,-0.906
1.049,-1.92
1.95,-4.108
2.453,-6.082
2.522,-6.570
2.444,-6.922
2.181,-7.290
1.277,-7.851
-0.698,-8.757
-1.972,-9.200
-4.363,-10.040
-5.665,-10.615
-5.852,-10.719
-6.161,-10.994
-6.205,-11.405
-6.009,-12.139
-5.785,-12.619
-5.670,-12.863
-5.376,-13.186
-5.175,-13.232
-4.642,-13.272
-4.085,-13.115
-3.453,-12.924
-2.236,-12.462
-1.260,-12.196
-0.418,-11.838
0.512,-11.379
1.587,-10.849
3.682,-9.708
4.355,-9.275
5.063,-8.669
5.582,-7.996
5.927,-7.119
6.195,-6.046
6.294,-5.217
6.065,-4.417
5.765,-3.476
5.466,-2.774
4.971,-1.823
4.518,-0.906
4.092,-0.210
3.580,0.552
3.327,0.908
3.034,1.223
2.767,1.363
2.419,1.463
1.684,1.346
1.287,1.248
0.808,0.873
0.515,0.444
0.462,0.183
0.477,-0.212
================================================
FILE: data/initial_safe_set.csv
================================================
0,0.697918,-1.06102,-1.09096,0,0,-0.00363945,0.15
1,0.698549,-1.06224,-1.09098,0.0539778,0,-0.00363945,0.15
2,0.700624,-1.06623,-1.09104,0.0979448,0,-0.00363945,0.15
3,0.703612,-1.07197,-1.09113,0.136974,0,-0.00363945,0.15
4,0.706459,-1.07744,-1.09121,0.164831,0,-0.00363945,0.15
5,0.711225,-1.08661,-1.09131,0.201727,0,-0.00363945,0.2
6,0.714945,-1.09376,-1.09141,0.225521,0,-0.00363945,0.2
7,0.721684,-1.10673,-1.0916,0.261864,0,-0.00363945,0.2
8,0.726671,-1.11633,-1.09167,0.284906,0,-0.00363945,0.2
9,0.73446,-1.13133,-1.09189,0.316389,0,-0.00363945,0.25
10,0.743145,-1.14806,-1.0921,0.346815,0,-0.00363945,0.25
11,0.749275,-1.15987,-1.09225,0.366065,0,-0.00363945,0.25
12,0.75929,-1.17918,-1.09249,0.394452,0,-0.00363945,0.3
13,0.769974,-1.1998,-1.09275,0.421433,0,-0.00363945,0.3
14,0.777634,-1.21458,-1.09293,0.439092,0,-0.00363945,0.3
15,0.789883,-1.23824,-1.09323,0.464949,0,-0.00363945,0.35
16,0.79817,-1.25426,-1.09343,0.481038,0,-0.00363945,0.35
17,0.810879,-1.27884,-1.09374,0.503868,0,-0.00363945,0.4
18,0.824485,-1.30517,-1.09407,0.52621,0,-0.00363945,0.4
19,0.834271,-1.32419,-1.0943,0.541171,0,-0.00363945,0.45
20,0.848581,-1.35205,-1.09465,0.561573,0,-0.00363945,0.45
21,0.858474,-1.37133,-1.09489,0.574759,0,-0.00363945,0.5
22,0.873463,-1.40056,-1.09526,0.593479,0,-0.00363945,0.55
23,0.884865,-1.42281,-1.09554,0.606819,0,-0.00363945,0.55
24,0.900494,-1.45334,-1.09592,0.623975,0,-0.00363945,0.6
25,0.916341,-1.48432,-1.09631,0.640188,0,-0.00363945,0.6
26,0.928669,-1.50844,-1.09661,0.652068,0,-0.00363945,0.65
27,0.945734,-1.54187,-1.09703,0.667554,0,-0.00363945,0.7
28,0.963436,-1.57657,-1.09746,0.682564,0,-0.00363945,0.7
29,0.975727,-1.60069,-1.09776,0.692415,0,-0.00363945,0.75
30,0.99369,-1.63597,-1.0982,0.706045,0,-0.00363945,0.8
31,1.00594,-1.66006,-1.0985,0.71486,0,-0.00363945,0.8
32,1.02474,-1.69705,-1.09897,0.727696,0,-0.00363945,0.85
33,1.04356,-1.73412,-1.09943,0.739769,0,-0.00363945,0.9
34,1.05635,-1.75933,-1.09975,0.74757,0,-0.00363945,0.95
35,1.07671,-1.79954,-1.10025,0.759374,0,-0.00363945,0.95
36,1.08982,-1.82544,-1.10057,0.766591,0,-0.00363945,1
37,1.10976,-1.86488,-1.10106,0.777047,0,-0.00363945,1.05
38,1.12982,-1.9046,-1.10156,0.786975,0,-0.00363945,1.1
39,1.14288,-1.9305,-1.10188,0.793145,0,-0.00363945,1.1
40,1.16331,-1.97103,-1.10239,0.802354,0,-0.00363945,1.15
41,1.17718,-1.99859,-1.10273,0.808324,0,-0.00363945,1.2
42,1.19913,-2.04224,-1.10328,0.817329,0,-0.00363945,1.25
43,1.2202,-2.0842,-1.1038,0.825499,0,-0.00363945,1.3
44,1.2339,-2.1115,-1.10414,0.830576,0,-0.00363945,1.3
45,1.25519,-2.154,-1.10467,0.838128,0,-0.00363945,1.35
46,1.27679,-2.19717,-1.10521,0.845391,0,-0.00363945,1.4
47,1.29093,-2.22545,-1.10556,0.849939,0,-0.00363945,1.45
48,1.31292,-2.26949,-1.10611,0.85671,0,-0.00363945,1.5
49,1.32836,-2.30045,-1.10649,0.861257,0,-0.41,1.55
50,1.34733,-2.34569,-1.12675,0.867432,0,-0.00363945,1.6
51,1.36241,-2.39494,-1.16249,0.873576,0,-0.00363945,1.65
52,1.37528,-2.42487,-1.16289,0.877287,0,-0.00363945,1.65
53,1.39525,-2.47134,-1.16345,0.882804,0,-0.00363945,1.7
54,1.41422,-2.51558,-1.16399,0.887796,0,-0.399614,1.75
55,1.42549,-2.54891,-1.18167,0.891298,0,-0.00363945,1.8
56,1.43733,-2.59775,-1.21822,0.89609,0,-0.00363945,1.85
57,1.44915,-2.63015,-1.21859,0.899237,0,-0.00363945,1.9
58,1.46673,-2.67823,-1.21904,0.903714,0,-0.00363945,1.95
59,1.48382,-2.72511,-1.21965,0.907868,0,-0.00363945,2
60,1.49742,-2.76248,-1.22011,0.911037,0,-0.00363945,2.05
61,1.51498,-2.81086,-1.22076,0.914962,0,-0.00363945,2.1
62,1.52697,-2.84388,-1.22106,0.917533,0,-0.00363945,2.1
63,1.54465,-2.89272,-1.22173,0.921179,0,-0.00363945,2.15
64,1.56283,-2.94302,-1.22232,0.924751,0,-0.00363945,2.2
65,1.5761,-2.97976,-1.22275,0.92725,0,-0.00363945,2.25
66,1.59373,-3.02869,-1.22332,0.930439,0,-0.00363945,2.3
67,1.60593,-3.06259,-1.22372,0.93256,0,-0.00363945,2.35
68,1.62382,-3.11237,-1.2243,0.935548,0,-0.00363945,2.4
69,1.63765,-3.15093,-1.22476,0.937766,0,-0.00363945,2.45
70,1.65563,-3.20112,-1.22535,0.94053,0,-0.00363945,2.5
71,1.6736,-3.25139,-1.22593,0.943168,0,-0.00363945,2.55
72,1.68706,-3.28911,-1.22638,0.945065,0,-0.00363945,2.6
73,1.7052,-3.34001,-1.22697,0.94752,0,-0.00363945,2.65
74,1.71727,-3.37395,-1.22737,0.949091,0,-0.00363945,2.65
75,1.7353,-3.42473,-1.22796,0.95135,0,-0.00363945,2.75
76,1.75321,-3.47525,-1.22856,0.953493,0,-0.00363945,2.8
77,1.76585,-3.51095,-1.22897,0.954947,0,-0.00363945,2.8
78,1.78417,-3.56279,-1.22958,0.956974,0,-0.00363945,2.85
79,1.7961,-3.5966,-1.22998,0.958244,0,-0.00363945,2.9
80,1.81452,-3.64891,-1.23059,0.960133,0,-0.00363945,2.95
81,1.83237,-3.69967,-1.23118,0.96188,0,-0.00363945,3
82,1.84465,-3.73465,-1.23159,0.963038,0,-0.00363945,3.05
83,1.86267,-3.78608,-1.23219,0.964673,0,-0.00363945,3.1
84,1.88083,-3.83801,-1.2328,0.966248,0,-0.00363945,3.15
85,1.89307,-3.87306,-1.23321,0.96727,0,-0.00363945,3.2
86,1.91095,-3.92435,-1.23381,0.968707,0,-0.00363945,3.25
87,1.92871,-3.97539,-1.2344,0.970073,0,-0.00363945,3.3
88,1.94083,-4.01026,-1.23481,0.97097,0,-0.00363945,3.35
89,1.95907,-4.06286,-1.23543,0.972271,0,-0.00363945,3.4
90,1.97127,-4.0981,-1.23584,0.973109,0,-0.00363945,3.45
91,1.9893,-4.15025,-1.23645,0.974302,0,-0.00363945,3.5
92,2.00732,-4.20249,-1.23706,0.975442,0,-0.00363945,3.55
93,2.01931,-4.23729,-1.23746,0.976172,0,-0.00363945,3.6
94,2.03359,-4.27884,-1.23795,0.977015,0,-0.31809,3.65
95,2.05103,-4.34424,-1.26668,0.978257,0,-0.00363945,3.7
96,2.0584,-4.3803,-1.28466,0.978903,0,-0.00363945,3.75
97,2.07348,-4.43202,-1.28521,0.979811,0,-0.00363945,3.8
98,2.08926,-4.48617,-1.2858,0.98072,0,-0.41,3.85
99,2.09681,-4.52231,-1.30423,0.981293,0,-0.00363945,3.9
100,2.10252,-4.5775,-1.3475,0.982121,0,-0.00363945,3.95
101,2.11441,-4.63064,-1.34819,0.982897,0,-0.00363945,4
102,2.12304,-4.66912,-1.34862,0.983438,0,-0.00363945,4.05
103,2.133,-4.7138,-1.34929,0.984043,0,-0.102633,4.1
104,2.14203,-4.75935,-1.35666,0.984635,0,-0.00363945,4.1
105,2.1521,-4.81393,-1.36457,0.985312,0,-0.0735173,4.2
106,2.16202,-4.8692,-1.37356,0.985967,0,-0.090987,4.25
107,2.16761,-4.90641,-1.38352,0.986389,0,-0.189981,4.25
108,2.1733,-4.96269,-1.40977,0.987,0,-0.254035,4.35
109,2.17467,-5.00263,-1.43756,0.987415,0,0.108747,4.35
110,2.17719,-5.0582,-1.45904,0.98797,0,0.140192,4.4
111,2.18781,-5.11403,-1.43634,0.988511,0,-0.131749,4.5
112,2.19354,-5.15066,-1.43437,0.988851,0,-0.131749,4.5
113,2.19684,-5.20604,-1.4565,0.989341,0,-0.0385785,4.55
114,2.20086,-5.26304,-1.46822,0.989822,0,0.281695,4.65
115,2.20655,-5.2998,-1.45546,0.990124,0,0.23511,4.65
116,2.22262,-5.35811,-1.40867,0.990596,0,0.140192,4.75
117,2.23216,-5.39413,-1.39001,0.990874,0,-0.20745,4.75
118,2.24373,-5.44929,-1.38604,0.991281,0,0.141939,4.8
119,2.25217,-5.50495,-1.39218,0.991669,0,0.23511,4.9
120,2.26332,-5.54187,-1.36757,0.991925,0,-0.0444016,4.9
121,2.27852,-5.59539,-1.34967,0.99228,0,-0.160865,4.95
122,2.28686,-5.65154,-1.37239,0.992625,0,0.0662384,5.05
123,2.2937,-5.68916,-1.37426,0.992849,0,0.108747,5.05
124,2.3076,-5.74312,-1.35787,0.993163,0,-0.189981,5.1
125,2.31744,-5.79679,-1.37009,0.993458,0,-0.102633,5.15
126,2.32289,-5.83853,-1.38523,0.993676,0,-0.137572,5.2
127,2.3291,-5.89467,-1.40749,0.993957,0,-0.120103,5.25
128,2.3326,-5.93171,-1.42126,0.994136,0,-0.213273,5.3
129,2.33527,-5.98686,-1.45116,0.994391,0,-0.0170327,5.35
130,2.33863,-6.04377,-1.46661,0.994643,0,0.0144123,5.4
131,2.34332,-6.08589,-1.46484,0.994822,0,-0.358852,5.45
132,2.34452,-6.14281,-1.49395,0.995055,0,-0.271505,5.5
133,2.34123,-6.17989,-1.52655,0.995201,0,-0.283151,5.55
134,2.33439,-6.23636,-1.57482,0.995415,0,-0.358852,5.6
135,2.3233,-6.29023,-1.63203,0.995614,0,-0.189981,5.65
136,2.31531,-6.32972,-1.66046,0.995753,0,-0.0793405,5.7
137,2.30629,-6.38512,-1.6799,0.995941,0,-0.213273,5.75
138,2.29869,-6.42271,-1.69978,0.996064,0,0.0877838,5.8
139,2.28797,-6.47627,-1.71588,0.996233,0,0.0248939,5.85
140,2.28128,-6.53241,-1.70881,0.996401,0,-0.382145,5.95
141,2.27298,-6.57317,-1.72583,0.996519,0,-0.259858,6
142,2.25411,-6.6273,-1.77789,0.996676,0,-0.160865,6.05
143,2.23704,-6.67894,-1.80839,0.996818,0,-0.0560479,6.15
144,2.22639,-6.71548,-1.81698,0.996914,0,-0.242389,6.2
145,2.20781,-6.76869,-1.84524,0.99705,0,-0.382145,6.25
146,2.1911,-6.80304,-1.88318,0.997139,0,0.0458572,6.3
147,2.1652,-6.85322,-1.92709,0.997266,0,-0.283151,6.4
148,2.14236,-6.90538,-1.94683,0.997389,0,-0.41,6.5
149,2.12186,-6.93673,-1.98905,0.997466,0,-0.31809,6.55
150,2.08729,-6.98416,-2.05139,0.997583,0,-0.300621,6.6
151,2.0532,-7.02812,-2.10254,0.997688,0,0.0144123,6.65
152,2.03142,-7.05831,-2.11875,0.997756,0,-0.0793405,6.7
153,2.00063,-7.10621,-2.12656,0.997856,0,-0.277328,6.8
154,1.96544,-7.15018,-2.16331,0.997951,0,-0.254035,6.85
155,1.94108,-7.177,-2.19157,0.99801,0,-0.358852,6.9
156,1.89992,-7.21662,-2.24669,0.998099,0,-0.323913,6.95
157,1.85683,-7.25283,-2.30304,0.998183,0,-0.219096,7
158,1.82953,-7.27577,-2.32921,0.998234,0,-0.178334,7.05
159,1.7881,-7.31058,-2.36055,0.998309,0,0.0353756,7.1
160,1.74618,-7.34908,-2.36919,0.998385,0,-0.00363945,7.2
161,1.7206,-7.37419,-2.36874,0.99843,0,-0.00363945,7.2
162,1.68056,-7.41306,-2.36941,0.998499,0,-0.00363945,7.3
163,1.64034,-7.45205,-2.37009,0.998565,0,-0.00363945,7.35
164,1.60118,-7.48996,-2.3707,0.998626,0,-0.41,7.4
165,1.56998,-7.51512,-2.39365,0.99867,0,-0.00363945,7.45
166,1.52473,-7.54577,-2.43286,0.998727,0,-0.00363945,7.5
167,1.48923,-7.57597,-2.43374,0.998774,0,-0.00363945,7.55
168,1.44646,-7.61241,-2.43436,0.998828,0,-0.00363945,7.6
169,1.41795,-7.63666,-2.43489,0.998862,0,-0.00363945,7.65
170,1.37474,-7.67333,-2.43564,0.998913,0,-0.00363945,7.7
171,1.33122,-7.71036,-2.43598,0.998961,0,-0.00363945,7.75
172,1.29794,-7.73858,-2.43653,0.998997,0,-0.00363945,7.8
173,1.26427,-7.76722,-2.43657,0.999032,0,-0.41,7.8
174,1.21609,-7.80195,-2.46555,0.999077,0,-0.00363945,7.9
175,1.17036,-7.82974,-2.49746,0.999116,0,-0.00363945,7.95
176,1.14007,-7.85238,-2.49789,0.999142,0,-0.00363945,7.95
177,1.09399,-7.88682,-2.49847,0.999181,0,-0.41,8.05
178,1.04811,-7.91412,-2.53221,0.999215,0,-0.41,8.1
179,1.01325,-7.92805,-2.57876,0.999239,0,-0.00363945,8.1
180,0.962934,-7.94859,-2.62426,0.999271,0,-0.00363945,8.15
181,0.930494,-7.96684,-2.62484,0.999292,0,-0.00363945,8.2
182,0.879757,-7.99555,-2.62534,0.999325,0,-0.00363945,8.25
183,0.831734,-8.02266,-2.62601,0.999354,0,-0.00363945,8.3
184,0.784343,-8.04937,-2.62669,0.999381,0,-0.00363945,8.35
185,0.752174,-8.06748,-2.6271,0.999399,0,-0.00363945,8.4
186,0.702659,-8.09533,-2.62774,0.999426,0,-0.00363945,8.45
187,0.650735,-8.12448,-2.62841,0.999453,0,-0.00363945,8.55
188,0.60799,-8.14845,-2.62896,0.999474,0,-0.00363945,8.6
189,0.57519,-8.16682,-2.62939,0.99949,0,-0.00363945,8.6
190,0.526824,-8.19388,-2.63001,0.999512,0,-0.00363945,8.65
191,0.477587,-8.22138,-2.63064,0.999533,0,-0.00363945,8.75
192,0.443145,-8.24059,-2.63109,0.999548,0,-0.00363945,8.75
193,0.394116,-8.26791,-2.63172,0.999568,0,-0.41,8.8
194,0.34331,-8.28995,-2.66492,0.999586,0,-0.00363945,8.9
195,0.308579,-8.30172,-2.69211,0.999598,0,-0.00363945,8.9
196,0.256131,-8.32657,-2.69357,0.999617,0,-0.00363945,8.95
197,0.20707,-8.34997,-2.6944,0.999633,0,-0.00363945,9.05
198,0.173674,-8.36591,-2.69479,0.999644,0,-0.00363945,9.05
199,0.122635,-8.39026,-2.69525,0.99966,0,-0.00363945,9.1
200,0.0722579,-8.41424,-2.69585,0.999674,0,-0.00363945,9.2
201,0.0378689,-8.43059,-2.69626,0.999684,0,-0.00363945,9.2
202,-0.0127758,-8.45463,-2.69687,0.999698,0,-0.00363945,9.25
203,-0.0619597,-8.47794,-2.69746,0.999711,0,-0.00363945,9.3
204,-0.0964956,-8.49429,-2.69787,0.99972,0,-0.00363945,9.35
205,-0.147475,-8.51839,-2.69848,0.999732,0,-0.00363945,9.4
206,-0.199003,-8.54271,-2.6991,0.999744,0,-0.00363945,9.5
207,-0.23244,-8.55847,-2.6995,0.999751,0,-0.00363945,9.5
208,-0.283125,-8.58234,-2.7001,0.999762,0,-0.00363945,9.55
209,-0.332539,-8.60556,-2.70069,0.999772,0,-0.00363945,9.6
210,-0.365878,-8.62121,-2.70109,0.999779,0,-0.00363945,9.65
211,-0.416573,-8.64498,-2.70169,0.999789,0,-0.00363945,9.7
212,-0.468134,-8.66912,-2.70231,0.999798,0,-0.00363945,9.8
213,-0.502474,-8.68517,-2.70272,0.999804,0,-0.00363945,9.8
214,-0.552694,-8.70861,-2.70332,0.999813,0,-0.00363945,9.85
215,-0.604353,-8.73269,-2.70393,0.999821,0,-0.00363945,9.95
216,-0.638665,-8.74866,-2.70434,0.999826,0,-0.00363945,9.95
217,-0.688805,-8.77197,-2.70494,0.999834,0,-0.00363945,10
218,-0.742564,-8.79692,-2.70558,0.999842,0,-0.00363945,10.1
219,-0.777112,-8.81294,-2.70599,0.999846,0,-0.00363945,10.1
220,-0.826674,-8.83588,-2.70658,0.999853,0,-0.00363945,10.15
221,-0.877498,-8.85937,-2.70719,0.999859,0,-0.00363945,10.25
222,-0.911551,-8.87508,-2.70759,0.999864,0,-0.00363945,10.25
223,-0.962162,-8.89841,-2.70819,0.99987,0,-0.00363945,10.3
224,-1.01274,-8.92169,-2.7088,0.999875,0,-0.00363945,10.35
225,-1.04769,-8.93775,-2.70921,0.999879,0,-0.00363945,10.4
226,-1.0971,-8.96043,-2.7098,0.999884,0,-0.00363945,10.45
227,-1.14774,-8.98363,-2.7104,0.999889,0,-0.00363945,10.5
228,-1.19738,-9.00635,-2.71099,0.999894,0,-0.00363945,10.6
229,-1.23271,-9.02249,-2.71141,0.999897,0,-0.00363945,10.6
230,-1.28539,-9.04653,-2.71204,0.999902,0,-0.00363945,10.65
231,-1.31935,-9.062,-2.71244,0.999905,0,-0.00363945,10.7
232,-1.37163,-9.08579,-2.71306,0.999909,0,-0.00363945,10.75
233,-1.42347,-9.10934,-2.71367,0.999913,0,-0.405437,10.8
234,-1.45925,-9.1226,-2.73134,0.999916,0,-0.00363945,10.85
235,-1.51379,-9.13632,-2.7754,0.999919,0,-0.00363945,10.9
236,-1.566,-9.1562,-2.77585,0.999923,0,-0.00363945,10.95
237,-1.60167,-9.16978,-2.77626,0.999925,0,-0.323913,11
238,-1.65805,-9.18656,-2.80301,0.999929,0,-0.00363945,11.05
239,-1.71125,-9.19958,-2.82736,0.999932,0,-0.00363945,11.1
240,-1.74699,-9.21112,-2.82773,0.999934,0,-0.00363945,11.15
241,-1.79903,-9.22787,-2.8284,0.999937,0,-0.00363945,11.2
242,-1.85226,-9.24498,-2.82906,0.999939,0,-0.00363945,11.25
243,-1.88904,-9.25678,-2.82949,0.999941,0,-0.00363945,11.3
244,-1.94144,-9.27356,-2.83011,0.999944,0,-0.00363945,11.35
245,-1.99514,-9.29072,-2.83074,0.999946,0,-0.00363945,11.4
246,-2.02996,-9.30183,-2.83115,0.999948,0,-0.00363945,11.45
247,-2.08445,-9.31918,-2.83179,0.99995,0,-0.00363945,11.5
248,-2.13822,-9.33626,-2.83242,0.999952,0,-0.00363945,11.55
249,-2.17491,-9.34789,-2.83285,0.999954,0,-0.00363945,11.6
250,-2.22863,-9.3649,-2.83349,0.999956,0,-0.00363945,11.65
251,-2.28208,-9.38178,-2.83411,0.999958,0,-0.00363945,11.7
252,-2.31834,-9.39321,-2.83454,0.999959,0,-0.00363945,11.75
253,-2.37177,-9.41002,-2.83517,0.999961,0,-0.00363945,11.8
254,-2.42556,-9.42691,-2.8358,0.999963,0,-0.00363945,11.85
255,-2.46295,-9.43863,-2.83624,0.999964,0,-0.00363945,11.9
256,-2.51615,-9.45527,-2.83686,0.999965,0,-0.00363945,11.95
257,-2.55258,-9.46665,-2.83729,0.999966,0,-0.00363945,12
258,-2.60692,-9.48359,-2.83793,0.999968,0,-0.00363945,12.05
259,-2.66123,-9.50047,-2.83857,0.999969,0,-0.00363945,12.1
260,-2.69757,-9.51175,-2.83899,0.99997,0,-0.00363945,12.15
261,-2.75202,-9.52862,-2.83963,0.999972,0,-0.00363945,12.2
262,-2.8067,-9.54552,-2.84027,0.999973,0,-0.00363945,12.25
263,-2.84257,-9.55659,-2.8407,0.999974,0,-0.00363945,12.3
264,-2.89611,-9.57307,-2.84132,0.999975,0,-0.00363945,12.35
265,-2.95085,-9.58989,-2.84196,0.999976,0,-0.00363945,12.4
266,-2.98747,-9.60112,-2.84239,0.999977,0,-0.00363945,12.45
267,-3.04131,-9.6176,-2.84303,0.999978,0,-0.00363945,12.5
268,-3.09444,-9.63382,-2.84365,0.999979,0,-0.00363945,12.55
269,-3.13223,-9.64534,-2.84409,0.999979,0,-0.00363945,12.6
270,-3.18544,-9.66153,-2.84471,0.99998,0,-0.00363945,12.65
271,-3.22188,-9.6726,-2.84514,0.999981,0,-0.00363945,12.7
272,-3.27765,-9.6895,-2.84579,0.999982,0,-0.00363945,12.75
273,-3.33033,-9.70543,-2.84641,0.999982,0,-0.00363945,12.8
274,-3.36697,-9.71649,-2.84684,0.999983,0,-0.00363945,12.85
275,-3.42259,-9.73324,-2.84749,0.999984,0,-0.00363945,12.9
276,-3.46502,-9.74599,-2.84799,0.999984,0,-0.00363945,12.95
277,-3.51679,-9.76152,-2.84859,0.999985,0,-0.00363945,13
278,-3.57169,-9.77795,-2.84924,0.999986,0,-0.00363945,13.05
279,-3.6075,-9.78865,-2.84966,0.999986,0,-0.00363945,13.1
280,-3.66214,-9.80494,-2.85029,0.999987,0,-0.00363945,13.15
281,-3.71662,-9.82115,-2.85093,0.999987,0,-0.00363945,13.2
282,-3.75288,-9.83191,-2.85136,0.999988,0,-0.00363945,13.25
283,-3.80881,-9.84848,-2.85201,0.999988,0,-0.00363945,13.3
284,-3.84488,-9.85915,-2.85243,0.999989,0,-0.00363945,13.35
285,-3.90035,-9.87552,-2.85308,0.999989,0,-0.00363945,13.4
286,-3.95449,-9.89146,-2.85371,0.99999,0,-0.00363945,13.45
287,-3.99153,-9.90234,-2.85414,0.99999,0,-0.00363945,13.5
288,-4.04549,-9.91817,-2.85477,0.99999,0,0.0196531,13.55
289,-4.08395,-9.92978,-2.85319,0.999991,0,0.0371225,13.6
290,-4.13687,-9.94644,-2.8482,0.999991,0,0.0778847,13.65
291,-4.19077,-9.96497,-2.83674,0.999991,0,0.101177,13.7
292,-4.22603,-9.97816,-2.82576,0.999992,0,0.130293,13.75
293,-4.2784,-9.99938,-2.80531,0.999992,0,0.141939,13.8
294,-4.32739,-10.0211,-2.78296,0.999992,0,0.171055,13.85
295,-4.37859,-10.0461,-2.75434,0.999993,0,0.147763,13.9
296,-4.41395,-10.0642,-2.73627,0.999993,0,0.188525,13.95
297,-4.46247,-10.0912,-2.70645,0.999993,0,0.275872,14
298,-4.49444,-10.1117,-2.67796,0.999993,0,0.252579,14.05
299,-4.54031,-10.1444,-2.63347,0.999994,0,0.270049,14.1
300,-4.585,-10.1794,-2.58766,0.999994,0,0.281695,14.15
301,-4.61356,-10.204,-2.55578,0.999994,0,0.205994,14.2
302,-4.65719,-10.2428,-2.51432,0.999994,0,-0.0152857,14.25
303,-4.70158,-10.2786,-2.50193,0.999995,0,0.0837078,14.3
304,-4.73155,-10.3024,-2.49456,0.999995,0,0.0662384,14.35
305,-4.77522,-10.3385,-2.48225,0.999995,0,0.0604153,14.4
306,-4.80418,-10.3625,-2.47533,0.999995,0,-0.11428,14.45
307,-4.84977,-10.3977,-2.48006,0.999995,0,0.0312994,14.5
308,-4.8963,-10.4323,-2.48492,0.999996,0,-0.195804,14.55
309,-4.92673,-10.4536,-2.49636,0.999996,0,-0.41,14.6
310,-4.97718,-10.4796,-2.54827,0.999996,0,0.101177,14.65
311,-5.01166,-10.4944,-2.58449,0.999996,0,-0.137572,14.7
312,-5.07131,-10.5287,-2.59692,0.999996,0,-0.41,14.75
313,-5.10927,-10.5445,-2.63266,0.999996,0,-0.41,14.8
314,-5.16417,-10.5595,-2.70315,0.999997,0,-0.41,14.85
315,-5.21697,-10.5702,-2.77,0.999997,0,-0.0502248,14.9
316,-5.25438,-10.5786,-2.7982,0.999997,0,0.182701,14.95
317,-5.31094,-10.6014,-2.78274,0.999997,0,0.188525,15
318,-5.34406,-10.618,-2.76168,0.999997,0,0.334104,15.05
319,-5.39356,-10.6475,-2.71303,0.999997,0,0.194348,15.1
320,-5.43998,-10.6774,-2.67381,0.999997,0,-0.0904046,15.15
321,-5.47391,-10.6959,-2.66982,0.999997,0,0.41,15.2
322,-5.52403,-10.723,-2.65853,0.999998,0,0.41,15.25
323,-5.55274,-10.7473,-2.61287,0.999998,0,0.41,15.3
324,-5.59527,-10.7888,-2.53914,0.999998,0,0.41,15.35
325,-5.63312,-10.8314,-2.46834,0.999998,0,0.41,15.4
326,-5.65613,-10.8605,-2.42218,0.999998,0,0.41,15.45
327,-5.6891,-10.9078,-2.35059,0.999998,0,0.41,15.5
328,-5.70874,-10.9399,-2.30391,0.999998,0,0.374866,15.55
329,-5.73744,-10.9923,-2.23367,0.999998,0,0.304988,15.6
330,-5.76294,-11.0425,-2.17855,0.999998,0,0.258403,15.65
331,-5.77999,-11.0772,-2.14748,0.999998,0,0.293341,15.7
332,-5.80276,-11.1294,-2.09862,0.999998,0,0.23511,15.75
333,-5.8168,-11.1642,-2.06991,0.999998,0,0.316634,15.85
334,-5.83662,-11.2209,-2.01913,0.999998,0,0.293341,16.15
335,-5.85185,-11.2756,-1.96767,0.999999,0,-0.048478,16.25
336,-5.86303,-11.3116,-1.9507,0.999999,0,0.316634,16.3
337,-5.88199,-11.3648,-1.93629,0.999999,0,0.41,16.35
338,-5.88795,-11.4022,-1.89296,0.999999,0,0.41,16.4
339,-5.89273,-11.4629,-1.81751,0.999999,0,0.351573,16.45
340,-5.89433,-11.5197,-1.75439,0.999999,0,0.275872,16.55
341,-5.89497,-11.5575,-1.72111,0.999999,0,0.34575,16.55
342,-5.89293,-11.6145,-1.66573,0.999999,0,-0.048478,16.65
343,-5.89096,-11.652,-1.63793,0.999999,0,0.293341,16.7
344,-5.89267,-11.7116,-1.62369,0.999999,0,0.182701,16.75
345,-5.8882,-11.7682,-1.58643,0.999999,0,0.23511,16.8
346,-5.88407,-11.8062,-1.56043,0.999999,0,0.101177,16.85
347,-5.87804,-11.8611,-1.53385,0.999999,0,0.41,16.9
348,-5.86717,-11.9195,-1.48541,0.999999,0,0.41,16.95
349,-5.85559,-11.955,-1.43896,0.999999,0,0.369042,17
350,-5.83539,-12.0086,-1.37238,0.999999,0,0.264226,17.05
351,-5.82175,-12.0437,-1.33823,0.999999,0,0.153586,17.1
352,-5.80124,-12.0988,-1.3025,0.999999,0,-0.0269322,17.2
353,-5.78486,-12.1523,-1.29623,0.999999,0,0.205994,17.25
354,-5.7732,-12.1877,-1.28543,0.999999,0,0.0837078,17.25
355,-5.75324,-12.2389,-1.26224,0.999999,0,0.246756,17.35
356,-5.73036,-12.291,-1.2296,0.999999,0,0.351573,17.4
357,-5.71136,-12.3244,-1.19221,0.999999,0,0.34575,17.45
358,-5.6792,-12.3729,-1.13069,0.999999,0,0.252579,17.5
359,-5.6471,-12.4191,-1.08339,0.999999,0,0.01383,17.55
360,-5.62712,-12.4512,-1.07134,0.999999,0,-0.0211091,17.55
361,-5.60025,-12.5011,-1.07323,0.999999,0,0.0778847,17.65
362,-5.57233,-12.5491,-1.06397,1,0,-0.0589597,17.7
363,-5.55055,-12.5896,-1.0683,1,0,0.41,17.75
364,-5.53175,-12.622,-1.06142,1,0,0.41,17.75
365,-5.49436,-12.6639,-0.996127,1,0,0.41,17.8
366,-5.4528,-12.7029,-0.925274,1,0,0.41,17.9
367,-5.42396,-12.727,-0.878824,1,0,0.41,17.9
368,-5.37786,-12.7611,-0.807566,1,0,0.41,18
369,-5.32897,-12.7921,-0.735684,1,0,0.409805,18.05
370,-5.28708,-12.8149,-0.676243,1,0,0.334104,18.1
371,-5.23959,-12.8391,-0.620356,1,0,0.240933,18.25
372,-5.19925,-12.8595,-0.584986,1,0,0.316634,18.5
373,-5.16485,-12.8751,-0.551509,1,0,0.0371225,18.55
374,-5.11365,-12.8978,-0.515941,1,0,0.258403,18.6
375,-5.06132,-12.9216,-0.487097,1,0,0.287518,18.65
376,-5.02539,-12.9339,-0.454802,1,0,0.23511,18.65
377,-4.96638,-12.9522,-0.407173,1,0,0.171055,18.75
378,-4.93039,-12.9633,-0.385969,1,0,0.147763,18.8
379,-4.87605,-12.9798,-0.359245,1,0,0.351573,18.85
380,-4.82088,-12.9913,-0.312481,1,0,0.322457,18.9
381,-4.78369,-12.9961,-0.275307,1,0,0.270049,18.95
382,-4.72502,-13.0021,-0.223658,1,0,0.229287,19.05
383,-4.66973,-13.0067,-0.183986,1,0,0.188525,19.1
384,-4.63035,-13.0096,-0.160745,1,0,0.176878,19.15
385,-4.57341,-13.0127,-0.129801,1,0,0.147763,19.25
386,-4.53559,-13.0143,-0.112127,1,0,0.118647,19.25
387,-4.47872,-13.0163,-0.0899062,1,0,0.101177,19.35
388,-4.42187,-13.0178,-0.0719599,1,0,0.0837078,19.4
389,-4.38419,-13.0187,-0.0620127,1,0,0.0720615,19.45
390,-4.32738,-13.0197,-0.0490817,1,0,0.0604153,19.5
391,-4.27003,-13.0203,-0.0380866,1,0,0.0487688,19.55
392,-4.23103,-13.0207,-0.0321032,1,0,0.0487688,19.6
393,-4.17394,-13.0209,-0.0238714,1,0,0.0545921,19.65
394,-4.13675,-13.0207,-0.0179079,1,0,0.0429456,19.65
395,-4.07926,-13.0201,-0.00976326,1,0,0.0371225,19.7
396,-4.02211,-13.0193,-0.00302959,1,0,0.0778847,19.75
397,-3.98392,-13.0181,0.00486039,1,0,0.0487688,19.8
398,-3.9246,-13.0156,0.0154577,1,0,0.0429456,19.85
399,-3.86906,-13.0134,0.0226433,1,0,0.0371225,19.9
400,-3.83122,-13.0117,0.0270337,1,0,0.12447,19.95
401,-3.77616,-13.0073,0.0430094,1,0,0.293341,20
402,-3.72077,-12.9974,0.0834535,1,0,0.258403,20.05
403,-3.68328,-12.9886,0.11386,1,0,-0.00363945,20.1
404,-3.63145,-12.9779,0.135705,1,0,0.0778847,20.15
405,-3.57645,-12.9687,0.145114,1,0,-0.00363945,20.2
406,-3.54059,-12.9629,0.14728,1,0,-0.00363945,20.25
407,-3.48425,-12.9547,0.146694,1,0,-0.00363945,20.3
408,-3.42787,-12.9465,0.146057,1,0,-0.00363945,20.35
409,-3.39033,-12.941,0.145633,1,0,-0.00363945,20.4
410,-3.33373,-12.9329,0.144994,1,0,-0.00363945,20.45
411,-3.27835,-12.9249,0.144368,1,0,-0.00363945,20.5
412,-3.24215,-12.9197,0.143959,1,0,-0.00363945,20.55
413,-3.19913,-12.9136,0.143473,1,0,0.41,20.6
414,-3.15013,-12.9051,0.152658,1,0,0.297417,20.65
415,-3.08754,-12.8821,0.219564,1,0,0.0312994,20.7
416,-3.05214,-12.87,0.239687,1,0,0.41,20.75
417,-2.99796,-12.8523,0.26539,1,0,0.0720615,20.8
418,-2.94567,-12.8288,0.308081,1,0,-0.00363945,20.85
419,-2.91016,-12.817,0.310188,1,0,0.119229,20.9
420,-2.8564,-12.7978,0.321259,1,0,0.0877838,20.95
421,-2.80324,-12.7766,0.337713,1,0,0.41,21
422,-2.76704,-12.7584,0.368632,1,0,0.159409,21.05
423,-2.73098,-12.7362,0.40793,1,0,-0.00363945,21.1
424,-2.68539,-12.713,0.42267,1,0,0.0353756,21.15
425,-2.62688,-12.6859,0.427014,1,0,-0.00655102,21.2
426,-2.59226,-12.67,0.427367,1,0,-0.00363945,21.25
427,-2.54123,-12.6469,0.426714,1,0,-0.00363945,21.3
428,-2.48854,-12.623,0.426076,1,0,-0.00363945,21.35
429,-2.45396,-12.6074,0.425656,1,0,-0.00363945,21.4
430,-2.40122,-12.5837,0.425014,1,0,-0.00363945,21.45
431,-2.36621,-12.5679,0.424589,1,0,-0.00363945,21.5
432,-2.31435,-12.5446,0.423958,1,0,-0.00363945,21.55
433,-2.26182,-12.521,0.42332,1,0,-0.00363945,21.6
434,-2.22787,-12.5058,0.422907,1,0,-0.00363945,21.65
435,-2.17576,-12.4825,0.422274,1,0,-0.00363945,21.7
436,-2.12375,-12.4593,0.421643,1,0,-0.00363945,21.75
437,-2.08826,-12.4434,0.421212,1,0,-0.00363945,21.8
438,-2.0346,-12.4195,0.42056,1,0,-0.00363945,21.85
439,-1.99961,-12.404,0.420136,1,0,-0.00363945,21.9
440,-1.94661,-12.3804,0.419493,1,0,-0.00363945,21.95
441,-1.9089,-12.3637,0.419035,1,0,-0.00363945,22
442,-1.85752,-12.3409,0.418412,1,0,-0.00363945,22.05
443,-1.81732,-12.3232,0.417925,1,0,-0.00363945,22.1
444,-1.77363,-12.3039,0.417395,1,0,-0.00363945,22.15
445,-1.71964,-12.2801,0.416741,1,0,-0.00363945,22.2
446,-1.66736,-12.2571,0.416108,1,0,-0.00363945,22.25
447,-1.63305,-12.242,0.415692,1,0,-0.00363945,22.3
448,-1.58045,-12.2189,0.415056,1,0,-0.00363945,22.35
449,-1.52864,-12.1962,0.414428,1,0,-0.00363945,22.4
450,-1.49284,-12.1806,0.413995,1,0,-0.41,22.45
451,-1.43871,-12.1621,0.384529,1,0,-0.00363945,22.5
452,-1.40223,-12.154,0.352937,1,0,-0.00363945,22.55
453,-1.34844,-12.1348,0.350971,1,0,-0.00363945,22.6
454,-1.29461,-12.1153,0.350342,1,0,-0.00363945,22.65
455,-1.25825,-12.1021,0.349917,1,0,-0.00363945,22.7
456,-1.20399,-12.0824,0.349283,1,0,-0.00363945,22.75
457,-1.16807,-12.0694,0.348863,1,0,-0.00363945,22.8
458,-1.12385,-12.0534,0.348346,1,0,-0.00363945,22.85
459,-1.05887,-12.03,0.347587,1,0,-0.00363945,22.9
460,-1.02082,-12.0163,0.347143,1,0,-0.00363945,22.95
461,-0.967562,-11.9971,0.346521,1,0,-0.00363945,23
462,-0.932076,-11.9844,0.346107,1,0,-0.00363945,23.05
463,-0.876137,-11.9644,0.345454,1,0,-0.00363945,23.1
464,-0.822812,-11.9453,0.344832,1,0,-0.00363945,23.15
465,-0.78626,-11.9323,0.344406,1,0,-0.00363945,23.2
466,-0.733377,-11.9134,0.343789,1,0,-0.00363945,23.25
467,-0.695413,-11.8999,0.343346,1,0,-0.00363945,23.3
468,-0.641918,-11.8809,0.342723,1,0,-0.00363945,23.35
469,-0.58791,-11.8618,0.342093,1,0,-0.00363945,23.4
470,-0.552714,-11.8493,0.341683,1,0,-0.00363945,23.45
471,-0.497794,-11.8299,0.341043,1,0,-0.00363945,23.5
472,-0.445031,-11.8113,0.340428,1,0,-0.00363945,23.55
473,-0.407708,-11.7982,0.339994,1,0,-0.00363945,23.6
474,-0.354297,-11.7794,0.339372,1,0,-0.00363945,23.65
475,-0.31871,-11.767,0.338957,1,0,-0.00363945,23.7
476,-0.263082,-11.7475,0.33831,1,0,-0.00363945,23.75
477,-0.20975,-11.7288,0.337689,1,0,-0.00363945,23.8
478,-0.173233,-11.7161,0.337264,1,0,-0.00363945,23.85
479,-0.119377,-11.6973,0.336638,1,0,-0.00363945,23.9
480,-0.0835081,-11.6849,0.33622,1,0,0.0196531,23.95
481,-0.0270055,-11.6647,0.338396,1,0,0.107,24
482,0.0181216,-11.6465,0.350669,1,0,0.194348,24.05
483,0.0638809,-11.6248,0.376432,1,0,0.176878,24.1
484,0.114621,-11.5981,0.40784,1,0,0.00800682,24.15
485,0.149099,-11.5816,0.414408,1,0,-0.00363945,24.2
486,0.201391,-11.5586,0.41409,1,0,0.0429456,24.25
487,0.235888,-11.5428,0.417798,1,0,0.150674,24.3
488,0.287105,-11.5165,0.435495,1,0,0.00393063,24.35
489,0.337794,-11.4905,0.445324,1,0,-0.00363945,24.4
490,0.371861,-11.4743,0.44507,1,0,-0.00363945,24.45
491,0.422793,-11.4501,0.444427,1,0,-0.00363945,24.5
492,0.47504,-11.4254,0.443782,1,0,-0.00363945,24.55
493,0.509965,-11.4088,0.443345,1,0,-0.00363945,24.6
494,0.561846,-11.3843,0.442697,1,0,-0.00363945,24.65
495,0.596329,-11.3681,0.442266,1,0,-0.00363945,24.7
496,0.647951,-11.3438,0.441622,1,0,-0.00363945,24.75
497,0.699041,-11.3198,0.440984,1,0,-0.00363945,24.8
498,0.734314,-11.3032,0.440544,1,0,-0.00363945,24.85
499,0.786342,-11.2788,0.439895,1,0,-0.00363945,24.9
500,0.837463,-11.2549,0.439257,1,0,-0.00363945,24.95
501,0.872081,-11.2387,0.438825,1,0,-0.00363945,25
502,0.926046,-11.2135,0.438153,1,0,-0.00363945,25.05
503,0.960396,-11.1975,0.437725,1,0,-0.00363945,25.1
504,1.01151,-11.1737,0.437088,1,0,-0.00363945,25.15
505,1.0635,-11.1496,0.43644,1,0,-0.00363945,25.2
506,1.09787,-11.1336,0.436012,1,0,-0.00363945,25.25
507,1.15193,-11.1086,0.435339,1,0,-0.00363945,25.3
508,1.18723,-11.0923,0.4349,1,0,-0.00363945,25.35
509,1.23843,-11.0686,0.434262,1,0,-0.00363945,25.4
510,1.29005,-11.0448,0.43362,1,0,0.0487688,25.45
511,1.32438,-11.0281,0.438275,1,0,-0.00363945,25.5
512,1.37564,-11.0034,0.440495,1,0,0.0254762,25.55
513,1.41021,-10.9867,0.442729,1,0,-0.00363945,25.6
514,1.46188,-10.9619,0.44408,1,0,-0.00363945,25.65
515,1.51315,-10.9376,0.443464,1,0,-0.00363945,25.7
516,1.54887,-10.9207,0.443034,1,0,-0.00363945,25.75
517,1.60002,-10.8966,0.442419,1,0,-0.00363945,25.8
518,1.65202,-10.8721,0.441794,1,0,-0.00363945,25.85
519,1.68621,-10.856,0.441384,1,0,-0.00363945,25.9
520,1.73967,-10.8309,0.440742,1,0,-0.00363945,25.95
521,1.77407,-10.8147,0.440328,1,0,-0.00363945,26
522,1.82492,-10.7909,0.439718,1,0,-0.00363945,26.05
523,1.87666,-10.7667,0.439097,1,0,-0.00363945,26.1
524,1.91163,-10.7504,0.438677,1,0,-0.00363945,26.15
525,1.96471,-10.7256,0.438041,1,0,-0.00363945,26.2
526,1.9988,-10.7097,0.437632,1,0,-0.00363945,26.25
527,2.05104,-10.6854,0.437005,1,0,-0.00363945,26.3
528,2.10278,-10.6614,0.436385,1,0,-0.00363945,26.35
529,2.13659,-10.6457,0.43598,1,0,-0.00363945,26.4
530,2.18847,-10.6217,0.435359,1,0,-0.00363945,26.45
531,2.24075,-10.5975,0.434732,1,0,-0.00363945,26.5
532,2.27505,-10.5816,0.434322,1,0,-0.00363945,26.55
533,2.32688,-10.5577,0.433701,1,0,-0.00363945,26.6
534,2.36269,-10.5412,0.433272,1,0,-0.00363945,26.65
535,2.41497,-10.5172,0.432647,1,0,-0.00363945,26.7
536,2.45408,-10.4992,0.432179,1,0,-0.00363945,26.75
537,2.50387,-10.4764,0.431583,1,0,-0.00363945,26.8
538,2.54838,-10.456,0.431051,1,0,-0.00363945,26.85
539,2.60087,-10.432,0.430424,1,0,0.0604153,26.9
540,2.63432,-10.4157,0.435536,1,0,-0.00363945,26.95
541,2.68771,-10.3899,0.439183,1,0,0.0778847,27
542,2.73807,-10.3642,0.449229,1,0,0.234528,27.05
543,2.77032,-10.3445,0.470916,1,0,0.00218368,27.1
544,2.81891,-10.3149,0.490538,1,0,0.0662384,27.15
545,2.85322,-10.2955,0.496112,1,0,-0.00363945,27.2
546,2.90321,-10.2673,0.500671,1,0,0.182119,27.25
547,2.95185,-10.2366,0.520819,1,0,0.0778847,27.3
548,2.98299,-10.216,0.532881,1,0,-0.00655102,27.35
549,3.03307,-10.1852,0.537366,1,0,-0.00363945,27.4
550,3.08079,-10.1569,0.536936,1,0,-0.00363945,27.45
551,3.11428,-10.1371,0.536523,1,0,-0.00363945,27.5
552,3.16333,-10.108,0.535881,1,0,-0.00363945,27.55
553,3.19621,-10.0886,0.535451,1,0,-0.00363945,27.6
554,3.24697,-10.0586,0.534787,1,0,-0.00363945,27.65
555,3.29562,-10.03,0.534151,1,0,-0.00363945,27.7
556,3.32821,-10.0108,0.533725,1,0,-0.00363945,27.75
557,3.37735,-9.98188,0.533084,1,0,-0.00363945,27.8
558,3.41008,-9.96265,0.532656,1,0,-0.00363945,27.85
559,3.45937,-9.93374,0.532013,1,0,-0.00363945,27.9
560,3.50872,-9.90484,0.531369,1,0,-0.00363945,27.95
561,3.54134,-9.88576,0.530943,1,0,-0.00363945,28
562,3.59118,-9.85664,0.530293,1,0,0.00800682,28.05
563,3.64098,-9.82724,0.531286,1,0,0.01383,28.1
564,3.67388,-9.80762,0.53269,1,0,0.0254762,28.15
565,3.72217,-9.77833,0.536444,1,0,0.0312994,28.2
566,3.75451,-9.75836,0.539975,1,0,0.0487688,28.25
567,3.80315,-9.72759,0.547265,1,0,0.0371225,28.3
568,3.8512,-9.69671,0.554177,1,0,0.0487688,28.35
569,3.88238,-9.6763,0.559428,1,0,0.0487688,28.4
570,3.9296,-9.64484,0.567679,1,0,0.0604153,28.45
571,3.97633,-9.61274,0.57766,1,0,0.0487688,28.5
572,4.00767,-9.59101,0.583463,1,0,0.0604153,28.55
573,4.05371,-9.5584,0.593145,1,0,0.0662384,28.6
574,4.10009,-9.52448,0.604477,1,0,0.0545921,28.65
575,4.13068,-9.50187,0.610944,1,0,0.0720615,28.7
576,4.1761,-9.46736,0.622515,1,0,0.0604153,28.75
577,4.20566,-9.44453,0.62947,1,0,0.0662384,28.8
578,4.24977,-9.40979,0.640443,1,0,0.0662384,28.85
579,4.29388,-9.37406,0.652258,1,0,0.0604153,28.9
580,4.32355,-9.34976,0.659133,1,0,0.0662384,28.95
581,4.36759,-9.31288,0.670459,1,0,0.0837078,29
582,4.41013,-9.27581,0.684047,1,0,0.0837078,29.05
583,4.4401,-9.24892,0.694264,1,0,0.0778847,29.1
584,4.48061,-9.21183,0.707513,1,0,0.0720615,29.15
585,4.52217,-9.173,0.720258,1,0,0.0720615,29.2
586,4.54937,-9.14712,0.72846,1,0,0.0720615,29.25
587,4.59088,-9.10678,0.741081,1,0,0.0778847,29.3
588,4.61768,-9.08005,0.749965,1,0,0.0662384,29.3
589,4.65816,-9.03904,0.762172,1,0,0.0662384,29.4
590,4.69732,-8.99857,0.773613,1,0,0.0604153,29.45
591,4.72383,-8.97081,0.780777,1,0,0.0604153,29.45
592,4.76344,-8.9287,0.791193,1,0,0.0662384,29.55
593,4.80129,-8.88735,0.802297,1,0,0.0604153,29.6
594,4.82782,-8.85799,0.809407,1,0,0.0545921,29.65
595,4.86539,-8.81582,0.819124,1,0,0.0604153,29.7
596,4.89069,-8.78692,0.825917,1,0,0.0545921,29.7
597,4.92932,-8.74212,0.835904,1,0,0.0487688,29.8
598,4.96599,-8.69906,0.84441,1,0,0.0487688,29.85
599,4.99071,-8.66964,0.850096,1,0,0.0429456,29.9
600,5.02726,-8.62571,0.857864,1,0,0.0429456,29.95
601,5.05149,-8.59624,0.863002,1,0,0.0487688,29.95
602,5.08845,-8.55063,0.871051,1,0,0.0429456,30.05
603,5.12375,-8.5064,0.878636,1,0,0.0371225,30.1
604,5.14742,-8.47659,0.882906,1,0,0.0487688,30.15
605,5.18247,-8.43162,0.890589,1,0,0.0487688,30.2
606,5.2069,-8.39965,0.896568,1,0,0.0604153,30.2
607,5.24239,-8.35199,0.907047,1,0,0.0545921,30.3
608,5.27581,-8.30622,0.916628,1,0,0.0604153,30.35
609,5.29775,-8.27554,0.923467,1,0,0.0662384,30.4
610,5.33063,-8.22838,0.934859,1,0,0.0604153,30.45
611,5.36138,-8.18343,0.944915,1,0,0.0662384,30.5
612,5.38295,-8.15114,0.952648,1,0,0.0604153,30.55
613,5.41589,-8.10099,0.963824,1,0,0.0662384,30.6
614,5.43614,-8.06948,0.971203,1,0,0.0662384,30.65
615,5.46656,-8.021,0.982694,1,0,0.0778847,30.7
616,5.49612,-7.9721,0.995843,1,0,0.0837078,30.75
617,5.51496,-7.93981,1.00533,1,0,0.0953541,30.8
618,5.54252,-7.89067,1.02115,1,0,0.0837078,30.85
619,5.56948,-7.8413,1.03543,1,0,0.0953541,30.9
620,5.58863,-7.80474,1.04721,1,0,0.101177,30.95
621,5.61406,-7.75404,1.06438,1,0,0.089531,31
622,5.6308,-7.71978,1.07491,1,0,0.0837078,31.05
623,5.65462,-7.66991,1.08924,1,0,0.112823,31.1
624,5.67819,-7.6171,1.10782,1,0,0.101177,31.15
625,5.69304,-7.5825,1.11943,1,0,0.101177,31.2
626,5.71468,-7.53013,1.13679,1,0,0.107,31.25
627,5.72961,-7.49213,1.14986,1,0,0.089531,31.3
628,5.75005,-7.43866,1.16604,1,0,0.101177,31.35
629,5.76896,-7.38638,1.18264,1,0,0.089531,31.4
630,5.78174,-7.34985,1.19334,1,0,0.0720615,31.45
631,5.8001,-7.29639,1.20666,1,0,0.0720615,31.5
632,5.81815,-7.24227,1.21911,1,0,0.0837078,31.55
633,5.83,-7.20459,1.22914,1,0,0.0778847,31.6
634,5.84668,-7.14963,1.24291,1,0,0.0662384,31.65
635,5.85748,-7.11333,1.25073,1,0,0.0837078,31.7
636,5.87509,-7.05054,1.26627,1,0,0.089531,31.75
637,5.88685,-7.00495,1.27923,1,0,0.0778847,31.8
638,5.89634,-6.96715,1.28864,1,0,0.0720615,31.85
639,5.91006,-6.91052,1.30183,1,0,0.0778847,31.9
640,5.92255,-6.85563,1.31497,1,0,0.0662384,31.95
641,5.93076,-6.81858,1.32263,1,0,0.0604153,32
642,5.94281,-6.76241,1.33341,1,0,0.0720615,32.05
643,5.95026,-6.7254,1.34142,1,0,0.0604153,32.1
644,5.9615,-6.66713,1.35302,1,0,0.0545921,32.15
645,5.972,-6.61098,1.36259,1,0,0.0487688,32.2
646,5.9788,-6.57354,1.36841,1,0,0.0604153,32.25
647,5.98828,-6.51812,1.37809,1,0,0.0545921,32.3
648,5.9945,-6.47978,1.3847,1,0,0.0487688,32.35
649,6.0036,-6.42154,1.39393,1,0,-0.160865,32.4
650,6.01604,-6.36558,1.37924,1,0,-0.131749,32.5
651,6.02603,-6.3292,1.36386,1,0,0.0487688,32.5
652,6.03894,-6.27394,1.35939,1,0,0.0953541,32.55
653,6.04798,-6.21874,1.37401,1,0,0.101177,32.65
654,6.05344,-6.1797,1.38633,1,0,-0.226666,32.65
655,6.06418,-6.1256,1.37936,1,0,0.112823,32.7
656,6.07466,-6.08607,1.36708,1,0,0.141939,32.75
657,6.08185,-6.02999,1.38987,1,0,0.136116,32.8
658,6.08753,-5.97298,1.41384,1,0,0.118647,32.85
659,6.09086,-5.9353,1.42775,1,0,0.112823,32.9
660,6.09512,-5.87829,1.44778,1,0,0.118647,32.95
661,6.09726,-5.84076,1.46123,1,0,0.0953541,33
662,6.09993,-5.78413,1.47911,1,0,0.101177,33.05
663,6.10183,-5.72659,1.49651,1,0,0.101177,33.1
664,6.10249,-5.68873,1.50811,1,0,0.0837078,33.15
665,6.10303,-5.63221,1.52334,1,0,0.0662384,33.2
666,6.10335,-5.57269,1.53592,1,0,0.0604153,33.3
667,6.10336,-5.53396,1.54308,1,0,0.0662384,33.3
668,6.10279,-5.47681,1.55419,1,0,0.0545921,33.4
669,6.10219,-5.43886,1.56075,1,0,0.0429456,33.4
670,6.1012,-5.3826,1.56861,1,0,0.0487688,33.5
671,6.09972,-5.32558,1.57688,1,0,0.0429456,33.55
672,6.09845,-5.28484,1.58231,1,0,0.0545921,33.6
673,6.09609,-5.22742,1.59116,1,0,0.0604153,33.65
674,6.0941,-5.18987,1.59785,1,0,0.0545921,33.7
675,6.09061,-5.13252,1.6077,1,0,0.0429456,33.8
676,6.08692,-5.07556,1.61569,1,0,0.0487688,33.85
677,6.08426,-5.03816,1.62102,1,0,0.0429456,33.9
678,6.0799,-4.98167,1.62878,1,0,0.0545921,33.95
679,6.07612,-4.93833,1.63563,1,0,0.0545921,34
680,6.07123,-4.88756,1.64386,1,0,0.0487688,34.05
681,6.06603,-4.83675,1.65137,1,0,0.0487688,34.1
682,6.05998,-4.78117,1.65953,1,0,0.0429456,34.15
683,6.05364,-4.72613,1.66687,1,0,0.0371225,34.2
684,6.04885,-4.68585,1.6717,1,0,0.0254762,34.25
685,6.04319,-4.63859,1.6761,1,0,0.0371225,34.3
686,6.03727,-4.59087,1.68079,1,0,0.0312994,34.35
687,6.03007,-4.53536,1.6862,1,0,0.0371225,34.4
688,6.02222,-4.47846,1.69266,1,0,0.264226,34.45
689,6.01449,-4.44114,1.71158,1,0,0.194348,34.5
690,5.99951,-4.38727,1.74832,1,0,0.240933,34.55
691,5.98842,-4.35156,1.7737,1,0,-0.00363945,34.6
692,5.97106,-4.2943,1.79862,1,0,-0.00363945,34.65
693,5.95822,-4.23838,1.79799,1,0,0.159409,34.7
694,5.94804,-4.20227,1.80907,1,0,0.0545921,34.75
695,5.9312,-4.14753,1.82547,1,0,-0.00363945,34.8
696,5.91652,-4.0929,1.82715,1,0,-0.00363945,34.85
697,5.90752,-4.05824,1.82669,1,0,-0.00363945,34.9
698,5.89381,-4.00527,1.82594,1,0,-0.00363945,34.95
699,5.87926,-3.949,1.82526,1,0,-0.00363945,35
700,5.87013,-3.9136,1.8249,1,0,-0.00363945,35.05
701,5.85632,-3.86,1.82437,1,0,-0.00363945,35.1
702,5.8425,-3.80625,1.82381,1,0,-0.00363945,35.15
703,5.82889,-3.75324,1.82333,1,0,-0.00363945,35.2
704,5.81797,-3.71041,1.82265,1,0,-0.00363945,35.25
705,5.80621,-3.6643,1.82213,1,0,-0.00363945,35.3
706,5.79379,-3.61553,1.82157,1,0,-0.00363945,35.35
707,5.77949,-3.55918,1.82093,1,0,0.00218368,35.4
708,5.76917,-3.51912,1.8214,1,0,-0.00363945,35.45
709,5.75529,-3.46462,1.82096,1,0,0.41,35.5
710,5.74437,-3.43085,1.83537,1,0,0.0429456,35.55
711,5.72121,-3.38147,1.88198,1,0,0.234528,35.6
712,5.69927,-3.32869,1.90827,1,0,0.188525,35.65
713,5.67507,-3.27924,1.9412,1,0,0.41,35.7
714,5.65517,-3.24655,1.9796,1,0,-0.00363945,35.75
715,5.62538,-3.20102,2.02374,1,0,-0.00363945,35.8
716,5.60031,-3.14943,2.02318,1,0,-0.00363945,35.85
717,5.58372,-3.11512,2.02275,1,0,-0.00363945,35.9
718,5.55974,-3.06545,2.02214,1,0,-0.00363945,35.95
719,5.5352,-3.01455,2.02151,1,0,-0.00363945,36
720,5.51872,-2.98031,2.02109,1,0,-0.00363945,36.05
721,5.49459,-2.93012,2.02048,1,0,-0.00363945,36.1
722,5.47071,-2.88036,2.01986,1,0,-0.00363945,36.15
723,5.45501,-2.84762,2.01946,1,0,-0.00363945,36.2
724,5.43018,-2.79576,2.01882,1,0,-0.00363945,36.25
725,5.40589,-2.74493,2.0182,1,0,-0.00363945,36.3
726,5.38995,-2.71154,2.01779,1,0,-0.00363945,36.35
727,5.36561,-2.66049,2.01716,1,0,-0.00363945,36.4
728,5.34266,-2.61227,2.01657,1,0,-0.00363945,36.45
729,5.32562,-2.5764,2.01613,1,0,-0.00363945,36.5
730,5.30216,-2.52699,2.01552,1,0,-0.00363945,36.55
731,5.27847,-2.47699,2.01491,1,0,-0.00363945,36.6
732,5.26223,-2.44268,2.01448,1,0,-0.00363945,36.65
733,5.23839,-2.39224,2.01386,1,0,-0.00363945,36.7
734,5.21465,-2.34194,2.01325,1,0,-0.00363945,36.75
735,5.19699,-2.30447,2.01279,1,0,0.0196531,36.8
736,5.17335,-2.25531,2.01483,1,0,-0.00363945,36.85
737,5.14828,-2.20291,2.01523,1,0,-0.00363945,36.9
738,5.13246,-2.16951,2.01481,1,0,-0.00363945,36.95
739,5.10867,-2.11919,2.01416,1,0,-0.00363945,37
740,5.08389,-2.06667,2.01341,1,0,-0.00363945,37.05
741,5.06831,-2.03364,2.013,1,0,-0.00363945,37.1
742,5.04376,-1.98148,2.01235,1,0,-0.00363945,37.15
743,5.02006,-1.93106,2.01173,1,0,-0.00363945,37.2
744,5.00432,-1.89753,2.01131,1,0,-0.00363945,37.25
745,4.98069,-1.84712,2.01069,1,0,-0.00363945,37.3
746,4.96467,-1.81289,2.01026,1,0,-0.00363945,37.35
747,4.93972,-1.75952,2.0096,1,0,-0.00363945,37.4
748,4.91539,-1.70738,2.00895,1,0,0.0458572,37.45
749,4.89873,-1.67338,2.01293,1,0,0.328862,37.5
750,4.87132,-1.62612,2.03968,1,0,0.203082,37.55
751,4.83905,-1.58092,2.08147,1,0,-0.00363945,37.6
752,4.81714,-1.54696,2.09308,1,0,-0.00363945,37.65
753,4.78881,-1.49753,2.09255,1,0,-0.00363945,37.7
754,4.76057,-1.44819,2.09194,1,0,-0.00363945,37.75
755,4.74241,-1.41636,2.09145,1,0,-0.00363945,37.8
756,4.71425,-1.36698,2.09072,1,0,-0.00363945,37.85
757,4.68576,-1.31696,2.09008,1,0,-0.00363945,37.9
758,4.66065,-1.27282,2.08952,1,0,-0.00363945,37.95
759,4.63751,-1.23207,2.089,1,0,-0.00363945,38
760,4.61704,-1.196,2.08854,1,0,-0.00363945,38.05
761,4.58972,-1.1478,2.08793,1,0,-0.00363945,38.1
762,4.56206,-1.09892,2.0873,1,0,-0.00363945,38.15
763,4.54355,-1.06618,2.08689,1,0,-0.00363945,38.2
764,4.51454,-1.01477,2.08623,1,0,-0.00363945,38.25
765,4.49598,-0.981866,2.08581,1,0,-0.00363945,38.3
766,4.46832,-0.932742,2.08519,1,0,-0.00363945,38.35
767,4.44065,-0.883535,2.08456,1,0,-0.00363945,38.4
768,4.42173,-0.849838,2.08414,1,0,-0.00363945,38.45
769,4.39439,-0.80111,2.08352,1,0,-0.00363945,38.5
770,4.36779,-0.753621,2.08291,1,0,-0.00363945,38.55
771,4.34868,-0.719456,2.08248,1,0,-0.00363945,38.6
772,4.32298,-0.67345,2.08189,1,0,-0.00363945,38.65
773,4.29954,-0.631448,2.08136,1,0,-0.00363945,38.7
774,4.27218,-0.582353,2.08074,1,0,-0.00363945,38.75
775,4.24485,-0.533248,2.08011,1,0,-0.00946259,38.8
776,4.22652,-0.499922,2.07875,1,0,0.0196531,38.85
777,4.1988,-0.450737,2.0806,1,0,0.0371225,38.9
778,4.17084,-0.402887,2.08618,1,0,0.0545921,38.95
779,4.15184,-0.371467,2.09203,1,0,0.0662384,39
780,4.11917,-0.319216,2.10389,1,0,0.0604153,39.05
781,4.09954,-0.288432,2.11066,1,0,0.0778847,39.1
782,4.06351,-0.233799,2.12508,1,0,0.0953541,39.15
783,4.03861,-0.197934,2.13742,1,0,0.130293,39.2
784,4.00534,-0.15306,2.1583,1,0,0.12447,39.25
785,3.98197,-0.122864,2.17292,1,0,0.153586,39.3
786,3.94581,-0.0787485,2.19798,1,0,0.171055,39.35
787,3.92053,-0.0499254,2.21728,1,0,0.182701,39.4
788,3.88187,-0.00847667,2.24773,1,0,0.188525,39.45
789,3.83856,0.0345743,2.28236,1,0,0.205994,39.5
790,3.81052,0.0604341,2.30598,1,0,0.205994,39.55
791,3.76831,0.0970389,2.34086,1,0,0.200171,39.6
792,3.7247,0.13243,2.37521,1,0,0.205994,39.65
793,3.69464,0.155281,2.39893,1,0,0.188525,39.7
794,3.64847,0.188799,2.43234,1,0,-0.0385785,39.75
795,3.60538,0.223447,2.43897,1,0,-0.155042,39.8
796,3.57731,0.250585,2.42412,1,0,-0.155042,39.8
797,3.53899,0.290732,2.39802,1,0,-0.201627,39.9
798,3.51381,0.319326,2.37601,1,0,-0.0269322,39.9
799,3.47574,0.361779,2.3564,1,0,-0.061871,39.95
800,3.43682,0.403047,2.34748,1,0,-0.0560479,40.05
801,3.41223,0.429717,2.34124,1,0,-0.236566,40.05
802,3.37645,0.472768,2.31688,1,0,0.147763,40.1
803,3.34011,0.516774,2.30479,1,0,-0.048478,40.2
804,3.31459,0.543624,2.30834,1,0,-0.142813,40.2
805,3.27919,0.58734,2.29075,1,0,0.229287,40.25
806,3.24384,0.628363,2.29224,1,0,0.182701,40.35
807,3.20257,0.665658,2.32466,1,0,0.293341,40.4
808,3.17208,0.689805,2.35639,1,0,-0.41,40.4
809,3.12932,0.725053,2.37961,1,0,-0.383891,40.5
810,3.09632,0.770451,2.32464,1,0,-0.31052,40.55
811,3.07539,0.803119,2.28679,1,0,0.229287,40.6
812,3.0423,0.850073,2.26119,1,0,0.23511,40.65
813,3.00338,0.88658,2.29746,1,0,0.334104,40.7
814,2.97278,0.910852,2.3347,1,0,0.281695,40.75
815,2.9276,0.943813,2.38495,1,0,0.188525,40.8
816,2.89737,0.96595,2.40935,1,0,0.41,40.9
817,2.84826,0.997175,2.46154,1,0,0.41,40.95
818,2.79778,1.02082,2.53094,1,0,-0.00655102,41.05
819,2.76402,1.03669,2.56314,1,0,0.369042,41.1
820,2.71799,1.063,2.58155,1,0,0.41,41.15
821,2.66122,1.08236,2.65317,1,0,0.398158,41.25
822,2.62553,1.09199,2.69809,1,0,0.34575,41.25
823,2.57037,1.10464,2.76083,1,0,0.351573,41.35
824,2.52814,1.11245,2.80611,1,0,0.200171,41.4
825,2.46372,1.12347,2.85856,1,0,0.182701,41.45
826,2.42119,1.13106,2.88283,1,0,0.334104,41.55
827,2.368,1.13641,2.92845,1,0,0.252579,41.65
828,2.32884,1.13855,2.96119,1,0,0.188525,41.7
829,2.27389,1.14141,2.99688,1,0,0.147763,41.8
830,2.21616,1.1443,3.02451,1,0,0.369042,41.85
831,2.18097,1.14328,3.05433,1,0,0.357396,41.9
832,2.12439,1.1361,3.11672,1,0,0.316634,41.95
833,2.06895,1.12657,-3.11049,1,0,0.153586,42.05
834,2.03286,1.12056,-3.08546,1,0,0.270049,42.05
835,1.97643,1.11028,-3.04747,1,0,-0.0379964,42.15
836,1.92272,1.10054,-3.02689,1,0,0.136116,42.2
837,1.86818,1.09193,-3.01306,1,0,0.264226,42.25
838,1.83157,1.08253,-2.98687,1,0,0.147763,42.25
839,1.77749,1.06737,-2.954,1,0,0.171055,42.3
840,1.72338,1.05153,-2.9256,1,0,0.0837078,42.35
841,1.68127,1.03961,-2.91226,1,0,-0.0385785,42.4
842,1.62745,1.02705,-2.91367,1,0,-0.216185,42.45
843,1.5916,1.02175,-2.93113,1,0,0.12447,42.5
844,1.5313,1.01212,-2.94317,1,0,0.392335,42.55
845,1.49037,0.997664,-2.90797,1,0,0.32828,42.6
846,1.43743,0.972742,-2.84691,1,0,0.229287,42.7
847,1.4043,0.957061,-2.8185,1,0,0.0953541,42.75
848,1.35226,0.933907,-2.79218,1,0,-0.079923,42.85
849,1.31648,0.920284,-2.7908,1,0,0.275872,42.9
850,1.25951,0.898795,-2.78368,1,0,0.41,43
851,1.2115,0.867843,-2.71915,1,0,0.41,43.05
852,1.18165,0.845069,-2.67265,1,0,0.41,43.1
853,1.13962,0.808944,-2.60381,1,0,0.374866,43.15
854,1.09979,0.770752,-2.53969,1,0,0.270049,43.25
855,1.0722,0.743692,-2.50446,1,0,0.21764,43.3
856,1.03241,0.704463,-2.46446,1,0,0.304988,43.35
857,0.994729,0.662446,-2.41636,1,0,0.258403,43.4
858,0.97098,0.633919,-2.38636,1,0,-0.0170327,43.45
859,0.932819,0.591193,-2.36418,1,0,-0.0379964,43.5
860,0.891526,0.5521,-2.37005,1,0,0.41,43.55
861,0.866026,0.524299,-2.35571,1,0,0.41,43.6
862,0.835468,0.474501,-2.2846,1,0,0.41,43.7
863,0.817808,0.440899,-2.23756,1,0,0.275872,43.75
864,0.792031,0.38924,-2.17888,1,0,0.41,43.8
865,0.770019,0.336839,-2.11592,1,0,0.403981,43.9
866,0.758119,0.30056,-2.06897,1,0,0.392335,43.95
867,0.742671,0.242952,-1.99728,1,0,0.223464,44.05
868,0.733241,0.206995,-1.96463,1,0,0.089531,44.1
869,0.716694,0.153362,-1.93891,1,0,0.299165,44.15
870,0.702803,0.0991422,-1.90264,1,0,0.304988,44.2
871,0.696602,0.0609402,-1.86698,1,0,0.281695,44.25
872,0.68979,0.0068475,-1.81913,1,0,0.211817,44.35
873,0.683737,-0.0485607,-1.77989,1,0,0.182701,44.4
874,0.679628,-0.0893175,-1.75632,1,0,-0.0694413,44.45
875,0.674609,-0.132719,-1.74204,1,0,-0.0694413,44.5
876,0.663914,-0.182682,-1.75282,1,0,0.246756,44.55
877,0.653828,-0.244024,-1.74291,1,0,0.23511,44.6
878,0.652004,-0.301653,-1.70151,1,0,0.194348,44.7
879,0.651328,-0.33965,-1.67864,1,0,0.176878,44.7
880,0.651306,-0.395667,-1.64791,1,0,0.153586,44.8
881,0.651814,-0.434462,-1.62926,1,0,0.141939,44.8
882,0.653413,-0.490603,-1.60429,1,0,0.130293,44.85
883,0.65617,-0.550042,-1.58017,1,0,0.112823,44.9
884,0.658306,-0.5885,-1.56684,1,0,0.0953541,44.95
885,0.661889,-0.644642,-1.54974,1,0,0.089531,45
886,0.666108,-0.700946,-1.5342,1,0,0.0720615,45.05
887,0.66915,-0.739408,-1.52554,1,0,0.0604153,45.1
888,0.673849,-0.795483,-1.51461,1,0,0.0662384,45.15
889,0.67902,-0.850162,-1.50375,1,0,0.0545921,45.2
890,0.682856,-0.888912,-1.49723,1,0,0.0545921,45.25
0,0.688866,-0.945429,-1.48778,1,0,0.0487688,0.05
1,0.693071,-0.983194,-1.4821,1,0,0.0429456,0.05
2,0.699384,-1.03755,-1.47459,1,0,0.0487688,0.15
3,0.706356,-1.09362,-1.46654,1,0,0.0371225,0.2
4,0.713368,-1.14845,-1.46013,1,0,0.0429456,0.25
5,0.718898,-1.18942,-1.45488,1,0,0.0837078,0.3
6,0.727743,-1.24495,-1.44231,1,0,0.0371225,0.35
7,0.733765,-1.28304,-1.43682,1,0,0.0312994,0.35
8,0.742373,-1.33846,-1.43108,1,0,0.101177,0.45
9,0.752811,-1.39342,-1.41655,1,0,0.01383,0.5
10,0.759492,-1.43062,-1.41251,1,0,0.41,0.5
11,0.773069,-1.48695,-1.38502,1,0,0.211817,0.6
12,0.792764,-1.53936,-1.33705,1,0,0.240933,0.65
13,0.806534,-1.57513,-1.30922,1,0,-0.00363945,0.65
14,0.82514,-1.62788,-1.28976,1,0,0.41,0.75
15,0.845055,-1.679,-1.25974,1,0,-0.00363945,0.8
16,0.861952,-1.71259,-1.23077,1,0,0.159409,0.8
17,0.884015,-1.76677,-1.21556,1,0,-0.00363945,0.9
18,0.904592,-1.81696,-1.20783,1,0,0.41,0.95
19,0.920622,-1.85123,-1.18989,1,0,-0.00363945,0.95
20,0.95031,-1.90039,-1.14564,1,0,-0.00363945,1
21,0.972732,-1.9501,-1.14621,1,0,-0.00363945,1.1
22,0.989805,-1.98805,-1.14662,1,0,-0.00363945,1.1
23,1.01311,-2.03994,-1.14724,1,0,-0.00363945,1.15
24,1.02885,-2.07506,-1.14767,1,0,-0.00363945,1.2
25,1.05174,-2.12618,-1.14828,1,0,0.00218368,1.25
26,1.07512,-2.17817,-1.14821,1,0,-0.00363945,1.3
27,1.0909,-2.21346,-1.14869,1,0,-0.00363945,1.35
28,1.1143,-2.26586,-1.14932,1,0,-0.00363945,1.4
29,1.1305,-2.30221,-1.14984,1,0,-0.00363945,1.45
30,1.15377,-2.35466,-1.15078,1,0,-0.00363945,1.5
31,1.17644,-2.40572,-1.1514,1,0,-0.00363945,1.55
32,1.19185,-2.44049,-1.15183,1,0,-0.00363945,1.6
33,1.21455,-2.49176,-1.15245,1,0,-0.00363945,1.65
34,1.23771,-2.54415,-1.15309,1,0,-0.00363945,1.7
35,1.25399,-2.58105,-1.15354,1,0,-0.00363945,1.75
36,1.277,-2.63324,-1.15418,1,0,-0.00363945,1.8
37,1.2925,-2.66846,-1.1546,1,0,-0.00363945,1.85
38,1.31511,-2.71992,-1.15523,1,0,-0.00363945,1.9
39,1.33833,-2.77286,-1.15587,1,0,-0.00363945,1.95
40,1.35343,-2.80734,-1.15629,1,0,-0.00363945,2
41,1.37673,-2.86061,-1.15694,1,0,-0.00363945,2.05
42,1.39186,-2.89523,-1.15736,1,0,-0.00363945,2.1
43,1.41491,-2.94809,-1.158,1,0,-0.00363945,2.15
44,1.43719,-2.99927,-1.15863,1,0,-0.00363945,2.2
45,1.45206,-3.03347,-1.15904,1,0,-0.00363945,2.25
46,1.47591,-3.0884,-1.15971,1,0,-0.00363945,2.3
47,1.49205,-3.12564,-1.16016,1,0,-0.00363945,2.35
48,1.51705,-3.18342,-1.16086,1,0,-0.00363945,2.4
49,1.53225,-3.2186,-1.16129,1,0,-0.00363945,2.45
50,1.55469,-3.2706,-1.16192,1,0,-0.00363945,2.5
51,1.57728,-3.32306,-1.16256,1,0,-0.00363945,2.55
52,1.59215,-3.35763,-1.16297,1,0,-0.00363945,2.6
53,1.61652,-3.41436,-1.16366,1,0,-0.00363945,2.65
54,1.63137,-3.44899,-1.16408,1,0,-0.00363945,2.7
55,1.65398,-3.5018,-1.16472,1,0,-0.00363945,2.75
56,1.67575,-3.55274,-1.16534,1,0,-0.00363945,2.8
57,1.69045,-3.58719,-1.16576,1,0,-0.00363945,2.85
58,1.71363,-3.64157,-1.16641,1,0,-0.00363945,2.9
59,1.72852,-3.67656,-1.16684,1,0,-0.00363945,2.95
60,1.75074,-3.72887,-1.16747,1,0,-0.00363945,3
61,1.77273,-3.78072,-1.1681,1,0,-0.00363945,3.05
62,1.78741,-3.81537,-1.16852,1,0,-0.00363945,3.1
63,1.80953,-3.86767,-1.16915,1,0,-0.00363945,3.15
64,1.83201,-3.92091,-1.16979,1,0,-0.00363945,3.25
65,1.84664,-3.95562,-1.17021,1,0,-0.00363945,3.25
66,1.86793,-4.00621,-1.17082,1,0,-0.00363945,3.3
67,1.88922,-4.05686,-1.17144,1,0,-0.00363945,3.35
68,1.90361,-4.09115,-1.17185,1,0,-0.00363945,3.4
69,1.92383,-4.13941,-1.17243,1,0,-0.364675,3.45
70,1.94191,-4.19296,-1.19754,1,0,-0.00363945,3.5
71,1.956,-4.24706,-1.22766,1,0,-0.00363945,3.6
72,1.96813,-4.28123,-1.22805,1,0,-0.00363945,3.6
73,1.98654,-4.33317,-1.22865,1,0,-0.22492,3.65
74,2.00118,-4.38498,-1.24976,1,0,-0.00363945,3.7
75,2.01573,-4.43861,-1.26339,1,0,-0.00363945,3.8
76,2.02716,-4.47493,-1.26393,1,0,-0.120103,3.8
77,2.04187,-4.52942,-1.27775,1,0,-0.00363945,3.9
78,2.05659,-4.58255,-1.28303,1,0,-0.0793405,3.95
79,2.06619,-4.61966,-1.29075,1,0,-0.189981,3.95
80,2.07694,-4.67372,-1.31585,1,0,-0.00363945,4
81,2.08864,-4.72846,-1.32652,1,0,0.328862,4.1
82,2.10087,-4.76502,-1.30746,1,0,-0.20745,4.1
83,2.12164,-4.81699,-1.27788,1,0,-0.155042,4.2
84,2.133,-4.87113,-1.30313,1,0,0.0458572,4.25
85,2.14236,-4.90825,-1.30563,1,0,-0.155042,4.25
86,2.15523,-4.96147,-1.31588,1,0,-0.31809,4.3
87,2.16026,-5.00273,-1.34713,1,0,-0.184157,4.35
88,2.16442,-5.05923,-1.38918,1,0,-0.230743,4.4
89,2.16745,-5.11349,-1.42502,1,0,0.0773021,4.5
90,2.17009,-5.15022,-1.43754,1,0,0.0668205,4.5
91,2.18002,-5.20662,-1.42525,1,0,-0.0560479,4.55
92,2.18796,-5.26172,-1.42737,1,0,-0.22492,4.65
93,2.19041,-5.30158,-1.44654,1,0,0.328862,4.65
94,2.19375,-5.35912,-1.46205,1,0,0.0458572,4.7
95,2.20401,-5.41336,-1.44133,1,0,0.150674,4.8
96,2.21162,-5.45203,-1.42667,1,0,0.349826,4.8
97,2.22732,-5.50512,-1.38336,1,0,0.349826,4.85
98,2.24882,-5.55676,-1.32378,1,0,0.265973,4.95
99,2.26455,-5.59265,-1.28955,1,0,-0.0851636,4.95
100,2.28483,-5.64557,-1.26878,1,0,-0.189981,5
101,2.29629,-5.69962,-1.29641,1,0,-0.31809,5.1
102,2.30085,-5.73756,-1.32943,1,0,-0.265682,5.1
103,2.30502,-5.79537,-1.37904,1,0,0.0353756,5.15
104,2.31234,-5.85083,-1.39306,1,0,-0.090987,5.25
105,2.3183,-5.88906,-1.39899,1,0,-0.189981,5.25
106,2.32296,-5.94617,-1.42669,1,0,0.0458572,5.3
107,2.32671,-5.98339,-1.43351,1,0,0.00393063,5.35
108,2.33477,-6.03707,-1.43063,1,0,-0.213273,5.4
109,2.33902,-6.09448,-1.45277,1,0,-0.387968,5.45
110,2.33697,-6.13254,-1.48964,1,0,-0.149218,5.5
111,2.33196,-6.18768,-1.53596,1,0,-0.300621,5.55
112,2.32562,-6.24629,-1.58061,1,0,0.0144123,5.6
113,2.32183,-6.28479,-1.59641,1,0,-0.22492,5.65
114,2.31708,-6.34097,-1.61615,1,0,-0.288974,5.7
115,2.30935,-6.37973,-1.64889,1,0,-0.277328,5.75
116,2.2953,-6.43585,-1.69829,1,0,-0.166688,5.8
117,2.2813,-6.49133,-1.7322,1,0,0.0458572,5.9
118,2.27443,-6.52667,-1.73637,1,0,-0.00655102,5.9
119,2.26552,-6.58285,-1.73459,1,0,-0.364675,6
120,2.25086,-6.638,-1.76688,1,0,-0.230743,6.1
121,2.23802,-6.67306,-1.79706,1,0,-0.120103,6.15
122,2.21964,-6.72777,-1.82465,1,0,-0.248212,6.2
123,2.20615,-6.76346,-1.8479,1,0,-0.265682,6.25
124,2.17829,-6.82354,-1.90032,1,0,0.0458572,6.35
125,2.16687,-6.84915,-1.91255,1,0,-0.370498,6.4
126,2.14967,-6.8965,-1.91596,1,0,-0.41,6.45
127,2.11579,-6.95019,-1.988,1,0,-0.283151,6.55
128,2.08305,-6.9969,-2.04339,1,0,-0.323913,6.6
129,2.05826,-7.0294,-2.08292,1,0,-0.131749,6.65
130,2.02425,-7.07483,-2.1187,1,0,-0.219096,6.75
131,2.00146,-7.10465,-2.14067,1,0,-0.143395,6.8
132,1.96572,-7.14926,-2.1707,1,0,-0.271505,6.85
133,1.92786,-7.19167,-2.20981,1,0,-0.341383,6.9
134,1.90019,-7.21753,-2.2472,1,0,-0.137572,6.95
135,1.85803,-7.25682,-2.28733,1,0,-0.22492,7.05
136,1.81625,-7.29479,-2.32213,1,0,-0.172511,7.1
137,1.78709,-7.32017,-2.34319,1,0,-0.259858,7.15
138,1.74278,-7.35483,-2.38314,1,0,-0.00363945,7.2
139,1.71278,-7.3785,-2.39958,1,0,-0.00363945,7.25
140,1.67105,-7.41656,-2.4003,1,0,-0.00363945,7.3
141,1.62859,-7.4554,-2.40041,1,0,-0.00363945,7.35
142,1.60081,-7.48077,-2.40065,1,0,-0.00363945,7.4
143,1.55822,-7.51956,-2.40129,1,0,-0.00363945,7.45
144,1.51726,-7.55683,-2.4019,1,0,-0.00363945,7.5
145,1.4887,-7.58278,-2.40233,1,0,-0.00363945,7.55
146,1.44721,-7.62044,-2.40295,1,0,-0.41,7.6
147,1.40174,-7.65454,-2.4353,1,0,-0.00363945,7.65
148,1.36972,-7.67427,-2.46406,1,0,-0.00363945,7.7
149,1.32258,-7.7117,-2.46531,1,0,-0.00363945,7.75
150,1.29309,-7.73527,-2.46573,1,0,-0.41,7.8
151,1.2465,-7.76767,-2.48964,1,0,-0.00363945,7.85
152,1.19682,-7.79505,-2.52874,1,0,-0.00363945,7.9
153,1.16503,-7.81733,-2.52899,1,0,-0.00363945,7.95
154,1.11819,-7.8501,-2.52963,1,0,-0.00363945,8
155,1.08137,-7.87583,-2.53014,1,0,-0.41,8.05
156,1.0406,-7.90075,-2.549,1,0,-0.00363945,8.1
157,0.999649,-7.91831,-2.59163,1,0,-0.00363945,8.15
158,0.950257,-7.94762,-2.5946,1,0,-0.00363945,8.2
159,0.901117,-7.9774,-2.59524,1,0,-0.00363945,8.25
160,0.869302,-7.99665,-2.59566,1,0,-0.00363945,8.3
161,0.820793,-8.02598,-2.59629,1,0,-0.00363945,8.35
162,0.787492,-8.04609,-2.59672,1,0,-0.41,8.35
163,0.72792,-8.07644,-2.62644,1,0,-0.00363945,8.45
164,0.686715,-8.09206,-2.65907,1,0,-0.00363945,8.5
165,0.635845,-8.11844,-2.65993,1,0,-0.00363945,8.55
166,0.601814,-8.13614,-2.66033,1,0,-0.00363945,8.6
167,0.550619,-8.1627,-2.66115,1,0,-0.00363945,8.65
168,0.515239,-8.18105,-2.66159,1,0,-0.00363945,8.65
169,0.462561,-8.20833,-2.66224,1,0,-0.00363945,8.75
170,0.412406,-8.23426,-2.66286,1,0,-0.00363945,8.8
171,0.378059,-8.252,-2.66329,1,0,-0.00363945,8.85
172,0.327672,-8.27798,-2.66391,1,0,-0.00363945,8.9
173,0.293465,-8.2956,-2.66434,1,0,-0.00363945,8.9
174,0.24118,-8.3225,-2.66498,1,0,-0.00363945,9
175,0.190952,-8.3483,-2.66561,1,0,-0.00363945,9.05
176,0.157088,-8.36567,-2.66603,1,0,-0.00363945,9.1
177,0.10589,-8.39189,-2.66666,1,0,-0.00363945,9.15
178,0.0562363,-8.41729,-2.66728,1,0,-0.00363945,9.2
179,0.0212317,-8.43517,-2.66771,1,0,-0.00363945,9.25
180,-0.029123,-8.46086,-2.66833,1,0,-0.00363945,9.3
181,-0.0632037,-8.47822,-2.66875,1,0,-0.00363945,9.35
182,-0.113359,-8.50374,-2.66937,1,0,-0.00363945,9.4
183,-0.164724,-8.52984,-2.67001,1,0,-0.00363945,9.45
184,-0.198241,-8.54685,-2.67042,1,0,-0.00363945,9.5
185,-0.248973,-8.57255,-2.67105,1,0,-0.00363945,9.55
186,-0.300383,-8.59857,-2.67168,1,0,-0.00363945,9.6
187,-0.334162,-8.61563,-2.6721,1,0,-0.00363945,9.65
188,-0.385639,-8.64161,-2.67274,1,0,-0.00363945,9.7
189,-0.422536,-8.6602,-2.67319,1,0,-0.00363945,9.75
190,-0.473085,-8.68564,-2.67382,1,0,-0.00363945,9.8
191,-0.523299,-8.71087,-2.67444,1,0,-0.00363945,9.85
192,-0.557137,-8.72786,-2.67485,1,0,-0.00363945,9.9
193,-0.606967,-8.75283,-2.67547,1,0,-0.00363945,9.95
194,-0.657448,-8.77809,-2.67609,1,0,-0.00363945,10
195,-0.69154,-8.79513,-2.67651,1,0,-0.00363945,10.05
196,-0.742098,-8.82036,-2.67713,1,0,-0.00363945,10.1
197,-0.775722,-8.83712,-2.67755,1,0,-0.00363945,10.1
198,-0.826956,-8.86263,-2.67818,1,0,-0.00363945,10.2
199,-0.876683,-8.88735,-2.67879,1,0,-0.00363945,10.25
200,-0.925444,-8.91154,-2.67939,1,0,-0.00363945,10.3
201,-0.960421,-8.92888,-2.67982,1,0,-0.00363945,10.35
202,-1.01134,-8.95408,-2.68044,1,0,-0.41,10.4
203,-1.0608,-8.97282,-2.71188,1,0,-0.41,10.45
204,-1.09769,-8.98032,-2.75857,1,0,-0.00363945,10.45
205,-1.15357,-8.99219,-2.8054,1,0,-0.00363945,10.55
206,-1.2053,-9.01008,-2.80605,1,0,-0.00363945,10.6
207,-1.24141,-9.02256,-2.80662,1,0,-0.00363945,10.6
208,-1.29411,-9.04073,-2.80744,1,0,-0.00363945,10.7
209,-1.33541,-9.05498,-2.80787,1,0,-0.00363945,10.7
210,-1.3881,-9.07313,-2.80847,1,0,-0.00363945,10.8
211,-1.44197,-9.09164,-2.80909,1,0,-0.00363945,10.85
212,-1.47666,-9.10354,-2.80949,1,0,-0.00363945,10.85
213,-1.52895,-9.12145,-2.81009,1,0,-0.00363945,10.95
214,-1.58504,-9.14063,-2.81073,1,0,-0.00363945,11
215,-1.62553,-9.15444,-2.8112,1,0,-0.00363945,11.05
216,-1.66857,-9.16911,-2.81169,1,0,-0.00363945,11.05
217,-1.72706,-9.18899,-2.81236,1,0,-0.00363945,11.15
218,-1.76337,-9.20132,-2.81278,1,0,-0.00363945,11.15
219,-1.81602,-9.21916,-2.81338,1,0,-0.00363945,11.25
220,-1.86829,-9.23684,-2.81398,1,0,-0.00363945,11.3
221,-1.90195,-9.2482,-2.81437,1,0,-0.00363945,11.3
222,-1.95579,-9.26635,-2.81499,1,0,-0.00363945,11.4
223,-2.00953,-9.28442,-2.8156,1,0,-0.00363945,11.45
224,-2.04532,-9.29644,-2.81601,1,0,-0.00363945,11.45
225,-2.09927,-9.31453,-2.81663,1,0,-0.00363945,11.55
226,-2.15405,-9.33286,-2.81726,1,0,-0.00363945,11.6
227,-2.18949,-9.34469,-2.81766,1,0,-0.00363945,11.6
228,-2.24413,-9.36291,-2.81829,1,0,-0.00363945,11.7
229,-2.28071,-9.37508,-2.81871,1,0,-0.00363945,11.7
230,-2.33396,-9.39278,-2.81932,1,0,-0.00363945,11.8
231,-2.38756,-9.41055,-2.81993,1,0,-0.00363945,11.85
232,-2.42404,-9.42262,-2.82035,1,0,-0.00363945,11.85
233,-2.47774,-9.44037,-2.82096,1,0,-0.00363945,11.95
234,-2.53141,-9.45806,-2.82158,1,0,-0.00363945,12
235,-2.56669,-9.46968,-2.82198,1,0,-0.00363945,12
236,-2.62075,-9.48744,-2.8226,1,0,-0.00363945,12.1
237,-2.67291,-9.50455,-2.82319,1,0,-0.00363945,12.15
238,-2.70923,-9.51644,-2.82361,1,0,-0.00363945,12.15
239,-2.76324,-9.53409,-2.82423,1,0,-0.00363945,12.25
240,-2.81588,-9.55126,-2.82483,1,0,-0.00363945,12.3
241,-2.85202,-9.56302,-2.82524,1,0,-0.00363945,12.3
242,-2.90541,-9.58037,-2.82585,1,0,-0.00363945,12.4
243,-2.9601,-9.59811,-2.82647,1,0,-0.00363945,12.45
244,-2.99601,-9.60974,-2.82688,1,0,-0.00363945,12.45
245,-3.04967,-9.62708,-2.8275,1,0,-0.00363945,12.55
246,-3.08649,-9.63896,-2.82792,1,0,-0.00363945,12.55
247,-3.1487,-9.65899,-2.82863,1,0,-0.00363945,12.65
248,-3.19542,-9.674,-2.82916,1,0,-0.00363945,12.7
249,-3.23775,-9.68758,-2.82964,1,0,-0.00363945,12.75
250,-3.27808,-9.70049,-2.8301,1,0,-0.00363945,12.75
251,-3.33304,-9.71806,-2.83073,1,0,-0.00363945,12.85
252,-3.38572,-9.73486,-2.83133,1,0,-0.00363945,12.9
253,-3.42155,-9.74627,-2.83174,1,0,-0.00363945,12.9
254,-3.47625,-9.76366,-2.83236,1,0,-0.00363945,13
255,-3.52982,-9.78065,-2.83297,1,0,-0.00363945,13.05
256,-3.56592,-9.79208,-2.83338,1,0,-0.00363945,13.05
257,-3.6198,-9.8091,-2.834,1,0,-0.00363945,13.15
258,-3.67281,-9.82582,-2.8346,1,0,-0.00363945,13.2
259,-3.7089,-9.83718,-2.83501,1,0,-0.00363945,13.2
260,-3.76102,-9.85356,-2.83561,1,0,-0.00363945,13.3
261,-3.81486,-9.87044,-2.83622,1,0,-0.00363945,13.35
262,-3.85155,-9.88193,-2.83664,1,0,-0.00363945,13.35
263,-3.90471,-9.89854,-2.83724,1,0,-0.00363945,13.45
264,-3.95914,-9.91551,-2.83786,1,0,-0.00363945,13.5
265,-3.99484,-9.92662,-2.83827,1,0,-0.00363945,13.5
266,-4.04904,-9.94346,-2.83888,1,0,-0.00363945,13.6
267,-4.10347,-9.96033,-2.8395,1,0,0.0196531,13.65
268,-4.13893,-9.97167,-2.83779,1,0,0.0662384,13.65
269,-4.19375,-9.99054,-2.82904,1,0,0.112823,13.75
270,-4.24547,-10.0107,-2.81164,1,0,0.107,13.8
271,-4.28034,-10.0251,-2.79916,1,0,0.141939,13.8
272,-4.33314,-10.0486,-2.77612,1,0,0.159409,13.9
273,-4.38328,-10.0733,-2.7497,1,0,0.188525,13.95
274,-4.41717,-10.0916,-2.72801,1,0,0.264226,13.95
275,-4.46567,-10.1215,-2.68621,1,0,0.258403,14.05
276,-4.49687,-10.143,-2.65636,1,0,0.264226,14.05
277,-4.54325,-10.1775,-2.6105,1,0,0.182701,14.1
278,-4.57956,-10.2055,-2.58073,1,0,0.293341,14.15
279,-4.62681,-10.246,-2.53268,1,0,0.270049,14.25
280,-4.66073,-10.2788,-2.49343,1,0,0.275872,14.25
281,-4.69104,-10.3102,-2.45717,1,0,-0.41,14.3
282,-4.73262,-10.35,-2.43846,1,0,-0.41,14.35
283,-4.76549,-10.3692,-2.47667,1,0,-0.300038,14.4
284,-4.81641,-10.3944,-2.5352,1,0,-0.00363945,14.45
285,-4.86609,-10.4224,-2.55916,1,0,0.182701,14.5
286,-4.89645,-10.4453,-2.54443,1,0,0.089531,14.55
287,-4.94097,-10.4807,-2.52391,1,0,-0.0379964,14.6
288,-4.97173,-10.5029,-2.52328,1,0,-0.41,14.65
289,-5.02016,-10.5321,-2.5487,1,0,-0.41,14.7
290,-5.07327,-10.5517,-2.61865,1,0,-0.41,14.75
291,-5.11022,-10.5629,-2.6665,1,0,-0.41,14.8
292,-5.16552,-10.5761,-2.73694,1,0,-0.41,14.85
293,-5.22152,-10.5853,-2.8073,1,0,0.0953541,14.9
294,-5.25925,-10.5918,-2.83831,1,0,0.304988,14.95
295,-5.31175,-10.6135,-2.80809,1,0,0.176878,15
296,-5.36238,-10.6387,-2.77238,1,0,0.322457,15.05
297,-5.39444,-10.6571,-2.74073,1,0,0.374866,15.1
298,-5.44144,-10.6899,-2.67856,1,0,0.200171,15.15
299,-5.48669,-10.7221,-2.63619,1,0,0.403981,15.2
300,-5.51743,-10.7473,-2.59566,1,0,0.398158,15.25
301,-5.5577,-10.7873,-2.52686,1,0,0.153586,15.3
302,-5.58497,-10.8142,-2.49509,1,0,0.41,15.35
303,-5.62333,-10.8529,-2.45177,1,0,0.41,15.4
304,-5.65717,-10.8984,-2.38149,1,0,0.41,15.45
305,-5.67821,-10.9305,-2.33383,1,0,0.351573,15.5
306,-5.70783,-10.9797,-2.26937,1,0,0.334104,15.55
307,-5.73484,-11.0292,-2.21193,1,0,0.32828,15.6
308,-5.7542,-11.0689,-2.16814,1,0,0.194348,15.65
309,-5.77436,-11.1103,-2.13425,1,0,0.275872,15.7
310,-5.79736,-11.162,-2.09133,1,0,0.339927,15.8
311,-5.81511,-11.2136,-2.03754,1,0,-0.048478,16.15
312,-5.82899,-11.2506,-2.01749,1,0,-0.100886,16.2
313,-5.85654,-11.3016,-2.0328,1,0,0.41,16.3
314,-5.87328,-11.3355,-2.02973,1,0,0.41,16.35
315,-5.88714,-11.3908,-1.96504,1,0,0.41,16.4
316,-5.89581,-11.4469,-1.89486,1,0,0.41,16.45
317,-5.89941,-11.4853,-1.84692,1,0,0.41,16.5
318,-5.90135,-11.542,-1.77647,1,0,0.41,16.55
319,-5.90042,-11.58,-1.72933,1,0,0.316634,16.6
320,-5.89713,-11.6374,-1.66689,1,0,0.304988,16.65
321,-5.89246,-11.6923,-1.61563,1,0,0.23511,16.7
322,-5.88766,-11.7385,-1.57984,1,0,0.21764,16.75
323,-5.88073,-11.795,-1.54176,1,0,-0.153294,16.85
324,-5.87776,-11.833,-1.53433,1,0,0.0545921,16.9
325,-5.87762,-11.8888,-1.54206,1,0,0.41,16.95
326,-5.86863,-11.9448,-1.49993,1,0,0.41,17
327,-5.8576,-11.9805,-1.45371,1,0,0.41,17.05
328,-5.8372,-12.0349,-1.38178,1,0,0.293341,17.1
329,-5.82305,-12.07,-1.3433,1,0,0.182701,17.15
330,-5.80209,-12.1245,-1.30296,1,0,0.107,17.2
331,-5.7826,-12.1784,-1.28049,1,0,-0.100886,17.25
332,-5.77183,-12.2149,-1.28299,1,0,0.246756,17.3
333,-5.7555,-12.2692,-1.27796,1,0,0.41,17.35
334,-5.7286,-12.3189,-1.21709,1,0,0.41,17.4
335,-5.70745,-12.3509,-1.16947,1,0,0.339927,17.45
336,-5.67443,-12.3974,-1.10702,1,0,0.23511,17.5
337,-5.65236,-12.4283,-1.07588,1,0,0.0778847,17.55
338,-5.62051,-12.4758,-1.04972,1,0,-0.0793405,17.6
339,-5.59297,-12.5246,-1.0535,1,0,0.118647,17.65
340,-5.57366,-12.5576,-1.04925,1,0,0.0545921,17.7
341,-5.54208,-12.6069,-1.03551,1,0,0.380689,17.75
342,-5.50777,-12.6512,-0.996033,1,0,0.41,17.8
343,-5.47963,-12.6783,-0.947846,1,0,0.41,17.85
344,-5.43814,-12.7138,-0.880048,1,0,0.41,17.9
345,-5.3936,-12.7469,-0.811135,1,0,0.41,17.95
346,-5.3609,-12.7683,-0.76258,1,0,0.369042,18
347,-5.31225,-12.7972,-0.697339,1,0,0.287518,18.05
348,-5.27806,-12.8167,-0.659998,1,0,0.264226,18.15
349,-5.22777,-12.8441,-0.612931,1,0,0.136116,18.4
350,-5.18039,-12.8701,-0.583535,1,0,0.357396,18.5
351,-5.14666,-12.8865,-0.554941,1,0,0.252579,18.55
352,-5.09418,-12.9071,-0.503774,1,0,0.270049,18.6
353,-5.04216,-12.926,-0.458956,1,0,0.188525,18.65
354,-5.00584,-12.9385,-0.433696,1,0,0.275872,18.7
355,-4.95139,-12.9552,-0.392562,1,0,-0.048478,18.75
356,-4.8981,-12.9722,-0.372343,1,0,0.363219,18.8
357,-4.86223,-12.9847,-0.362331,1,0,0.363219,18.85
358,-4.80601,-12.9936,-0.300959,1,0,0.304988,18.9
359,-4.75038,-12.9998,-0.246866,1,0,0.252579,19
360,-4.71232,-13.0034,-0.215992,1,0,0.211817,19.05
361,-4.65574,-13.008,-0.177212,1,0,0.176878,19.1
362,-4.61402,-13.0109,-0.153629,1,0,0.165232,19.2
363,-4.55666,-13.0139,-0.124074,1,0,0.141939,19.25
364,-4.50051,-13.016,-0.0989402,1,0,0.118647,19.3
365,-4.46283,-13.0171,-0.0850307,1,0,0.0953541,19.35
366,-4.40748,-13.0184,-0.0677467,1,0,0.0778847,19.4
367,-4.34955,-13.0195,-0.0532612,1,0,0.0662384,19.45
368,-4.312,-13.02,-0.0455846,1,0,0.0545921,19.5
369,-4.25658,-13.0206,-0.0359222,1,0,0.0487688,19.55
370,-4.19914,-13.021,-0.0274194,1,0,0.0545921,19.6
371,-4.16064,-13.021,-0.0212403,1,0,0.0487688,19.65
372,-4.10505,-13.0204,-0.0124869,1,0,0.0371225,19.7
373,-4.06525,-13.02,-0.00752369,1,0,0.0545921,19.75
374,-4.0066,-13.0187,0.00122981,1,0,0.0545921,19.8
375,-3.95067,-13.0169,0.0105726,1,0,0.089531,19.85
376,-3.91298,-13.0147,0.0202106,1,0,0.107,19.9
377,-3.85656,-13.0103,0.0373018,1,0,0.211817,19.95
378,-3.79914,-13.0022,0.0693147,1,0,-0.00363945,20
379,-3.76219,-12.9976,0.0780177,1,0,0.00218368,20.05
380,-3.70475,-12.9931,0.0781125,1,0,0.246756,20.1
381,-3.66777,-12.9883,0.0905698,1,0,0.0604153,20.1
382,-3.61182,-12.9773,0.118844,1,0,0.0196531,20.2
383,-3.55528,-12.9694,0.124115,1,0,0.182119,20.25
384,-3.51765,-12.9624,0.137752,1,0,-0.00363945,20.25
385,-3.46195,-12.9514,0.152473,1,0,-0.00363945,20.35
386,-3.40568,-12.9429,0.15188,1,0,-0.00363945,20.4
387,-3.36341,-12.9365,0.151413,1,0,-0.00363945,20.45
388,-3.30722,-12.928,0.150797,1,0,-0.00363945,20.5
389,-3.26863,-12.9222,0.150482,1,0,-0.00363945,20.5
390,-3.21198,-12.9138,0.149812,1,0,0.334104,20.6
391,-3.15763,-12.9004,0.180031,1,0,0.194348,20.65
392,-3.12061,-12.8883,0.207483,1,0,0.386512,20.65
393,-3.06834,-12.8675,0.259134,1,0,-0.00363945,20.7
394,-3.03324,-12.8521,0.288345,1,0,0.224046,20.75
395,-2.98,-12.8332,0.306031,1,0,0.223464,20.8
396,-2.92802,-12.8088,0.34497,1,0,-0.00363945,20.9
397,-2.88942,-12.7926,0.354866,1,0,0.12447,20.9
398,-2.83703,-12.7705,0.369102,1,0,0.0953541,20.95
399,-2.80253,-12.7548,0.380661,1,0,-0.00363945,21
400,-2.75056,-12.7324,0.387037,1,0,0.360308,21.05
401,-2.69984,-12.7069,0.414236,1,0,-0.00363945,21.15
402,-2.66725,-12.687,0.439393,1,0,-0.00363945,21.15
403,-2.61575,-12.6627,0.439198,1,0,-0.00363945,21.2
404,-2.564,-12.6385,0.438761,1,0,-0.00363945,21.3
405,-2.52924,-12.6223,0.438449,1,0,-0.00363945,21.3
406,-2.47738,-12.5981,0.437772,1,0,-0.00363945,21.35
407,-2.44367,-12.5824,0.437371,1,0,-0.00363945,21.4
408,-2.39138,-12.5581,0.436749,1,0,-0.00363945,21.45
409,-2.3403,-12.5344,0.436142,1,0,-0.00363945,21.5
410,-2.30581,-12.5184,0.435732,1,0,-0.00363945,21.55
411,-2.25358,-12.4942,0.435111,1,0,-0.00363945,21.6
412,-2.20143,-12.4701,0.434492,1,0,-0.00363945,21.7
413,-2.16411,-12.4529,0.434048,1,0,-0.00363945,21.7
414,-2.11386,-12.4297,0.433452,1,0,-0.00363945,21.75
415,-2.07896,-12.4136,0.433037,1,0,-0.00363945,21.8
416,-2.02774,-12.3901,0.432429,1,0,-0.00363945,21.85
417,-1.98959,-12.3726,0.431977,1,0,-0.00363945,21.9
418,-1.93533,-12.3477,0.431333,1,0,-0.00363945,21.95
419,-1.88411,-12.3242,0.430725,1,0,-0.00363945,22
420,-1.84816,-12.3078,0.430299,1,0,-0.00363945,22.05
421,-1.79608,-12.284,0.429682,1,0,-0.00363945,22.1
422,-1.75949,-12.2674,0.429248,1,0,-0.00363945,22.15
423,-1.71795,-12.2484,0.428756,1,0,-0.00363945,22.2
424,-1.65611,-12.2203,0.428023,1,0,-0.306444,22.25
425,-1.61987,-12.2071,0.409303,1,0,-0.00363945,22.3
426,-1.56598,-12.1904,0.381388,1,0,-0.00363945,22.35
427,-1.53039,-12.1761,0.381025,1,0,-0.254035,22.4
428,-1.47622,-12.1579,0.361732,1,0,-0.00363945,22.45
429,-1.41936,-12.141,0.342343,1,0,-0.00363945,22.5
430,-1.37621,-12.1258,0.341889,1,0,-0.00363945,22.55
431,-1.33032,-12.1095,0.341354,1,0,-0.00363945,22.6
432,-1.29265,-12.0963,0.340812,1,0,-0.00363945,22.65
433,-1.23758,-12.0769,0.340175,1,0,-0.00363945,22.7
434,-1.18357,-12.0579,0.339551,1,0,-0.00363945,22.8
435,-1.14758,-12.0452,0.339135,1,0,-0.00363945,22.8
436,-1.09454,-12.0267,0.338522,1,0,-0.00363945,22.85
437,-1.05772,-12.0138,0.338096,1,0,-0.00363945,22.9
438,-1.00386,-11.995,0.337474,1,0,-0.00363945,22.95
439,-0.950099,-11.9762,0.336853,1,0,-0.00363945,23.05
440,-0.913951,-11.9637,0.336436,1,0,-0.00363945,23.05
441,-0.861255,-11.9454,0.335827,1,0,-0.00363945,23.15
442,-0.807018,-11.9266,0.335201,1,0,-0.00363945,23.2
443,-0.771199,-11.9142,0.334788,1,0,-0.00363945,23.2
444,-0.716209,-11.8952,0.334153,1,0,-0.00363945,23.3
445,-0.680773,-11.8829,0.333744,1,0,-0.00363945,23.3
446,-0.626058,-11.8641,0.333113,1,0,-0.00363945,23.35
447,-0.573045,-11.8459,0.332502,1,0,-0.00363945,23.45
448,-0.535141,-11.8329,0.332065,1,0,-0.00363945,23.45
449,-0.477127,-11.813,0.331396,1,0,-0.00363945,23.55
450,-0.441435,-11.8008,0.330984,1,0,-0.00363945,23.55
451,-0.387273,-11.7823,0.33036,1,0,-0.00363945,23.65
452,-0.333942,-11.7642,0.329746,1,0,-0.00363945,23.7
453,-0.297519,-11.7518,0.329326,1,0,-0.00363945,23.7
454,-0.24195,-11.7329,0.328686,1,0,-0.00363945,23.8
455,-0.206335,-11.7209,0.328276,1,0,-0.00363945,23.8
456,-0.152379,-11.7026,0.327655,1,0,-0.00363945,23.85
457,-0.0984965,-11.6844,0.327034,1,0,-0.00363945,23.95
458,-0.0620631,-11.6721,0.326615,1,0,-0.00363945,23.95
459,-0.00516325,-11.653,0.32596,1,0,-0.00363945,24.05
460,0.031133,-11.6408,0.325543,1,0,-0.00363945,24.05
461,0.0857267,-11.6225,0.324915,1,0,0.41,24.1
462,0.137542,-11.5997,0.355498,1,0,0.41,24.2
463,0.169127,-11.5789,0.40226,1,0,0.159409,24.2
464,0.217127,-11.5475,0.451072,1,0,0.0720615,24.25
465,0.251041,-11.5284,0.463319,1,0,-0.00363945,24.3
466,0.301279,-11.502,0.468298,1,0,-0.00363945,24.35
467,0.352597,-11.4762,0.467687,1,0,-0.00363945,24.4
468,0.386591,-11.4591,0.467258,1,0,-0.00363945,24.45
469,0.44201,-11.4313,0.46656,1,0,-0.00363945,24.5
470,0.476111,-11.4142,0.466131,1,0,-0.00363945,24.55
471,0.528637,-11.3879,0.465469,1,0,-0.00363945,24.6
472,0.563255,-11.3706,0.465033,1,0,-0.00363945,24.65
473,0.614196,-11.3452,0.464392,1,0,-0.00363945,24.7
474,0.66519,-11.3198,0.463751,1,0,-0.00363945,24.75
475,0.699051,-11.303,0.463325,1,0,-0.00363945,24.8
476,0.749982,-11.2777,0.462684,1,0,-0.00363945,24.85
477,0.802689,-11.2515,0.462022,1,0,-0.00363945,24.95
478,0.83737,-11.2343,0.461586,1,0,-0.00363945,24.95
479,0.888352,-11.2091,0.460945,1,0,-0.00363945,25
480,0.922418,-11.1923,0.460518,1,0,-0.00363945,25.05
481,0.973248,-11.1672,0.459879,1,0,-0.00363945,25.1
482,1.02523,-11.1416,0.459227,1,0,-0.00363945,25.2
483,1.05862,-11.1252,0.458808,1,0,-0.00363945,25.2
484,1.11047,-11.0997,0.458157,1,0,-0.00363945,25.25
485,1.16155,-11.0747,0.457517,1,0,-0.00363945,25.35
486,1.19569,-11.0579,0.457089,1,0,-0.00363945,25.35
487,1.2464,-11.0331,0.456453,1,0,-0.00363945,25.4
488,1.2818,-11.0158,0.456009,1,0,-0.00363945,25.45
489,1.33594,-10.9894,0.455331,1,0,-0.00363945,25.5
490,1.37017,-10.9728,0.454902,1,0,-0.00363945,25.55
491,1.42198,-10.9476,0.454253,1,0,-0.00363945,25.6
492,1.47345,-10.9226,0.453609,1,0,-0.00363945,25.65
493,1.50687,-10.9064,0.453191,1,0,-0.00363945,25.7
494,1.55869,-10.8813,0.452542,1,0,-0.00363945,25.75
495,1.6114,-10.8558,0.451883,1,0,-0.00363945,25.85
496,1.64639,-10.8389,0.451445,1,0,-0.00363945,25.85
497,1.69795,-10.814,0.450801,1,0,-0.00363945,25.9
498,1.73223,-10.7975,0.450372,1,0,-0.00363945,25.95
499,1.78343,-10.7729,0.449733,1,0,-0.00363945,26
500,1.83481,-10.7483,0.449091,1,0,-0.00363945,26.1
501,1.86939,-10.7317,0.448659,1,0,-0.00363945,26.1
502,1.92141,-10.7068,0.44801,1,0,-0.00363945,26.15
503,1.97061,-10.6833,0.447396,1,0,-0.00363945,26.25
504,2.00486,-10.6669,0.446968,1,0,-0.00363945,26.25
505,2.05673,-10.6422,0.446321,1,0,-0.00363945,26.3
506,2.09136,-10.6257,0.445889,1,0,-0.00363945,26.35
507,2.14304,-10.6011,0.445245,1,0,-0.00363945,26.4
508,2.19509,-10.5764,0.444596,1,0,-0.00363945,26.45
509,2.23066,-10.5596,0.444153,1,0,-0.00363945,26.5
510,2.28231,-10.5352,0.443509,1,0,-0.00363945,26.55
511,2.33288,-10.5113,0.44288,1,0,-0.00363945,26.65
512,2.36962,-10.4939,0.442422,1,0,-0.00363945,26.65
513,2.42228,-10.4691,0.441767,1,0,-0.00363945,26.7
514,2.45702,-10.4528,0.441334,1,0,-0.00363945,26.75
515,2.50844,-10.4286,0.440695,1,0,0.0254762,26.8
516,2.54344,-10.4117,0.442811,1,0,-0.00363945,26.85
517,2.59473,-10.3871,0.444187,1,0,-0.00363945,26.9
518,2.64637,-10.3626,0.443506,1,0,-0.00363945,26.95
519,2.68027,-10.3466,0.442987,1,0,-0.00363945,27
520,2.73303,-10.3217,0.44224,1,0,-0.00363945,27.05
521,2.78481,-10.2974,0.441597,1,0,0.089531,27.1
522,2.81838,-10.28,0.449611,1,0,0.130293,27.15
523,2.86815,-10.2517,0.470073,1,0,0.01383,27.2
524,2.90166,-10.2331,0.476434,1,0,-0.00363945,27.25
525,2.95148,-10.2072,0.47702,1,0,-0.00363945,27.3
526,3.00327,-10.1805,0.476728,1,0,-0.00363945,27.35
527,3.03719,-10.1631,0.47644,1,0,0.161156,27.4
528,3.08736,-10.1341,0.492478,1,0,-0.00363945,27.45
529,3.12036,-10.1143,0.500798,1,0,-0.00363945,27.5
530,3.16967,-10.0875,0.500073,1,0,-0.00363945,27.55
531,3.22049,-10.0599,0.499258,1,0,-0.00363945,27.6
532,3.25342,-10.042,0.49884,1,0,-0.00363945,27.65
533,3.30329,-10.015,0.498222,1,0,-0.00363945,27.7
534,3.35379,-9.98762,0.497687,1,0,-0.00363945,27.75
535,3.38734,-9.96946,0.497369,1,0,-0.00363945,27.8
536,3.43759,-9.94233,0.496713,1,0,0.01383,27.85
537,3.48751,-9.9149,0.498529,1,0,0.01383,27.9
538,3.52094,-9.89635,0.500179,1,0,0.0312994,27.95
539,3.57066,-9.86823,0.504636,1,0,0.0371225,28
540,3.60294,-9.84955,0.508705,1,0,0.0312994,28.05
541,3.65246,-9.82058,0.514719,1,0,0.0429456,28.1
542,3.7031,-9.79033,0.52198,1,0,0.0487688,28.15
543,3.73525,-9.77065,0.527614,1,0,0.0604153,28.2
544,3.78374,-9.74023,0.537281,1,0,0.0487688,28.25
545,3.83068,-9.71026,0.546028,1,0,0.0545921,28.3
546,3.86296,-9.68927,0.552418,1,0,0.0487688,28.35
547,3.90983,-9.6584,0.560994,1,0,0.0545921,28.4
548,3.95526,-9.62784,0.569875,1,0,0.0604153,28.45
549,3.98664,-9.60622,0.576761,1,0,0.0545921,28.5
550,4.03195,-9.57456,0.586155,1,0,0.0604153,28.55
551,4.07589,-9.54315,0.595853,1,0,0.0662384,28.6
552,4.1062,-9.52093,0.60336,1,0,0.0720615,28.65
553,4.15019,-9.48787,0.615088,1,0,0.0662384,28.7
554,4.19524,-9.45325,0.62672,1,0,0.0604153,28.75
555,4.22533,-9.42983,0.633841,1,0,0.0720615,28.8
556,4.26865,-9.39524,0.645394,1,0,0.0662384,28.85
557,4.31224,-9.35959,0.65691,1,0,0.0662384,28.9
558,4.34148,-9.33525,0.664535,1,0,0.0720615,28.95
559,4.38389,-9.29911,0.676459,1,0,0.0778847,29
560,4.42624,-9.26184,0.689658,1,0,0.0778847,29.05
561,4.45494,-9.236,0.698661,1,0,0.0662384,29.1
562,4.49597,-9.19848,0.710587,1,0,0.0662384,29.15
563,4.53688,-9.16035,0.721962,1,0,0.089531,29.2
564,4.56447,-9.13368,0.731807,1,0,0.0837078,29.25
565,4.60311,-9.09528,0.745929,1,0,0.0662384,29.3
566,4.64303,-9.05512,0.758081,1,0,0.0720615,29.35
567,4.68088,-9.01612,0.770005,1,0,0.0662384,29.4
568,4.70759,-8.98822,0.777691,1,0,0.0604153,29.45
569,4.74068,-8.95324,0.7866,1,0,0.0545921,29.5
570,4.77434,-8.91722,0.794995,1,0,0.0662384,29.55
571,4.81744,-8.86977,0.807502,1,0,0.0545921,29.6
572,4.84335,-8.8409,0.814147,1,0,0.0487688,29.65
573,4.88034,-8.7993,0.82258,1,0,0.0545921,29.7
574,4.9058,-8.7702,0.828774,1,0,0.0487688,29.75
575,4.94268,-8.72742,0.837541,1,0,0.0429456,29.8
576,4.97888,-8.68509,0.844894,1,0,0.0487688,29.85
577,5.00325,-8.65614,0.850228,1,0,0.0487688,29.9
578,5.03911,-8.61285,0.858604,1,0,0.0429456,29.95
579,5.0743,-8.56984,0.866047,1,0,0.0487688,30
580,5.10821,-8.52765,0.873884,1,0,0.0429456,30.05
581,5.13204,-8.49772,0.878893,1,0,0.0371225,30.1
582,5.16682,-8.45368,0.885601,1,0,0.0429456,30.15
583,5.20158,-8.4089,0.892912,1,0,0.0604153,30.2
584,5.22356,-8.37969,0.899539,1,0,0.0604153,30.25
585,5.25709,-8.33422,0.909957,1,0,0.0545921,30.3
586,5.29024,-8.28855,0.919477,1,0,0.0662384,30.35
587,5.31246,-8.2571,0.927055,1,0,0.0720615,30.4
588,5.34469,-8.21024,0.939187,1,0,0.0662384,30.45
589,5.36574,-8.17908,0.946713,1,0,0.0662384,30.5
590,5.39709,-8.13176,0.95811,1,0,0.0662384,30.55
591,5.42824,-8.08354,0.969626,1,0,0.0662384,30.6
592,5.44878,-8.05105,0.977339,1,0,0.0604153,30.65
593,5.47871,-8.00292,0.988087,1,0,0.0837078,30.7
594,5.50792,-7.95384,1.00177,1,0,0.0837078,30.8
595,5.52674,-7.92112,1.01134,1,0,0.0953541,30.8
596,5.55373,-7.87238,1.02688,1,0,0.089531,30.85
597,5.58025,-7.8227,1.04231,1,0,0.0837078,30.95
598,5.59759,-7.78949,1.05187,1,0,0.0953541,30.95
599,5.62335,-7.7382,1.06774,1,0,0.0953541,31.05
600,5.64716,-7.68854,1.08363,1,0,0.101177,31.1
601,5.66283,-7.65448,1.09499,1,0,0.112823,31.1
602,5.68552,-7.60248,1.11389,1,0,0.0953541,31.2
603,5.70727,-7.55083,1.13053,1,0,0.101177,31.25
604,5.72123,-7.51629,1.14186,1,0,0.0953541,31.3
605,5.7419,-7.46329,1.15852,1,0,0.089531,31.35
606,5.76098,-7.41248,1.17329,1,0,0.0953541,31.4
607,5.77348,-7.3777,1.18391,1,0,0.089531,31.45
608,5.79154,-7.32546,1.19916,1,0,0.0720615,31.5
609,5.80885,-7.27424,1.21176,1,0,0.0662384,31.55
610,5.82649,-7.2209,1.2232,1,0,0.0837078,31.6
611,5.83713,-7.18681,1.23208,1,0,0.0720615,31.65
612,5.85324,-7.13358,1.24476,1,0,0.0837078,31.7
613,5.86858,-7.07998,1.25843,1,0,0.0720615,31.75
614,5.87881,-7.04319,1.26686,1,0,0.0837078,31.8
615,5.89314,-6.9888,1.28036,1,0,0.0778847,31.85
616,5.90647,-6.93517,1.29367,1,0,0.0662384,31.9
617,5.9156,-6.89758,1.30163,1,0,0.0778847,31.95
618,5.9283,-6.84219,1.31456,1,0,0.0720615,32
619,5.94031,-6.78656,1.32725,1,0,0.0720615,32.05
620,5.9478,-6.75017,1.33536,1,0,0.0662384,32.1
621,5.95889,-6.69414,1.34704,1,0,0.0604153,32.15
622,5.96898,-6.64096,1.35713,1,0,0.0545921,32.2
623,5.97599,-6.60292,1.36366,1,0,0.0487688,32.25
624,5.98601,-6.54685,1.37243,1,0,0.0604153,32.3
625,5.99535,-6.49062,1.3824,1,0,0.0545921,32.35
626,6.00116,-6.45411,1.38848,1,0,0.0429456,32.4
627,6.00988,-6.39811,1.39648,1,0,0.0429456,32.45
628,6.01815,-6.34303,1.40373,1,0,-0.24763,32.5
629,6.02623,-6.3063,1.39116,1,0,0.0778847,32.55
630,6.04015,-6.25194,1.37434,1,0,0.0371225,32.6
631,6.04927,-6.19848,1.38166,1,0,0.089531,32.65
632,6.05455,-6.16304,1.39013,1,0,0.0196531,32.7
633,6.0632,-6.10592,1.39824,1,0,-0.0904046,32.75
634,6.07402,-6.05251,1.38931,1,0,0.12447,32.8
635,6.08064,-6.0159,1.3917,1,0,0.136116,32.85
636,6.08647,-5.95908,1.41443,1,0,0.118647,32.9
637,6.09109,-5.90386,1.43504,1,0,0.118647,32.95
638,6.09465,-5.84972,1.45469,1,0,0.107,33
639,6.09677,-5.81088,1.46751,1,0,0.112823,33.05
640,6.09888,-5.75595,1.48599,1,0,0.089531,33.1
641,6.10049,-5.69949,1.50208,1,0,0.0778847,33.15
642,6.10139,-5.66148,1.51126,1,0,0.0778847,33.2
643,6.10215,-5.60548,1.52448,1,0,0.0662384,33.25
644,6.10246,-5.55105,1.53586,1,0,0.0604153,33.3
645,6.10248,-5.51305,1.54285,1,0,0.0662384,33.35
646,6.10191,-5.45621,1.55397,1,0,0.0545921,33.4
647,6.10101,-5.40195,1.56324,1,0,0.0604153,33.45
648,6.10007,-5.36506,1.56987,1,0,0.0487688,33.5
649,6.09833,-5.30809,1.57896,1,0,0.0545921,33.55
650,6.09613,-5.25193,1.58796,1,0,0.0662384,33.65
651,6.09407,-5.21332,1.5956,1,0,0.0545921,33.7
652,6.09083,-5.15885,1.6052,1,0,0.0429456,33.75
653,6.08734,-5.10269,1.61307,1,0,0.0371225,33.8
654,6.08506,-5.06695,1.61719,1,0,0.0429456,33.85
655,6.08104,-5.00994,1.62426,1,0,0.0429456,33.9
656,6.07679,-4.95593,1.63132,1,0,0.0604153,34
657,6.07155,-4.90055,1.64117,1,0,0.0487688,34.05
658,6.06773,-4.86226,1.64708,1,0,0.0429456,34.05
659,6.06196,-4.80661,1.65479,1,0,0.0487688,34.15
660,6.05574,-4.75129,1.66286,1,0,0.0429456,34.2
661,6.05129,-4.71343,1.66796,1,0,0.0312994,34.2
662,6.04461,-4.65697,1.67382,1,0,0.0371225,34.3
663,6.03789,-4.60296,1.67976,1,0,0.0312994,34.35
664,6.03306,-4.56524,1.68344,1,0,0.0371225,34.35
665,6.02541,-4.50808,1.68958,1,0,0.12447,34.4
666,6.01567,-4.45406,1.70731,1,0,0.41,34.45
667,6.00578,-4.41877,1.73681,1,0,-0.00363945,34.5
668,5.98747,-4.36655,1.78184,1,0,-0.00363945,34.55
669,5.97587,-4.31215,1.78123,1,0,-0.00363945,34.6
670,5.9682,-4.27589,1.78082,1,0,0.118647,34.65
671,5.95466,-4.22155,1.79208,1,0,-0.00363945,34.7
672,5.94116,-4.16774,1.79778,1,0,0.141939,34.75
673,5.93103,-4.13121,1.80799,1,0,-0.00363945,34.8
674,5.91576,-4.07795,1.81826,1,0,-0.00363945,34.85
675,5.90213,-4.02349,1.81764,1,0,0.0144123,34.9
676,5.89265,-3.98679,1.819,1,0,-0.00363945,34.95
677,5.87889,-3.93312,1.81958,1,0,-0.00363945,35
678,5.86523,-3.87896,1.81908,1,0,-0.00363945,35.05
679,5.85195,-3.82599,1.81835,1,0,-0.00363945,35.1
680,5.84181,-3.78556,1.81795,1,0,-0.00363945,35.15
681,5.82762,-3.72883,1.81731,1,0,-0.00363945,35.2
682,5.81828,-3.69138,1.81688,1,0,-0.00363945,35.25
683,5.80501,-3.63805,1.81626,1,0,-0.00363945,35.3
684,5.79176,-3.58466,1.81564,1,0,-0.00363945,35.35
685,5.78249,-3.54723,1.8152,1,0,-0.00363945,35.4
686,5.76862,-3.49111,1.81455,1,0,0.0837078,35.45
687,5.75372,-3.4388,1.82492,1,0,0.41,35.5
688,5.74006,-3.40433,1.85227,1,0,0.240933,35.55
689,5.71345,-3.35195,1.90692,1,0,0.171055,35.6
690,5.69351,-3.31036,1.93338,1,0,0.41,35.65
691,5.66372,-3.26039,1.98839,1,0,0.00218368,35.7
692,5.64472,-3.23118,2.01622,1,0,-0.00363945,35.75
693,5.61991,-3.18005,2.01706,1,0,-0.00363945,35.8
694,5.59586,-3.12951,2.01646,1,0,-0.00363945,35.85
695,5.58009,-3.09632,2.01605,1,0,-0.00363945,35.9
696,5.5558,-3.04514,2.01541,1,0,-0.00363945,35.95
697,5.53179,-2.99445,2.01478,1,0,-0.00363945,36
698,5.51601,-2.9611,2.01437,1,0,-0.00363945,36.05
699,5.49214,-2.9106,2.01374,1,0,-0.00363945,36.1
700,5.46879,-2.86109,2.01313,1,0,-0.00363945,36.15
701,5.45226,-2.826,2.01269,1,0,-0.00363945,36.2
702,5.42876,-2.77605,2.01207,1,0,-0.00363945,36.25
703,5.40453,-2.72446,2.01143,1,0,-0.00363945,36.3
704,5.38148,-2.67531,2.01083,1,0,-0.00363945,36.35
705,5.36542,-2.641,2.0104,1,0,-0.00363945,36.4
706,5.34213,-2.59119,2.00978,1,0,-0.00363945,36.45
707,5.31922,-2.54214,2.00918,1,0,-0.00363945,36.5
708,5.30243,-2.50611,2.00873,1,0,-0.00363945,36.55
709,5.27911,-2.45603,2.00811,1,0,-0.00363945,36.6
710,5.25481,-2.40376,2.00746,1,0,-0.00363945,36.65
711,5.23545,-2.36205,2.00695,1,0,-0.00363945,36.7
712,5.21945,-2.32753,2.00652,1,0,-0.00363945,36.75
713,5.19517,-2.27509,2.00587,1,0,-0.00363945,36.8
714,5.17127,-2.22337,2.00523,1,0,-0.00363945,36.85
715,5.1559,-2.19007,2.00482,1,0,-0.00363945,36.9
716,5.13216,-2.13855,2.00419,1,0,-0.00363945,36.95
717,5.10801,-2.08606,2.00354,1,0,-0.00363945,37
718,5.09289,-2.05315,2.00313,1,0,-0.00363945,37.05
719,5.06918,-2.00146,2.00249,1,0,-0.00363945,37.1
720,5.04531,-1.94935,2.00185,1,0,-0.00363945,37.15
721,5.02875,-1.91315,2.0014,1,0,0.107,37.2
722,5.00375,-1.8636,2.01318,1,0,0.0144123,37.25
723,4.97892,-1.81453,2.01958,1,0,0.265973,37.3
724,4.9594,-1.78138,2.03909,1,0,0.108747,37.35
725,4.92824,-1.73306,2.06804,1,0,0.0487688,37.4
726,4.90913,-1.70082,2.07519,1,0,-0.00363945,37.45
727,4.8808,-1.65092,2.07809,1,0,0.203082,37.5
728,4.85037,-1.60326,2.0982,1,0,-0.00363945,37.55
729,4.82931,-1.57084,2.10647,1,0,-0.00363945,37.6
730,4.79983,-1.52091,2.1058,1,0,-0.00363945,37.65
731,4.77174,-1.47328,2.1052,1,0,-0.00363945,37.7
732,4.75204,-1.43985,2.10478,1,0,-0.00363945,37.75
733,4.7232,-1.39082,2.10417,1,0,-0.00363945,37.8
734,4.70368,-1.35762,2.10375,1,0,-0.00363945,37.85
735,4.67396,-1.30699,2.10312,1,0,-0.00363945,37.9
736,4.64479,-1.25724,2.10249,1,0,-0.00363945,37.95
737,4.626,-1.22515,2.10209,1,0,-0.00363945,38
738,4.59687,-1.17533,2.10147,1,0,-0.00363945,38.05
739,4.57797,-1.14297,2.10106,1,0,-0.00363945,38.1
740,4.54908,-1.09346,2.10044,1,0,-0.00363945,38.15
741,4.52049,-1.04439,2.09983,1,0,-0.00363945,38.2
742,4.50134,-1.01149,2.09942,1,0,-0.00363945,38.25
743,4.47248,-0.961821,2.0988,1,0,-0.00363945,38.3
744,4.44404,-0.912829,2.09818,1,0,-0.00363945,38.35
745,4.42516,-0.880255,2.09778,1,0,-0.00363945,38.4
746,4.39661,-0.830962,2.09716,1,0,-0.00363945,38.45
747,4.36854,-0.782413,2.09655,1,0,-0.00363945,38.5
748,4.3487,-0.748064,2.09612,1,0,-0.00363945,38.55
749,4.32051,-0.699196,2.09551,1,0,-0.00363945,38.6
750,4.30159,-0.66636,2.0951,1,0,-0.00363945,38.65
751,4.27313,-0.6169,2.09449,1,0,-0.00363945,38.7
752,4.24484,-0.567674,2.09387,1,0,-0.00363945,38.75
753,4.22567,-0.53427,2.09346,1,0,-0.00363945,38.8
754,4.19723,-0.484657,2.09284,1,0,0.01383,38.85
755,4.16843,-0.435352,2.09492,1,0,0.0312994,38.9
756,4.1492,-0.403252,2.09823,1,0,0.0312994,38.95
757,4.11918,-0.353849,2.1037,1,0,0.0545921,39
758,4.09935,-0.322195,2.1093,1,0,0.0604153,39.05
759,4.0683,-0.274083,2.11956,1,0,0.0837078,39.1
760,4.03633,-0.226763,2.13312,1,0,0.0837078,39.15
761,4.01466,-0.195756,2.14282,1,0,0.136116,39.2
762,3.98049,-0.149762,2.16312,1,0,0.153586,39.25
763,3.94482,-0.10564,2.1888,1,0,0.171055,39.3
764,3.91985,-0.0766608,2.20817,1,0,0.188525,39.35
765,3.88159,-0.0351276,2.23944,1,0,0.194348,39.4
766,3.84172,0.00495874,2.27263,1,0,0.205994,39.45
767,3.81345,0.0315204,2.29665,1,0,0.182701,39.5
768,3.7715,0.0692239,2.32909,1,0,0.205994,39.55
769,3.74137,0.0945959,2.35314,1,0,0.200171,39.6
770,3.69692,0.129773,2.38768,1,0,0.182701,39.65
771,3.6657,0.153362,2.41003,1,0,0.171055,39.7
772,3.61287,0.19138,2.44514,1,0,-0.178334,39.75
773,3.57898,0.217917,2.4496,1,0,-0.41,39.8
774,3.54914,0.251153,2.41225,1,0,-0.0152857,39.85
775,3.51073,0.299298,2.36431,1,0,-0.236566,39.9
776,3.48535,0.327711,2.34914,1,0,0.188525,39.95
777,3.44878,0.371391,2.32903,1,0,-0.100886,40
778,3.40876,0.411375,2.33431,1,0,-0.41,40.05
779,3.38621,0.441829,2.30597,1,0,-0.143395,40.1
780,3.35601,0.490284,2.25687,1,0,0.258403,40.15
781,3.31991,0.533841,2.26306,1,0,0.118647,40.2
782,3.29278,0.561004,2.28106,1,0,0.0196531,40.25
783,3.2542,0.602785,2.29018,1,0,0.21764,40.3
784,3.2139,0.641736,2.31639,1,0,0.107,40.35
785,3.18528,0.668123,2.33229,1,0,0.0837078,40.4
786,3.14403,0.706984,2.34766,1,0,-0.41,40.45
787,3.11796,0.734288,2.34223,1,0,-0.41,40.5
788,3.08768,0.783458,2.2754,1,0,0.0254762,40.55
789,3.05809,0.831472,2.23576,1,0,0.334104,40.6
790,3.03273,0.859283,2.25347,1,0,0.299165,40.65
791,2.98943,0.896089,2.30689,1,0,0.293341,40.7
792,2.94367,0.931588,2.35866,1,0,0.275872,40.8
793,2.91351,0.953401,2.39007,1,0,0.0312994,40.85
794,2.86429,0.991576,2.41594,1,0,0.41,40.95
795,2.82611,1.01804,2.44881,1,0,0.41,41
796,2.78847,1.03672,2.50094,1,0,0.41,41.05
797,2.73401,1.05962,2.5741,1,0,0.374866,41.15
798,2.69098,1.0752,2.62704,1,0,0.374866,41.2
799,2.64662,1.08881,2.68007,1,0,0.374866,41.25
800,2.58938,1.10276,2.74695,1,0,0.310811,41.3
801,2.55253,1.1106,2.78404,1,0,0.32828,41.35
802,2.49636,1.12013,2.83963,1,0,0.0429456,41.45
803,2.44081,1.13111,2.86744,1,0,0.41,41.5
804,2.40158,1.13769,2.89348,1,0,0.334104,41.55
805,2.34485,1.13971,2.95477,1,0,0.229287,41.65
806,2.30691,1.14102,2.98502,1,0,-0.0589597,41.75
807,2.24564,1.1462,3.00398,1,0,0.41,41.8
808,2.20728,1.15052,3.01172,1,0,0.41,41.85
809,2.14555,1.14445,3.08502,1,0,0.293341,41.95
810,2.10617,1.13905,3.12517,1,0,0.258403,42
811,2.0501,1.13091,-3.11174,1,0,0.182701,42.05
812,2.01022,1.12487,-3.08635,1,0,0.293341,42.1
813,1.95446,1.1139,-3.04446,1,0,0.089531,42.15
814,1.89914,1.10234,-3.01522,1,0,0.0953541,42.2
815,1.86167,1.09559,-3.00449,1,0,0.316634,42.25
816,1.80284,1.08054,-2.96667,1,0,0.240933,42.3
817,1.76703,1.06847,-2.93664,1,0,-0.0327554,42.35
818,1.71229,1.05235,-2.91599,1,0,-0.362928,42.4
819,1.65635,1.04551,-2.95003,1,0,-0.0211091,42.45
820,1.61609,1.04335,-2.97712,1,0,0.240933,42.45
821,1.56107,1.03092,-2.95701,1,0,0.293341,42.55
822,1.52492,1.01826,-2.92512,1,0,0.240933,42.55
823,1.47211,0.997571,-2.88006,1,0,0.34575,42.65
824,1.42046,0.973206,-2.82631,1,0,0.264226,42.7
825,1.38647,0.955514,-2.79302,1,0,-0.205703,42.8
826,1.33481,0.931204,-2.77023,1,0,-0.174258,42.9
827,1.27998,0.916038,-2.80039,1,0,0.41,42.95
828,1.2438,0.904197,-2.80285,1,0,0.41,43
829,1.19203,0.872841,-2.73675,1,0,0.41,43.1
830,1.16098,0.849981,-2.68901,1,0,0.41,43.1
831,1.11667,0.813066,-2.6175,1,0,0.41,43.2
832,1.0765,0.774471,-2.5483,1,0,0.41,43.25
833,1.04827,0.743865,-2.49659,1,0,0.380689,43.3
834,1.01226,0.70076,-2.43057,1,0,0.223464,43.35
835,0.987483,0.671115,-2.39791,1,0,0.101177,43.4
836,0.946588,0.625781,-2.37058,1,0,-0.079923,43.45
837,0.91896,0.598847,-2.37188,1,0,0.293341,43.5
838,0.878398,0.558328,-2.36351,1,0,0.41,43.55
839,0.84745,0.510533,-2.29828,1,0,0.41,43.65
840,0.829244,0.476976,-2.25087,1,0,0.304988,43.7
841,0.803188,0.426408,-2.19248,1,0,0.264226,43.75
842,0.786381,0.392961,-2.16126,1,0,0.357396,43.8
843,0.763299,0.339397,-2.10533,1,0,0.386512,43.9
844,0.745817,0.2851,-2.03975,1,0,0.386512,44
845,0.736306,0.248133,-1.99507,1,0,0.165232,44.05
846,0.72198,0.192933,-1.94789,1,0,0.165232,44.1
847,0.706448,0.139009,-1.91997,1,0,0.304988,44.15
848,0.698716,0.101566,-1.8888,1,0,-0.0170327,44.2
849,0.686282,0.0430405,-1.85958,1,0,0.322457,44.3
850,0.677688,0.00641193,-1.84518,1,0,0.293341,44.35
851,0.672189,-0.0514411,-1.79186,1,0,0.246756,44.4
852,0.668326,-0.107791,-1.74806,1,0,0.211817,44.45
853,0.666189,-0.146479,-1.72286,1,0,0.182701,44.5
854,0.664048,-0.203251,-1.68982,1,0,-0.00655102,44.6
855,0.661557,-0.240839,-1.68077,1,0,0.165232,44.6
856,0.65758,-0.299025,-1.66616,1,0,0.147763,44.7
857,0.657269,-0.356159,-1.64002,1,0,0.136116,44.75
858,0.657639,-0.396166,-1.62326,1,0,0.118647,44.8
859,0.658866,-0.453284,-1.6018,1,0,0.107,44.85
860,0.660035,-0.491925,-1.58918,1,0,0.112823,44.85
861,0.662769,-0.550096,-1.56961,1,0,0.0953541,44.9
862,0.666216,-0.6066,-1.55243,1,0,0.0778847,45
863,0.668765,-0.646326,-1.54279,1,0,0.0778847,45
864,0.672924,-0.70248,-1.5295,1,0,0.0662384,45.05
865,0.675945,-0.740321,-1.52176,1,0,0.0545921,45.1
866,0.680748,-0.797328,-1.51162,1,0,0.0604153,45.15
867,0.686192,-0.854667,-1.50108,1,0,0.0487688,45.2
868,0.689872,-0.891837,-1.49533,1,0,0.0545921,45.25
================================================
FILE: include/LearningMPC/CSVReader.h
================================================
//
// Created by yuwei on 11/11/19.
//
#include <iostream>
#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>
class CSVReader
{
std::string fileName;
std::string delimeter;
public:
CSVReader(std::string filename, std::string delm = ",") :
fileName(filename), delimeter(delm)
{ }
std::vector<std::vector<std::string> > getData();
};
std::vector<std::vector<std::string> > CSVReader::getData() {
std::ifstream file(fileName);
std::vector<std::vector<std::string> > dataList;
std::string line = "";
while (getline(file, line)) {
std::vector<std::string> vec;
boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
dataList.push_back(vec);
}
file.close();
return dataList;
}
================================================
FILE: include/LearningMPC/car_params.h
================================================
struct CarParams {
double wheelbase;
double friction_coeff;
double h_cg; // height of car's CG
double l_f; // length from CG to front axle
double l_r; // length from CG to rear axle
double cs_f; // cornering stiffness coeff for front wheels
double cs_r; // cornering stiffness coeff for rear wheels
double mass;
double I_z; // moment of inertia about z axis from CG
};
================================================
FILE: include/LearningMPC/occupancy_grid.h
================================================
//
// Author: Yuwei Wang
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/Marker.h>
#include <math.h>
#define THRESHOLD 50
using namespace std;
namespace occupancy_grid{
int xy_ind2ind(const nav_msgs::OccupancyGrid& grid, int x_ind, int y_ind){
return max(0, min(y_ind * int(grid.info.width) + x_ind, int(grid.data.size())-1));
}
int xy2ind(const nav_msgs::OccupancyGrid& grid, float x, float y){
int x_ind = static_cast<int>(ceil((x-grid.info.origin.position.x)/grid.info.resolution))-1;
int y_ind = static_cast<int>(ceil((y-grid.info.origin.position.y)/grid.info.resolution))-1;
return xy_ind2ind(grid, x_ind, y_ind);
}
struct Pair{
int x_ind;
int y_ind;
};
Pair ind2xy_ind(const nav_msgs::OccupancyGrid& grid, int ind){
int y_ind = ind/grid.info.width;
int x_ind = ind - y_ind*grid.info.width;
Pair res;
res.x_ind = x_ind;
res.y_ind = y_ind;
return res;
}
float ind2x(const nav_msgs::OccupancyGrid& grid, int ind){
return grid.info.origin.position.x + ind2xy_ind(grid, ind).x_ind * grid.info.resolution;
}
float ind2y(const nav_msgs::OccupancyGrid& grid, int ind){
return grid.info.origin.position.y + ind2xy_ind(grid, ind).y_ind * grid.info.resolution;
}
bool is_xy_occupied(nav_msgs::OccupancyGrid& grid, float x, float y){
return int(grid.data.at(xy2ind(grid, x, y))) > THRESHOLD;
}
void set_xy_occupied(nav_msgs::OccupancyGrid& grid, float x, float y){
grid.data.at(xy2ind(grid, x, y)) = 100;
}
void inflate_cell(nav_msgs::OccupancyGrid &grid, int i, float margin, int val) {
int margin_cells = static_cast<int>(ceil(margin/grid.info.resolution));
Pair res = ind2xy_ind(grid, i);
for (int x = max(0, res.x_ind-margin_cells); x<min(int(grid.info.width-1), res.x_ind+margin_cells); x++){
for (int y = max(0, res.y_ind-margin_cells); y<min(int(grid.info.height-1), res.y_ind+margin_cells); y++){
grid.data.at(xy_ind2ind(grid,x,y)) = val;
}
}
}
void inflate_map(nav_msgs::OccupancyGrid& grid, float margin){
vector<int> occupied_ind;
occupied_ind.clear();
for (int i=0; i<grid.data.size(); i++){
if (grid.data.at(i)>THRESHOLD){
occupied_ind.push_back(i);
}
}
for (int i=0; i<occupied_ind.size(); i++){
inflate_cell(grid, occupied_ind[i], margin, 100);
}
}
}
================================================
FILE: include/LearningMPC/spline.h
================================================
//
// Created by yuwei on 11/19/19.
//
#ifndef SRC_SPLINE_H
#define SRC_SPLINE_H
#endif //SRC_SPLINE_H
/*
* spline.h
*
* simple cubic spline interpolation library without external
* dependencies
*
* ---------------------------------------------------------------------
* Copyright (C) 2011, 2014 Tino Kluge (ttk448 at gmail.com)
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
* ---------------------------------------------------------------------
*
*/
#ifndef TK_SPLINE_H
#define TK_SPLINE_H
#include <cstdio>
#include <cassert>
#include <vector>
#include <algorithm>
// unnamed namespace only because the implementation is in this
// header file and we don't want to export symbols to the obj files
namespace
{
namespace tk
{
// band matrix solver
class band_matrix
{
private:
std::vector< std::vector<double> > m_upper; // upper band
std::vector< std::vector<double> > m_lower; // lower band
public:
band_matrix() {}; // constructor
band_matrix(int dim, int n_u, int n_l); // constructor
~band_matrix() {}; // destructor
void resize(int dim, int n_u, int n_l); // init with dim,n_u,n_l
int dim() const; // matrix dimension
int num_upper() const
{
return m_upper.size()-1;
}
int num_lower() const
{
return m_lower.size()-1;
}
// access operator
double & operator () (int i, int j); // write
double operator () (int i, int j) const; // read
// we can store an additional diogonal (in m_lower)
double& saved_diag(int i);
double saved_diag(int i) const;
void lu_decompose();
std::vector<double> r_solve(const std::vector<double>& b) const;
std::vector<double> l_solve(const std::vector<double>& b) const;
std::vector<double> lu_solve(const std::vector<double>& b,
bool is_lu_decomposed=false);
};
// spline interpolation
class spline
{
public:
enum bd_type {
first_deriv = 1,
second_deriv = 2
};
private:
std::vector<double> m_x,m_y; // x,y coordinates of points
// interpolation parameters
// f(x) = a*(x-x_i)^3 + b*(x-x_i)^2 + c*(x-x_i) + y_i
std::vector<double> m_a,m_b,m_c; // spline coefficients
double m_b0, m_c0; // for left extrapol
bd_type m_left, m_right;
double m_left_value, m_right_value;
bool m_force_linear_extrapolation;
public:
// set default boundary condition to be zero curvature at both ends
spline(): m_left(second_deriv), m_right(second_deriv),
m_left_value(0.0), m_right_value(0.0),
m_force_linear_extrapolation(false)
{
;
}
// optional, but if called it has to come be before set_points()
void set_boundary(bd_type left, double left_value,
bd_type right, double right_value,
bool force_linear_extrapolation=false);
void set_points(const std::vector<double>& x,
const std::vector<double>& y, bool cubic_spline=true);
double operator() (double x) const;
double eval_d(double x);
double eval_dd(double x);
};
// ---------------------------------------------------------------------
// implementation part, which could be separated into a cpp file
// ---------------------------------------------------------------------
// band_matrix implementation
// -------------------------
band_matrix::band_matrix(int dim, int n_u, int n_l)
{
resize(dim, n_u, n_l);
}
void band_matrix::resize(int dim, int n_u, int n_l)
{
assert(dim>0);
assert(n_u>=0);
assert(n_l>=0);
m_upper.resize(n_u+1);
m_lower.resize(n_l+1);
for(size_t i=0; i<m_upper.size(); i++) {
m_upper[i].resize(dim);
}
for(size_t i=0; i<m_lower.size(); i++) {
m_lower[i].resize(dim);
}
}
int band_matrix::dim() const
{
if(m_upper.size()>0) {
return m_upper[0].size();
} else {
return 0;
}
}
// defines the new operator (), so that we can access the elements
// by A(i,j), index going from i=0,...,dim()-1
double & band_matrix::operator () (int i, int j)
{
int k=j-i; // what band is the entry
assert( (i>=0) && (i<dim()) && (j>=0) && (j<dim()) );
assert( (-num_lower()<=k) && (k<=num_upper()) );
// k=0 -> diogonal, k<0 lower left part, k>0 upper right part
if(k>=0) return m_upper[k][i];
else return m_lower[-k][i];
}
double band_matrix::operator () (int i, int j) const
{
int k=j-i; // what band is the entry
assert( (i>=0) && (i<dim()) && (j>=0) && (j<dim()) );
assert( (-num_lower()<=k) && (k<=num_upper()) );
// k=0 -> diogonal, k<0 lower left part, k>0 upper right part
if(k>=0) return m_upper[k][i];
else return m_lower[-k][i];
}
// second diag (used in LU decomposition), saved in m_lower
double band_matrix::saved_diag(int i) const
{
assert( (i>=0) && (i<dim()) );
return m_lower[0][i];
}
double & band_matrix::saved_diag(int i)
{
assert( (i>=0) && (i<dim()) );
return m_lower[0][i];
}
// LR-Decomposition of a band matrix
void band_matrix::lu_decompose()
{
int i_max,j_max;
int j_min;
double x;
// preconditioning
// normalize column i so that a_ii=1
for(int i=0; i<this->dim(); i++) {
assert(this->operator()(i,i)!=0.0);
this->saved_diag(i)=1.0/this->operator()(i,i);
j_min=std::max(0,i-this->num_lower());
j_max=std::min(this->dim()-1,i+this->num_upper());
for(int j=j_min; j<=j_max; j++) {
this->operator()(i,j) *= this->saved_diag(i);
}
this->operator()(i,i)=1.0; // prevents rounding errors
}
// Gauss LR-Decomposition
for(int k=0; k<this->dim(); k++) {
i_max=std::min(this->dim()-1,k+this->num_lower()); // num_lower not a mistake!
for(int i=k+1; i<=i_max; i++) {
assert(this->operator()(k,k)!=0.0);
x=-this->operator()(i,k)/this->operator()(k,k);
this->operator()(i,k)=-x; // assembly part of L
j_max=std::min(this->dim()-1,k+this->num_upper());
for(int j=k+1; j<=j_max; j++) {
// assembly part of R
this->operator()(i,j)=this->operator()(i,j)+x*this->operator()(k,j);
}
}
}
}
// solves Ly=b
std::vector<double> band_matrix::l_solve(const std::vector<double>& b) const
{
assert( this->dim()==(int)b.size() );
std::vector<double> x(this->dim());
int j_start;
double sum;
for(int i=0; i<this->dim(); i++) {
sum=0;
j_start=std::max(0,i-this->num_lower());
for(int j=j_start; j<i; j++) sum += this->operator()(i,j)*x[j];
x[i]=(b[i]*this->saved_diag(i)) - sum;
}
return x;
}
// solves Rx=y
std::vector<double> band_matrix::r_solve(const std::vector<double>& b) const
{
assert( this->dim()==(int)b.size() );
std::vector<double> x(this->dim());
int j_stop;
double sum;
for(int i=this->dim()-1; i>=0; i--) {
sum=0;
j_stop=std::min(this->dim()-1,i+this->num_upper());
for(int j=i+1; j<=j_stop; j++) sum += this->operator()(i,j)*x[j];
x[i]=( b[i] - sum ) / this->operator()(i,i);
}
return x;
}
std::vector<double> band_matrix::lu_solve(const std::vector<double>& b,
bool is_lu_decomposed)
{
assert( this->dim()==(int)b.size() );
std::vector<double> x,y;
if(is_lu_decomposed==false) {
this->lu_decompose();
}
y=this->l_solve(b);
x=this->r_solve(y);
return x;
}
// spline implementation
// -----------------------
void spline::set_boundary(spline::bd_type left, double left_value,
spline::bd_type right, double right_value,
bool force_linear_extrapolation)
{
assert(m_x.size()==0); // set_points() must not have happened yet
m_left=left;
m_right=right;
m_left_value=left_value;
m_right_value=right_value;
m_force_linear_extrapolation=force_linear_extrapolation;
}
void spline::set_points(const std::vector<double>& x,
const std::vector<double>& y, bool cubic_spline)
{
assert(x.size()==y.size());
assert(x.size()>2);
m_x=x;
m_y=y;
int n=x.size();
// TODO: maybe sort x and y, rather than returning an error
for(int i=0; i<n-1; i++) {
assert(m_x[i]<m_x[i+1]);
}
if(cubic_spline==true) { // cubic spline interpolation
// setting up the matrix and right hand side of the equation system
// for the parameters b[]
band_matrix A(n,1,1);
std::vector<double> rhs(n);
for(int i=1; i<n-1; i++) {
A(i,i-1)=1.0/3.0*(x[i]-x[i-1]);
A(i,i)=2.0/3.0*(x[i+1]-x[i-1]);
A(i,i+1)=1.0/3.0*(x[i+1]-x[i]);
rhs[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
}
// boundary conditions
if(m_left == spline::second_deriv) {
// 2*b[0] = f''
A(0,0)=2.0;
A(0,1)=0.0;
rhs[0]=m_left_value;
} else if(m_left == spline::first_deriv) {
// c[0] = f', needs to be re-expressed in terms of b:
// (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f')
A(0,0)=2.0*(x[1]-x[0]);
A(0,1)=1.0*(x[1]-x[0]);
rhs[0]=3.0*((y[1]-y[0])/(x[1]-x[0])-m_left_value);
} else {
assert(false);
}
if(m_right == spline::second_deriv) {
// 2*b[n-1] = f''
A(n-1,n-1)=2.0;
A(n-1,n-2)=0.0;
rhs[n-1]=m_right_value;
} else if(m_right == spline::first_deriv) {
// c[n-1] = f', needs to be re-expressed in terms of b:
// (b[n-2]+2b[n-1])(x[n-1]-x[n-2])
// = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]))
A(n-1,n-1)=2.0*(x[n-1]-x[n-2]);
A(n-1,n-2)=1.0*(x[n-1]-x[n-2]);
rhs[n-1]=3.0*(m_right_value-(y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
} else {
assert(false);
}
// solve the equation system to obtain the parameters b[]
m_b=A.lu_solve(rhs);
// calculate parameters a[] and c[] based on b[]
m_a.resize(n);
m_c.resize(n);
for(int i=0; i<n-1; i++) {
m_a[i]=1.0/3.0*(m_b[i+1]-m_b[i])/(x[i+1]-x[i]);
m_c[i]=(y[i+1]-y[i])/(x[i+1]-x[i])
- 1.0/3.0*(2.0*m_b[i]+m_b[i+1])*(x[i+1]-x[i]);
}
} else { // linear interpolation
m_a.resize(n);
m_b.resize(n);
m_c.resize(n);
for(int i=0; i<n-1; i++) {
m_a[i]=0.0;
m_b[i]=0.0;
m_c[i]=(m_y[i+1]-m_y[i])/(m_x[i+1]-m_x[i]);
}
}
// for left extrapolation coefficients
m_b0 = (m_force_linear_extrapolation==false) ? m_b[0] : 0.0;
m_c0 = m_c[0];
// for the right extrapolation coefficients
// f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1}
double h=x[n-1]-x[n-2];
// m_b[n-1] is determined by the boundary condition
m_a[n-1]=0.0;
m_c[n-1]=3.0*m_a[n-2]*h*h+2.0*m_b[n-2]*h+m_c[n-2]; // = f'_{n-2}(x_{n-1})
if(m_force_linear_extrapolation==true)
m_b[n-1]=0.0;
}
double spline::operator() (double x) const
{
size_t n=m_x.size();
// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
std::vector<double>::const_iterator it;
it=std::lower_bound(m_x.begin(),m_x.end(),x);
int idx=std::max( int(it-m_x.begin())-1, 0);
double h=x-m_x[idx];
double interpol;
if(x<m_x[0]) {
// extrapolation to the left
interpol=(m_b0*h + m_c0)*h + m_y[0];
} else if(x>m_x[n-1]) {
// extrapolation to the right
interpol=(m_b[n-1]*h + m_c[n-1])*h + m_y[n-1];
} else {
// interpolation
interpol=((m_a[idx]*h + m_b[idx])*h + m_c[idx])*h + m_y[idx];
}
return interpol;
}
double spline::eval_d(double x){
size_t n=m_x.size();
// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
std::vector<double>::const_iterator it;
it=std::lower_bound(m_x.begin(),m_x.end(),x);
int idx=std::max( int(it-m_x.begin())-1, 0);
double h=x-m_x[idx];
double interpol;
if(x<m_x[0]) {
// extrapolation to the left
interpol=2*m_b0*h + m_c0;
} else if(x>m_x[n-1]) {
// extrapolation to the right
interpol=2*m_b[n-1]*h + m_c[n-1];
} else {
// interpolation
interpol=3*m_a[idx]* h*h + 2*m_b[idx]*h + m_c[idx];
}
return interpol;
}
double spline::eval_dd(double x){
size_t n=m_x.size();
// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
std::vector<double>::const_iterator it;
it=std::lower_bound(m_x.begin(),m_x.end(),x);
int idx=std::max( int(it-m_x.begin())-1, 0);
double h=x-m_x[idx];
double interpol;
if(x<m_x[0]) {
// extrapolation to the left
interpol=2*m_b0;
} else if(x>m_x[n-1]) {
// extrapolation to the right
interpol=2*m_b[n-1];
} else {
// interpolation
interpol=6*m_a[idx]* h + 2*m_b[idx];
}
return interpol;
}
} // namespace tk
} // namespace
#endif /* TK_SPLINE_H */
================================================
FILE: include/LearningMPC/track.h
================================================
//
// Created by yuwei on 11/20/19.
//
#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_listener.h>
// standard
#include <math.h>
#include <vector>
#include <array>
#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <algorithm>
#include <random>
#include <LearningMPC/spline.h>
#include <LearningMPC/CSVReader.h>
#include <Eigen/Dense>
#include <nav_msgs/OccupancyGrid.h>
#include <LearningMPC/occupancy_grid.h>
const int SEARCH_RANGE = 10;
const double HALF_WIDTH_MAX = 0.8;
using namespace std;
typedef struct Point_ref{
double x;
double y;
double theta; // theta represents progress along centerline_points
double left_half_width;
double right_half_width;
}Point_ref;
class Track{
public:
vector<Point_ref> centerline_points;
tk::spline X_spline;
tk::spline Y_spline;
double length;
double space;
nav_msgs::OccupancyGrid map;
vector<double> width_info;
Track(string file_name, nav_msgs::OccupancyGrid& map, bool sparse=false) : space(0.05), map(map){
space = 0.05;
centerline_points.clear();
vector<geometry_msgs::Point> waypoints;
CSVReader reader(file_name);
// Get the data from CSV File
std::vector<std::vector<std::string> > dataList = reader.getData();
// Print the content of row by row on screen
for(std::vector<std::string> vec : dataList){
geometry_msgs::Point wp;
wp.x = std::stof(vec.at(0));
wp.y = std::stof(vec.at(1));
waypoints.push_back(wp);
}
/*** process raw waypoints data, extract equally spaced points ***/
int curr = 0;
int next =1;
Point_ref p_start;
p_start.x = waypoints.at(0).x; p_start.y = waypoints.at(0).y; p_start.theta = 0.0;
centerline_points.push_back(p_start);
float theta = 0.0;
while(next < waypoints.size()){
float dist = sqrt(pow(waypoints.at(next).x-waypoints.at(curr).x, 2)
+pow(waypoints.at(next).y-waypoints.at(curr).y, 2));
float dist_to_start = sqrt(pow(waypoints.at(next).x-waypoints.at(0).x, 2)
+pow(waypoints.at(next).y-waypoints.at(0).y, 2));
if (dist>space){
theta += dist;
Point_ref p;
p.x = waypoints.at(next).x; p.y = waypoints.at(next).y; p.theta = theta;
p.left_half_width = p.right_half_width = HALF_WIDTH_MAX;
centerline_points.push_back(p);
curr = next;
}
next++;
// terminate when finished a lap
if (next > waypoints.size()/2 && dist_to_start<space){
break;
}
}
double last_space = sqrt(pow(centerline_points.back().x-waypoints.at(0).x, 2)
+pow(centerline_points.back().y-waypoints.at(0).y, 2));
length = theta + last_space;
Point_ref p_last, p_second_last;
p_last.x = waypoints.at(0).x; p_last.y = waypoints.at(0).y; p_last.theta = length;
p_second_last.x = 0.5*(centerline_points.back().x + p_last.x);
p_second_last.y = 0.5*(centerline_points.back().y + p_last.y);
p_second_last.theta = length - 0.5*last_space;
if(last_space > space) {
centerline_points.push_back(p_second_last);
}
centerline_points.push_back(p_last); //close the loop
vector<double> X;
vector<double> Y;
vector<double> thetas;
for (int i=0; i<centerline_points.size(); i++){
X.push_back(centerline_points.at(i).x);
Y.push_back(centerline_points.at(i).y);
thetas.push_back(centerline_points.at(i).theta);
}
X_spline.set_points(thetas, X);
Y_spline.set_points(thetas, Y);
/** if sparse, reinitialize centerline points such that they are densely and equally spaced **/
if (sparse) {
double s=0;
centerline_points.clear();
while (s<length){
Point_ref p;
p.x = X_spline(s);
p.y = Y_spline(s);
p.theta = s;
s += space;
centerline_points.push_back(p);
}
}
initialize_width();
}
void initialize_width(){
using namespace Eigen;
int t=0;
Vector2d p_right, p_left;
for (int i=0; i<centerline_points.size(); i++){
double dx_dtheta = x_eval_d(centerline_points[i].theta);
double dy_dtheta = y_eval_d(centerline_points[i].theta);
Vector2d p(centerline_points[i].x, centerline_points[i].y);
//search right until hit right track boundary
while(true){
double x = (p + t*map.info.resolution*Vector2d(dy_dtheta, -dx_dtheta).normalized())(0);
double y = (p + t*map.info.resolution*Vector2d(dy_dtheta, -dx_dtheta).normalized())(1);
if(occupancy_grid::is_xy_occupied(map, x, y)){
p_right(0) = x;
p_right(1) = y;
break;
}
t++;
}
t=0;
//search left until hit right track boundary
while(true){
double x = (p + t*map.info.resolution*Vector2d(-dy_dtheta, dx_dtheta).normalized())(0);
double y = (p + t*map.info.resolution*Vector2d(-dy_dtheta, dx_dtheta).normalized())(1);
if(occupancy_grid::is_xy_occupied(map, x, y)){
p_left(0) = x;
p_left(1) = y;
break;
}
t++;
}
centerline_points[i].left_half_width = (p_left-p).norm();
centerline_points[i].right_half_width = (p_right-p).norm();
}
}
double findTheta(double x, double y, double theta_guess, bool global_search= false){
/* return: projected theta along centerline_points, theta is between [0, length]
* */
// wrapTheta(theta_guess);
// int start, end;
// if(global_search){
// start = 0;
// end = centerline_points.size();
// }
// else{
// int mid = int(floor(theta_guess/space));
// start = mid - SEARCH_RANGE;
// end = mid + SEARCH_RANGE;
// }
// int min_ind, second_min_ind;
// double min_dist2 = 10000000.0;
// for (int i=start; i<end; i++){
// if (i>centerline_points.size()-1){ i=0;}
// if (i<0){ i=centerline_points.size()-1;}
// double dist2 = pow(x-centerline_points.at(i).x, 2) + pow(y-centerline_points.at(i).y, 2);
// if( dist2 < min_dist2){
// min_dist2 = dist2;
// min_ind = i;
// }
// }
// if (sqrt(min_dist2)>HALF_WIDTH_MAX && !global_search){
// return findTheta(x, y, theta_guess, true);
// }
// Eigen::Vector2d p, p0, p1;
// int min_ind_prev = min_ind-1;
// int min_ind_next = min_ind+1;
// if (min_ind_next>centerline_points.size()-1){ min_ind_next -= centerline_points.size();}
// if (min_ind_prev<0){ min_ind_prev += centerline_points.size();}
//
// //closest line segment: either [min_ind ,min_ind+1] or [min_ind,min_ind-1]
// if (pow(x-centerline_points.at(min_ind_next).x, 2) + pow(y-centerline_points.at(min_ind_next).y, 2) <
// pow(x-centerline_points.at(min_ind_prev).x, 2) + pow(y-centerline_points.at(min_ind_prev).y, 2)){
// second_min_ind = min_ind_next;
// }
// else{
// second_min_ind = min_ind_prev;
// }
//
// p(0) = x; p(1) = y;
// p0(0) = centerline_points.at(min_ind).x; p0(1) = centerline_points.at(min_ind).y;
// p1(0) = centerline_points.at(second_min_ind).x; p1(1) = centerline_points.at(second_min_ind).y;
//
// double projection = abs((p - p0).dot(p1 - p0)/(p1 - p0).norm());
// double theta;
//
// if (min_ind > second_min_ind){
// theta = centerline_points.at(min_ind).theta - projection;
// }
// else {
// if (min_ind == 0 && second_min_ind == centerline_points.size()-1) {
// theta = length - projection;
// } else {
// theta = centerline_points.at(min_ind).theta + projection;
// }
// }
// if (theta>length){ theta -= length;}
// if (theta<0){ theta += length;}
int min_ind;
double min_dist2 = 10000000.0;
for (int i=0; i<centerline_points.size(); i++){
double dist2 = pow(x-centerline_points.at(i).x, 2) + pow(y-centerline_points.at(i).y, 2);
if( dist2 < min_dist2){
min_dist2 = dist2;
min_ind = i;
}
}
return min_ind*space;
}
void wrapTheta(double& theta){
while(theta > length){ theta -= length;}
while(theta < 0){theta += length; }
}
double x_eval(double theta){
wrapTheta(theta);
return X_spline(theta);
}
double y_eval(double theta){
wrapTheta(theta);
return Y_spline(theta);
}
double x_eval_d(double theta){
wrapTheta(theta);
return X_spline.eval_d(theta);
}
double y_eval_d(double theta){
wrapTheta(theta);
return Y_spline.eval_d(theta);
}
double x_eval_dd(double theta){
wrapTheta(theta);
return X_spline.eval_dd(theta);
}
double y_eval_dd(double theta){
wrapTheta(theta);
return Y_spline.eval_dd(theta);
}
double getPhi(double theta){
wrapTheta(theta);
double dx_dtheta = X_spline.eval_d(theta);
double dy_dtheta = Y_spline.eval_d(theta);
return atan2(dy_dtheta, dx_dtheta);
}
double getLeftHalfWidth(double theta){
// wrapTheta(theta);
int ind = static_cast<int>(floor(theta/space));
ind = max(0, min(int(centerline_points.size()-1), ind));
return centerline_points.at(ind).left_half_width;
}
double getRightHalfWidth(double theta){
// wrapTheta(theta);
int ind = static_cast<int>(floor(theta/space));
ind = max(0, min(int(centerline_points.size()-1), ind));
return centerline_points.at(ind).right_half_width;
}
void setHalfWidth(double theta, double left_val, double right_val){
// wrapTheta(theta);
int ind = static_cast<int>(floor(theta/space));
ind = max(0, min(int(centerline_points.size()-1), ind));
centerline_points.at(ind).left_half_width = left_val;
centerline_points.at(ind).right_half_width = right_val;
}
double getcenterline_pointsCurvature(double theta){
return (X_spline.eval_d(theta)*Y_spline.eval_dd(theta) - Y_spline.eval_d(theta)*X_spline.eval_dd(theta))/
pow((pow(X_spline.eval_d(theta),2) + pow(Y_spline.eval_d(theta),2)), 1.5);
}
double getcenterline_pointsRadius(double theta){
return 1.0/(getcenterline_pointsCurvature(theta));
}
};
================================================
FILE: launch/lmpc.launch
================================================
<?xml version="1.0"?>
<launch>
<!-- Load all parameters -->
<rosparam command="load" file="$(find LearningMPC)/Lmpc_params.yaml"/>
<param name="waypoint_file" value ="$(find LearningMPC)/data/centerline_waypoints.csv"/>
<param name="init_data_file" value ="$(find LearningMPC)/data/initial_safe_set.csv"/>
<!-- Launch the learning MPC -->
<node pkg="LearningMPC" name="LMPC" type="LMPC" output="screen">
</node>
</launch>
================================================
FILE: package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>LearningMPC</name>
<version>0.0.0</version>
<description>The LearningMPC package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="yuwei@todo.todo">yuwei</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/LearningMPC</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
FILE: src/LMPC.cpp
================================================
#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_listener.h>
#include <math.h>
#include <vector>
#include <iostream>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <random>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Odometry.h>
#include <LearningMPC/track.h>
#include <Eigen/Sparse>
#include "OsqpEigen/OsqpEigen.h"
#include <unsupported/Eigen/MatrixFunctions>
#include <LearningMPC/car_params.h>
const int nx = 6;
const int nu = 2;
const int RRT_INTERVAL = 0.1;
using namespace std;
using namespace Eigen;
struct Sample{
Matrix<double,nx,1> x;
Matrix<double,nu,1> u;
double s;
int time;
int iter;
int cost;
};
enum rviz_id{
CENTERLINE,
CENTERLINE_POINTS,
CENTERLINE_SPLINE,
PREDICTION,
BORDERLINES,
SAFE_SET,
TERMINAL_CANDIDATE,
DEBUG
};
class LMPC{
public:
LMPC(ros::NodeHandle& nh);
void run();
private:
ros::NodeHandle nh_;
ros::Publisher track_viz_pub_;
ros::Publisher LMPC_viz_pub_;
ros::Publisher drive_pub_;
ros::Publisher debugger_pub_;
ros::Subscriber odom_sub_;
ros::Subscriber rrt_sub_;
ros::Subscriber map_sub_;
/*Paramaters*/
CarParams car;
string pose_topic;
string drive_topic;
string waypoint_file;
string init_data_file;
double WAYPOINT_SPACE;
double Ts;
int N;
int K_NEAR;
double SPEED_MAX;
double STEER_MAX;
double ACCELERATION_MAX;
double DECELERATION_MAX;
double MAP_MARGIN;
double VEL_THRESHOLD;
// MPC params
double q_s;
double q_s_terminal;
double r_accel;
double r_steer;
Matrix<double, nu, nu> R;
Track* track_;
//odometry
tf::Transform tf_;
tf::Vector3 car_pos_;
double yaw_;
double vel_;
double yawdot_;
double slip_angle_;
double s_prev_;
double s_curr_;
// use dynamic model or not
bool use_dyn_;
//Sample Safe set
vector<vector<Sample>> SS_;
vector<Sample> curr_trajectory_;
int iter_;
int time_;
Matrix<double,nx,1> terminal_state_pred_;
// map info
nav_msgs::OccupancyGrid map_;
nav_msgs::OccupancyGrid map_updated_;
vector<geometry_msgs::Point> rrt_path_;
VectorXd QPSolution_;
bool first_run_;
vector<geometry_msgs::Point> border_lines_;
void getParameters(ros::NodeHandle& nh);
void init_occupancy_grid();
void init_SS_from_data(const string data_file);
void map_callback(const nav_msgs::OccupancyGrid::Ptr &map_msg);
void rrt_path_callback(const visualization_msgs::Marker::ConstPtr &path_msg);
void visualize_centerline();
int reset_QPSolution(int iter);
void odom_callback(const nav_msgs::Odometry::ConstPtr &odom_msg);
void add_point();
void solve_MPC(const Matrix<double,nx,1>& terminal_candidate);
void get_linearized_dynamics(Matrix<double,nx,nx>& Ad, Matrix<double,nx, nu>& Bd, Matrix<double,nx,1>& hd,
Matrix<double,nx,1>& x_op, Matrix<double,nu,1>& u_op, bool use_dyn);
Vector3d global_to_track(double x, double y, double yaw, double s);
Vector3d track_to_global(double e_y, double e_yaw, double s);
void applyControl();
void visualize_mpc_solution(const vector<Sample>& convex_safe_set, const Matrix<double,nx,1>& terminal_candidate);
Matrix<double,nx,1> select_terminal_candidate();
void select_convex_safe_set(vector<Sample>& convex_safe_set, int iter_start, int iter_end, double s);
int find_nearest_point(vector<Sample>& trajectory, double s);
void update_cost_to_go(vector<Sample>& trajectory);
};
LMPC::LMPC(ros::NodeHandle &nh): nh_(nh){
getParameters(nh_);
init_occupancy_grid();
track_ = new Track(waypoint_file, map_, true);
odom_sub_ = nh_.subscribe(pose_topic, 10, &LMPC::odom_callback, this);
drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(drive_topic, 1);
rrt_sub_ = nh_.subscribe("path_found", 1, &LMPC::rrt_path_callback, this);
map_sub_ = nh_.subscribe("map_updated", 1, &LMPC::map_callback, this);
track_viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("track_centerline", 1);
LMPC_viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("LMPC", 1);
debugger_pub_ = nh_.advertise<visualization_msgs::Marker>("Debugger", 1);
nav_msgs::Odometry odom_msg;
boost::shared_ptr<nav_msgs::Odometry const> odom_ptr;
odom_ptr = ros::topic::waitForMessage<nav_msgs::Odometry>("odom", ros::Duration(5));
if (odom_ptr == nullptr){cout<< "fail to receive odom message!"<<endl;}
else{
odom_msg = *odom_ptr;
}
float x = odom_msg.pose.pose.position.x;
float y = odom_msg.pose.pose.position.y;
s_prev_ = track_->findTheta(x, y,0,true);
car_pos_ = tf::Vector3(x, y, 0.0);
yaw_ = tf::getYaw(odom_msg.pose.pose.orientation);
vel_ = odom_msg.twist.twist.linear.x;
yawdot_ = 0;
slip_angle_ = 0;
iter_ = 2;
use_dyn_ = false;
init_SS_from_data(init_data_file);
}
void LMPC::getParameters(ros::NodeHandle &nh) {
nh.getParam("pose_topic", pose_topic);
nh.getParam("drive_topic", drive_topic);
nh.getParam("waypoint_file", waypoint_file);
nh.getParam("init_data_file", init_data_file);
nh.getParam("N",N);
nh.getParam("Ts",Ts);
nh.getParam("K_NEAR", K_NEAR);
nh.getParam("ACCELERATION_MAX", ACCELERATION_MAX);
nh.getParam("DECELERATION_MAX", DECELERATION_MAX);
nh.getParam("SPEED_MAX", SPEED_MAX);
nh.getParam("STEER_MAX", STEER_MAX);
nh.getParam("VEL_THRESHOLD", VEL_THRESHOLD);
nh.getParam("WAYPOINT_SPACE", WAYPOINT_SPACE);
nh.getParam("r_accel",r_accel);
nh.getParam("r_steer",r_steer);
nh.getParam("q_s",q_s);
nh.getParam("q_s_terminal", q_s_terminal);
R.setZero();
R.diagonal() << r_accel, r_steer;
nh.getParam("MAP_MARGIN",MAP_MARGIN);
nh.getParam("wheelbase", car.wheelbase);
nh.getParam("friction_coeff", car.friction_coeff);
nh.getParam("height_cg", car.h_cg);
nh.getParam("l_cg2rear", car.l_r);
nh.getParam("l_cg2front", car.l_f);
nh.getParam("C_S_front", car.cs_f);
nh.getParam("C_S_rear", car.cs_r);
nh.getParam("moment_inertia", car.I_z);
nh.getParam("mass", car.mass);
}
int compare_s(Sample& s1, Sample& s2){
return (s1.s< s2.s);
}
void LMPC::init_occupancy_grid(){
boost::shared_ptr<nav_msgs::OccupancyGrid const> map_ptr;
map_ptr = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("map", ros::Duration(5.0));
if (map_ptr == nullptr){ROS_INFO("No map received");}
else{
map_ = *map_ptr;
map_updated_ = map_;
ROS_INFO("Map received");
}
ROS_INFO("Initializing occupancy grid for map ...");
occupancy_grid::inflate_map(map_, MAP_MARGIN);
}
void LMPC::init_SS_from_data(string data_file) {
CSVReader reader(data_file);
// Get the data from CSV File
std::vector<std::vector<std::string>> dataList = reader.getData();
SS_.clear();
// Print the content of row by row on screen
int time_prev=0;
int it =0;
vector<Sample> traj;
for(std::vector<std::string> vec : dataList){
Sample sample;
sample.time = std::stof(vec.at(0));
// check if it's a new lap
if (sample.time - time_prev < 0) {
it++;
update_cost_to_go(traj);
SS_.push_back(traj);
traj.clear();
}
sample.x(0) = std::stof(vec.at(1));
sample.x(1) = std::stof(vec.at(2));
sample.x(2) = std::stof(vec.at(3));
sample.x(3) = std::stof(vec.at(4));
sample.x(4) = 0;
sample.x(5) = 0;
sample.u(0) = std::stof(vec.at(5));
sample.u(1) = std::stof(vec.at(6));
sample.s = std::stof(vec.at(7));
sample.iter = it;
traj.push_back(sample);
time_prev = sample.time;
}
update_cost_to_go(traj);
SS_.push_back(traj);
}
void LMPC::map_callback(const nav_msgs::OccupancyGrid::Ptr &map_msg){
visualization_msgs::Marker dots;
dots.header.frame_id = "map";
dots.id = rviz_id::DEBUG;
dots.ns = "debug_points";
dots.type = visualization_msgs::Marker::POINTS;
dots.scale.x = dots.scale.y = 0.1;
dots.scale.z = 0.1;
dots.action = visualization_msgs::Marker::ADD;
dots.pose.orientation.w = 1.0;
dots.color.b = 1.0;
dots.color.g = 0.5;
dots.color.a = 1.0;
vector<geometry_msgs::Point> path_processed;
if(rrt_path_.empty()) return;
for (int i=0; i< rrt_path_.size()-1; i++){
path_processed.push_back(rrt_path_[i]);
double dist = sqrt(pow(rrt_path_[i+1].x-rrt_path_[i].x, 2)
+pow(rrt_path_[i+1].y-rrt_path_[i].y, 2));
if (dist < RRT_INTERVAL) continue;
int num = static_cast<int>(ceil(dist/RRT_INTERVAL));
for(int j=1; j< num; j++){
geometry_msgs::Point p;
p.x = rrt_path_[i].x + j*((rrt_path_[i+1].x - rrt_path_[i].x)/num);
p.y = rrt_path_[i].y + j*((rrt_path_[i+1].y - rrt_path_[i].y)/num);
path_processed.push_back(p);
}
}
for (int i=0; i<path_processed.size(); i++){
double theta = track_->findTheta(path_processed[i].x, path_processed[i].y, 0, true);
Vector2d p_path(path_processed[i].x,path_processed[i].y);
Vector2d p_proj(track_->x_eval(theta), track_->y_eval(theta));
Vector2d p1, p2;
int t=0;
// search one direction until hit obstacle
while(true){
double x = (p_path + t*map_msg->info.resolution*(p_path-p_proj).normalized())(0);
double y = (p_path + t*map_msg->info.resolution*(p_path-p_proj).normalized())(1);
if(occupancy_grid::is_xy_occupied(*map_msg, x, y)){
p1(0) = x; p1(1) = y;
geometry_msgs::Point point;
point.x = x; point.y = y;
dots.points.push_back(point);
break;
}
t++;
}
t=0;
//search the other direction until hit obstacle
while(true){
double x = (p_path - t*map_msg->info.resolution*(p_path-p_proj).normalized())(0);
double y = (p_path - t*map_msg->info.resolution*(p_path-p_proj).normalized())(1);
if(occupancy_grid::is_xy_occupied(*map_msg, x, y)){
p2(0) = x; p2(1) = y;
geometry_msgs::Point point;
point.x = x; point.y = y;
dots.points.push_back(point);
break;
}
t++;
}
double dx_dtheta = track_->x_eval_d(theta);
double dy_dtheta = track_->y_eval_d(theta);
double right_width = Vector2d(dy_dtheta, -dx_dtheta).dot(p1-p_proj)>0 ? (p1-p_proj).norm() : -(p1-p_proj).norm();
double left_width = Vector2d(-dy_dtheta, dx_dtheta).dot(p2-p_proj)>0 ? (p2-p_proj).norm() : -(p2-p_proj).norm();
// right_width = -0.15;
// left_width = 0.4;
cout<<"p1: "<< p1<<endl;
cout<<"p2: "<< p2<<endl;
track_->setHalfWidth(theta, left_width, right_width);
}
debugger_pub_.publish(dots);
}
void LMPC:: rrt_path_callback(const visualization_msgs::Marker::ConstPtr &path_msg){
rrt_path_ = path_msg->points;
}
void LMPC::odom_callback(const nav_msgs::Odometry::ConstPtr &odom_msg){
//visualize_centerline();
/** process pose info **/
float x = odom_msg->pose.pose.position.x;
float y = odom_msg->pose.pose.position.y;
s_curr_ = track_->findTheta(x, y,0,true);
car_pos_ = tf::Vector3(x, y, 0.0);
yaw_ = tf::getYaw(odom_msg->pose.pose.orientation);
vel_ = sqrt(pow(odom_msg->twist.twist.linear.x,2) + pow(odom_msg->twist.twist.linear.y,2));
yawdot_ = odom_msg->twist.twist.angular.z;
slip_angle_ = atan2(odom_msg->twist.twist.linear.y, odom_msg->twist.twist.linear.x);
/** STATE MACHINE: check if dynamic model should be used based on current speed **/
if ((!use_dyn_) && (vel_ > VEL_THRESHOLD)){
use_dyn_ = true;
}
if(use_dyn_ && (vel_< VEL_THRESHOLD*0.5)){
use_dyn_ = false;
}
if (vel_ > 4.5) {
R(0,0) = 1.3 * r_accel;
R(1,1) = 1.8 * r_steer;
}
}
void LMPC::run(){
if (first_run_){
// initialize QPSolution_ from initial Sample Safe Set (using the 2nd iteration)
reset_QPSolution(1);
}
/******** LMPC MAIN LOOP starts ********/
/***check if it is new lap***/
if (s_curr_ - s_prev_ < -track_->length/2){
iter_++;
update_cost_to_go(curr_trajectory_);
//sort(curr_trajectory_.begin(), curr_trajectory_.end(), compare_s);
SS_.push_back(curr_trajectory_);
curr_trajectory_.clear();
// reset_QPSolution(iter_-1);
time_ = 0;
}
/*** select terminal state candidate and its convex safe set ***/
Matrix<double,nx,1> terminal_candidate = select_terminal_candidate();
/** solve MPC and record current state***/
for (int i=0; i<1; i++){
solve_MPC(terminal_candidate);
}
applyControl();
add_point();
/*** store info and advance to next time step***/
terminal_state_pred_ = QPSolution_.segment<nx>(N*nx);
s_prev_ = s_curr_;
time_++;
first_run_ = false;
}
void LMPC::visualize_centerline(){
visualization_msgs::Marker spline_dots;
spline_dots.header.stamp = ros::Time::now();
spline_dots.header.frame_id = "map";
spline_dots.id = rviz_id::CENTERLINE_SPLINE;
spline_dots.ns = "centerline";
spline_dots.type = visualization_msgs::Marker::LINE_STRIP;
spline_dots.scale.x = spline_dots.scale.y = 0.02;
spline_dots.scale.z = 0.02;
spline_dots.action = visualization_msgs::Marker::ADD;
spline_dots.pose.orientation.w = 1.0;
spline_dots.color.b = 1.0;
spline_dots.color.a = 1.0;
// spline_dots.lifetime = ros::Duration();
for (float t=0.0; t<track_->length; t+=0.05){
geometry_msgs::Point p;
p.x = track_->x_eval(t);
p.y = track_->y_eval(t);
spline_dots.points.push_back(p);
}
visualization_msgs::MarkerArray markers;
markers.markers.push_back(spline_dots);
track_viz_pub_.publish(markers);
}
int LMPC::reset_QPSolution(int iter){
QPSolution_ = VectorXd::Zero((N+1)*nx+ N*nu + nx*(N+1) + (2*K_NEAR+1));
for (int i=0; i<N+1; i++){
QPSolution_.segment<nx>(i*nx) = SS_[iter][i].x;
if (i<N) QPSolution_.segment<nu>((N+1)*nx + i*nu) = SS_[iter][i].u;
}
}
Matrix<double,nx,1> LMPC::select_terminal_candidate(){
if (first_run_){
return SS_.back()[N].x;
}
else{
return terminal_state_pred_;
}
}
void LMPC::add_point(){
Sample point;
point.x << car_pos_.x(), car_pos_.y(), yaw_, vel_, yawdot_, slip_angle_;
point.s = s_curr_;
point.iter = iter_;
point.time = time_;
point.u = QPSolution_.segment<nu>((N+1)*nx);
curr_trajectory_.push_back(point);
}
void LMPC::select_convex_safe_set(vector<Sample>& convex_safe_set, int iter_start, int iter_end, double s){
for (int it = iter_start; it<= iter_end; it++){
int nearest_ind = find_nearest_point(SS_[it], s);
int start_ind, end_ind;
int lap_cost = SS_[it][0].cost;
if (K_NEAR%2 != 0 ) {
start_ind = nearest_ind - (K_NEAR-1)/2;
end_ind = nearest_ind + (K_NEAR-1)/2;
}
else{
start_ind = nearest_ind - K_NEAR/2 + 1;
end_ind = nearest_ind + K_NEAR/2;
}
vector<Sample> curr_set;
if (end_ind > SS_[it].size()-1){ // front portion of set crossed finishing line
for (int ind=start_ind; ind<SS_[it].size(); ind++){
curr_set.push_back(SS_[it][ind]);
// modify the cost-to-go for each point before finishing line
// to incentivize the car to cross finishing line towards a new lap
curr_set[curr_set.size()-1].cost += lap_cost;
}
for (int ind=0; ind<end_ind-SS_[it].size()+1; ind ++){
curr_set.push_back(SS_[it][ind]);
}
if (curr_set.size()!=K_NEAR) throw; // for debug
}
else if (start_ind < 0){ // set crossed finishing line
for (int ind=start_ind+SS_[it].size(); ind<SS_[it].size(); ind++){
// modify the cost-to-go, same
curr_set.push_back(SS_[it][ind]);
curr_set[curr_set.size()-1].cost += lap_cost;
}
for (int ind=0; ind<end_ind+1; ind ++){
curr_set.push_back(SS_[it][ind]);
}
if (curr_set.size()!=K_NEAR) throw; // for debug
}
else { // no overlapping with finishing line
for (int ind=start_ind; ind<=end_ind; ind++){
curr_set.push_back(SS_[it][ind]);
}
}
convex_safe_set.insert(convex_safe_set.end(), curr_set.begin(), curr_set.end());
}
}
int LMPC::find_nearest_point(vector<Sample>& trajectory, double s){
// binary search to find closest point to a given s
int low = 0; int high = trajectory.size()-1;
while (low<=high){
int mid = (low + high)/2;
if (s == trajectory[mid].s) return mid;
if (s < trajectory[mid].s) high = mid-1;
else low = mid+1;
}
return abs(trajectory[low].s-s) < (abs(trajectory[high].s-s))? low : high;
}
void LMPC::update_cost_to_go(vector<Sample>& trajectory){
trajectory[trajectory.size()-1].cost = 0;
for (int i=trajectory.size()-2; i>=0; i--){
trajectory[i].cost = trajectory[i+1].cost + 1;
}
}
Vector3d LMPC::global_to_track(double x, double y, double yaw, double s){
double x_proj = track_->x_eval(s);
double y_proj = track_->y_eval(s);
double e_y = sqrt((x-x_proj)*(x-x_proj) + (y-y_proj)*(y-y_proj));
double dx_ds = track_->x_eval_d(s);
double dy_ds = track_->y_eval_d(s);
e_y = dx_ds*(y-y_proj) - dy_ds*(x-x_proj) >0 ? e_y : -e_y;
double e_yaw = yaw - atan2(dy_ds, dx_ds);
while(e_yaw > M_PI) e_yaw -= 2*M_PI;
while(e_yaw < -M_PI) e_yaw += 2*M_PI;
return Vector3d(e_y, e_yaw, s);
}
Vector3d LMPC::track_to_global(double e_y, double e_yaw, double s){
double dx_ds = track_->x_eval_d(s);
double dy_ds = track_->y_eval_d(s);
Vector2d proj(track_->x_eval(s), track_->y_eval(s));
Vector2d pos = proj + Vector2d(-dy_ds, dx_ds).normalized()*e_y;
double yaw = e_yaw + atan2(dy_ds, dx_ds);
return Vector3d(pos(0), pos(1), yaw);
}
void LMPC::get_linearized_dynamics(Matrix<double,nx,nx>& Ad, Matrix<double,nx, nu>& Bd, Matrix<double,nx,1>& hd,
Matrix<double,nx,1>& x_op, Matrix<double,nu,1>& u_op, bool use_dyn){
double yaw = x_op(2);
double v = x_op(3);
double accel = u_op(0);
double steer = u_op(1);
double yaw_dot = x_op(4);
double slip_angle = x_op(5);
VectorXd dynamics(6), h(6);
Matrix<double, nx, nx> A, M12;
Matrix<double, nx, nu> B;
if (!use_dyn) {
// Kinematic Model
dynamics(0) = v * cos(yaw);
dynamics(1) = v * sin(yaw);
dynamics(2) = v * tan(steer)/car.wheelbase;
dynamics(3) = accel;
dynamics(4) = 0;
dynamics(5) = 0;
A << 0.0, 0.0, -v*sin(yaw), cos(yaw), 0.0, 0.0,
0.0, 0.0, v*cos(yaw), sin(yaw), 0.0, 0.0,
0.0, 0.0, 0.0, tan(steer)/car.wheelbase, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
B << 0.0, 0.0,
0.0, 0.0,
0.0, v / (cos(steer) * cos(steer) * car.wheelbase),
1.0, 0.0,
0.0, 0.0,
0.0, 0.0;
}
else{
// Single Track Dynamic Model
double g = 9.81;
double rear_val = g * car.l_r - accel * car.h_cg;
double front_val = g * car.l_f + accel * car.h_cg;
dynamics(0) = v * cos(yaw+slip_angle);
dynamics(1) = v * sin(yaw+slip_angle);
dynamics(2) = yaw_dot;
dynamics(3) = accel;
dynamics(4) = (car.friction_coeff * car.mass / (car.I_z * car.wheelbase)) *
(car.l_f * car.cs_f * steer * (rear_val) +
slip_angle * (car.l_r * car.cs_r * (front_val) - car.l_f * car.cs_f * (rear_val)) -
(yaw_dot/v) * (pow(car.l_f, 2) * car.cs_f * (rear_val) + pow(car.l_r, 2) * car.cs_r * (front_val))); // yaw_dot dynamics
dynamics(5) = (car.friction_coeff / (v * (car.l_r + car.l_f))) *
(car.cs_f * steer * rear_val - slip_angle * (car.cs_r * front_val + car.cs_f * rear_val) +
(yaw_dot/v) * (car.cs_r * car.l_r * front_val - car.cs_f * car.l_f * rear_val)) - yaw_dot; // slip_angle dynamics
double dfyawdot_dv, dfyawdot_dyawdot, dfyawdot_dslip, dfslip_dv, dfslip_dyawdot, dfslip_dslip;
double dfyawdot_da, dfyawdot_dsteer, dfslip_da, dfslip_dsteer;
dfyawdot_dv = (car.friction_coeff * car.mass / (car.I_z * car.wheelbase))
* (pow(car.l_f, 2) * car.cs_f * (rear_val) + pow(car.l_r, 2) * car.cs_r * (front_val))
* yaw_dot / pow(v, 2);
dfyawdot_dyawdot = -(car.friction_coeff * car.mass / (car.I_z * car.wheelbase))
* (pow(car.l_f, 2) * car.cs_f * (rear_val) + pow(car.l_r, 2) * car.cs_r * (front_val))/v;
dfyawdot_dslip = (car.friction_coeff * car.mass / (car.I_z * car.wheelbase))
* (car.l_r * car.cs_r * (front_val) - car.l_f * car.cs_f * (rear_val));
dfslip_dv = -(car.friction_coeff / (car.l_r + car.l_f)) *
(car.cs_f * steer * rear_val - slip_angle * (car.cs_r * front_val + car.cs_f * rear_val))/pow(v,2)
-2*(car.friction_coeff / (car.l_r + car.l_f)) * (car.cs_r * car.l_r * front_val - car.cs_f * car.l_f * rear_val) * yaw_dot/pow(v,3);
dfslip_dyawdot = (car.friction_coeff / (pow(v,2) * (car.l_r + car.l_f))) * (car.cs_r * car.l_r * front_val - car.cs_f * car.l_f * rear_val) - 1;
dfslip_dslip = -(car.friction_coeff / (v * (car.l_r + car.l_f)))*(car.cs_r * front_val + car.cs_f * rear_val);
dfyawdot_da = (car.friction_coeff * car.mass / (car.I_z * car.wheelbase))
*(-car.l_f*car.cs_f*car.h_cg*steer + car.l_r*car.cs_r*car.h_cg*slip_angle + car.l_f*car.cs_f*car.h_cg*slip_angle
- (yaw_dot/v)*(-pow(car.l_f,2)*car.cs_f*car.h_cg) + pow(car.l_r,2)*car.cs_r*car.h_cg);
dfyawdot_dsteer = (car.friction_coeff * car.mass / (car.I_z * car.wheelbase)) *
(car.l_f * car.cs_f * rear_val);
dfslip_da = (car.friction_coeff / (v * (car.l_r + car.l_f))) *
(-car.cs_f*car.h_cg*steer - (car.cs_r*car.h_cg - car.cs_f*car.h_cg)*slip_angle +
(car.cs_r*car.h_cg*car.l_r + car.cs_f*car.h_cg*car.l_f)*(yaw_dot/v));
dfslip_dsteer = (car.friction_coeff / (v * (car.l_r + car.l_f))) *
(car.cs_f * rear_val);
A << 0.0, 0.0, -v*sin(yaw+slip_angle), cos(yaw+slip_angle), 0.0, -v*sin(yaw+slip_angle),
0.0, 0.0, v*cos(yaw+slip_angle), sin(yaw+slip_angle), 0.0, v*cos(yaw+slip_angle),
0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, dfyawdot_dv, dfyawdot_dyawdot, dfyawdot_dslip,
0.0, 0.0, 0.0, dfslip_dv, dfslip_dyawdot, dfslip_dslip;
B << 0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
1.0, 0.0,
dfyawdot_da, dfyawdot_dsteer,
dfslip_da, dfslip_dsteer;
}
/** Discretize using Zero-Order Hold **/
Matrix<double,nx+nx,nx+nx> aux, M;
aux.setZero();
aux.block<nx,nx>(0,0) << A;
aux.block<nx,nx>(0, nx) << Matrix<double,nx,nx>::Identity();
M = (aux*Ts).exp();
M12 = M.block<nx,nx>(0,nx);
h = dynamics - (A*x_op + B*u_op);
Ad = (A*Ts).exp();
Bd = M12*B;
hd = M12*h;
}
void wrap_angle(double& angle, const double angle_ref){
while(angle - angle_ref > M_PI) {angle -= 2*M_PI;}
while(angle - angle_ref < -M_PI) {angle += 2*M_PI;}
}
void LMPC::solve_MPC(const Matrix<double,nx,1>& terminal_candidate){
vector<Sample> terminal_CSS;
double s_t = track_->findTheta(terminal_candidate(0), terminal_candidate(1), 0, true);
select_convex_safe_set(terminal_CSS, iter_-2, iter_-1, s_t);
/** MPC variables: z = [x0, ..., xN, u0, ..., uN-1, s0, ..., sN, lambda0, ....., lambda(2*K_NEAR), s_t1, s_t2, .. s_t6]*
* constraints: dynamics, track bounds, input limits, acceleration limit, slack, lambdas, terminal state, sum of lambda's*/
SparseMatrix<double> HessianMatrix((N+1)*nx+ N*nu + (N+1) + (2*K_NEAR) +nx, (N+1)*nx+ N*nu + (N+1)+ (2*K_NEAR) +nx);
SparseMatrix<double> constraintMatrix((N+1)*nx+ 2*(N+1) + N*nu + (N+1) + (N+1) + (2*K_NEAR) + 2*nx+1, (N+1)*nx+ N*nu + (N+1)+ (2*K_NEAR) +nx);
VectorXd gradient((N+1)*nx+ N*nu + (N+1) + (2*K_NEAR) +nx);
VectorXd lower((N+1)*nx+ 2*(N+1) + N*nu + (N+1) + (N+1) + (2*K_NEAR) + 2*nx+1);
VectorXd upper((N+1)*nx+ 2*(N+1) + N*nu + (N+1) + (N+1) + (2*K_NEAR) + 2*nx+1);
gradient.setZero();
lower.setZero(); upper.setZero();
Matrix<double,nx,1> x_k_ref;
Matrix<double,nu,1> u_k_ref;
Matrix<double,nx,nx> Ad;
Matrix<double,nx,nu> Bd;
Matrix<double,nx,1> x0, hd;
border_lines_.clear();
if (use_dyn_) x0 <<car_pos_.x(), car_pos_.y(), yaw_, vel_, yawdot_, slip_angle_;
else{ x0 <<car_pos_.x(), car_pos_.y(), yaw_, vel_, 0.0, 0.0; }
/** make sure there are no discontinuities in yaw**/
// first check terminal safe_set
for (int i=0; i<terminal_CSS.size(); i++){
wrap_angle(terminal_CSS[i].x(2), x0(2));
}
// also check for previous QPSolution
for (int i=0; i<N+1; i++){
wrap_angle(QPSolution_(i*nx+2), x0(2));
}
for (int i=0; i<N+1; i++){ //0 to N
x_k_ref = QPSolution_.segment<nx>(i*nx);
u_k_ref = QPSolution_.segment<nu>((N+1)*nx + i*nu);
double s_ref = track_->findTheta(x_k_ref(0), x_k_ref(1), 0, true);
get_linearized_dynamics(Ad, Bd, hd, x_k_ref, u_k_ref, use_dyn_);
/* form Hessian entries*/
// cost does not depend on x0, only 1 to N
if (i>0) {
HessianMatrix.insert((N+1)*nx + N*nu + i, (N+1)*nx + N*nu + i) = q_s;
}
if (i<N){
for (int row=0; row<nu; row++){
HessianMatrix.insert((N+1)*nx + i*nu + row, (N+1)*nx + i*nu + row) = R(row, row);
}
}
/* form constraint matrix */
if (i<N){
// Ad
for (int row=0; row<nx; row++){
for(int col=0; col<nx; col++){
constraintMatrix.insert((i+1)*nx+row, i*nx+col) = Ad(row,col);
}
}
// Bd
for (int row=0; row<nx; row++){
for(int col=0; col<nu; col++){
constraintMatrix.insert((i+1)*nx+row, (N+1)*nx+ i*nu+col) = Bd(row,col);
}
}
lower.segment<nx>((i+1)*nx) = -hd;//-OsqpEigen::INFTY,-OsqpEigen::INFTY,-OsqpEigen::INFTY,-OsqpEigen::INFTY;//-hd;
upper.segment<nx>((i+1)*nx) = -hd; //OsqpEigen::INFTY, OsqpEigen::INFTY,OsqpEigen::INFTY,OsqpEigen::INFTY;//-hd;
}
// -I for each x_k+1
for (int row=0; row<nx; row++) {
constraintMatrix.insert(i*nx+row, i*nx+row) = -1.0;
}
double dx_dtheta = track_->x_eval_d(s_ref);
double dy_dtheta = track_->y_eval_d(s_ref);
constraintMatrix.insert((N+1)*nx+ 2*i, i*nx) = -dy_dtheta; // a*x
constraintMatrix.insert((N+1)*nx+ 2*i, i*nx+1) = dx_dtheta; // b*y
constraintMatrix.insert((N+1)*nx+ 2*i, (N+1)*nx +N*nu +i) = 1.0; // min(C1,C2) <= a*x + b*y + s_k <= inf
constraintMatrix.insert((N+1)*nx+ 2*i+1, i*nx) = -dy_dtheta; // a*x
constraintMatrix.insert((N+1)*nx+ 2*i+1, i*nx+1) = dx_dtheta; // b*y
constraintMatrix.insert((N+1)*nx+ 2*i+1, (N+1)*nx +N*nu +i) = -1.0; // -inf <= a*x + b*y - s_k <= max(C1,C2)
//get upper line and lower line
Vector2d left_tangent_p, right_tangent_p, center_p;
Vector2d right_line_p1, right_line_p2, left_line_p1, left_line_p2;
geometry_msgs::Point r_p1, r_p2, l_p1, l_p2;
center_p << track_->x_eval(s_ref), track_->y_eval(s_ref);
right_tangent_p = center_p + track_->getRightHalfWidth(s_ref) * Vector2d(dy_dtheta, -dx_dtheta).normalized();
left_tangent_p = center_p + track_->getLeftHalfWidth(s_ref) * Vector2d(-dy_dtheta, dx_dtheta).normalized();
right_line_p1 = right_tangent_p + 0.15*Vector2d(dx_dtheta, dy_dtheta).normalized();
right_line_p2 = right_tangent_p - 0.15*Vector2d(dx_dtheta, dy_dtheta).normalized();
left_line_p1 = left_tangent_p + 0.15*Vector2d(dx_dtheta, dy_dtheta).normalized();
left_line_p2 = left_tangent_p - 0.15*Vector2d(dx_dtheta, dy_dtheta).normalized();
// For visualizing track boundaries
r_p1.x = right_line_p1(0); r_p1.y = right_line_p1(1);
r_p2.x = right_line_p2(0); r_p2.y = right_line_p2(1);
l_p1.x = left_line_p1(0); l_p1.y = left_line_p1(1);
l_p2.x = left_line_p2(0); l_p2.y = left_line_p2(1);
border_lines_.push_back(r_p1); border_lines_.push_back(r_p2);
border_lines_.push_back(l_p1); border_lines_.push_back(l_p2);
double C1 = - dy_dtheta*right_tangent_p(0) + dx_dtheta*right_tangent_p(1);
double C2 = - dy_dtheta*left_tangent_p(0) + dx_dtheta*left_tangent_p(1);
lower((N+1)*nx+ 2*i) = min(C1, C2);
upper((N+1)*nx+ 2*i) = OsqpEigen::INFTY;
lower((N+1)*nx+ 2*i+1) = -OsqpEigen::INFTY;
upper((N+1)*nx+ 2*i+1) = max(C1, C2);
// u_min < u < u_max
if (i<N){
for (int row=0; row<nu; row++){
constraintMatrix.insert((N+1)*nx+ 2*(N+1) +i*nu+row, (N+1)*nx+i*nu+row) = 1.0;
}
// input bounds: speed and steer
lower.segment<nu>((N+1)*nx+ 2*(N+1) +i*nu) << -DECELERATION_MAX, -STEER_MAX;
upper.segment<nu>((N+1)*nx+ 2*(N+1) +i*nu) << ACCELERATION_MAX, STEER_MAX;
}
//max velocity
constraintMatrix.insert((N+1)*nx+ 2*(N+1) + N*nu +i, i*nx+3) = 1;
lower((N+1)*nx+ 2*(N+1) + N*nu +i) = 0;
upper((N+1)*nx+ 2*(N+1) + N*nu +i) = SPEED_MAX;
// s_k >= 0
constraintMatrix.insert((N+1)*nx + 2*(N+1) + N*nu + (N+1) + i, (N+1)*nx+N*nu +i) = 1.0;
lower((N+1)*nx + 2*(N+1) + N*nu + (N+1) + i) = 0;
upper((N+1)*nx + 2*(N+1) + N*nu + (N+1) + i) = OsqpEigen::INFTY;
}
int numOfConstraintsSoFar = (N+1)*nx + 2*(N+1) + N*nu + (N+1) + (N+1);
// lamda's >= 0
for (int i=0; i<2*K_NEAR; i++){
constraintMatrix.insert(numOfConstraintsSoFar + i, (N+1)*nx+ N*nu + (N+1) + i) = 1.0;
lower(numOfConstraintsSoFar + i) = 0;
upper(numOfConstraintsSoFar + i) = OsqpEigen::INFTY;
}
numOfConstraintsSoFar += 2*K_NEAR;
// terminal state constraints: -s_t <= -x_N+1 + linear_combination(lambda's) <= s_t
// 0 <= s_t -x_N+1 + linear_combination(lambda's) <= inf
for (int i=0; i<2*K_NEAR; i++){
for (int state_ind=0; state_ind<nx; state_ind++){
constraintMatrix.insert(numOfConstraintsSoFar + state_ind, (N+1)*nx+ N*nu + (N+1) + i) = terminal_CSS[i].x(state_ind);
}
}
for (int state_ind=0; state_ind<nx; state_ind++){
constraintMatrix.insert(numOfConstraintsSoFar + state_ind, N*nx + state_ind) = -1;
constraintMatrix.insert(numOfConstraintsSoFar+state_ind, (N+1)*nx+ N*nu + (N+1) + 2*K_NEAR + state_ind) = 1;
lower(numOfConstraintsSoFar+state_ind) = 0;
upper(numOfConstraintsSoFar+state_ind) = OsqpEigen::INFTY;
}
numOfConstraintsSoFar += nx;
//-inf <= -x_N+1 + linear_combination(lambda's) - s_t <= 0
for (int i=0; i<2*K_NEAR; i++){
for (int state_ind=0; state_ind<nx; state_ind++){
constraintMatrix.insert(numOfConstraintsSoFar + state_ind, (N+1)*nx+ N*nu + (N+1) + i) = terminal_CSS[i].x(state_ind);
}
}
for (int state_ind=0; state_ind<nx; state_ind++){
constraintMatrix.insert(numOfConstraintsSoFar + state_ind, N*nx + state_ind) = -1;
constraintMatrix.insert(numOfConstraintsSoFar+state_ind, (N+1)*nx+ N*nu + (N+1) + 2*K_NEAR + state_ind) = -1;
lower(numOfConstraintsSoFar+state_ind) = -OsqpEigen::INFTY;
upper(numOfConstraintsSoFar+state_ind) = 0;
}
numOfConstraintsSoFar += nx;
// cout<<"con dim: "<< (N+1)*nx+ 2*(N+1)*nx + N*nu + (N-1) + (N+1)*nx + (2*K_NEAR) + nx+1 <<endl;
// sum of lamda's = 1;
for (int i=0; i<2*K_NEAR; i++){
constraintMatrix.insert(numOfConstraintsSoFar, (N+1)*nx+ N*nu + (N+1) + i) = 1;
}
lower(numOfConstraintsSoFar) = 1.0;
upper(numOfConstraintsSoFar) = 1.0;
numOfConstraintsSoFar++;
if (numOfConstraintsSoFar != (N+1)*nx+ 2*(N+1) + N*nu + (N+1) + (N+1) + (2*K_NEAR) + 2*nx+1) throw; // for debug
// gradient
int lowest_cost1 = terminal_CSS[K_NEAR-1].cost;
int lowest_cost2 = terminal_CSS[2*K_NEAR-1].cost;
for (int i=0; i<K_NEAR; i++){
gradient((N+1)*nx+ N*nu + (N+1) + i) = terminal_CSS[i].cost-lowest_cost1;
}
for (int i=K_NEAR; i<2*K_NEAR; i++){
gradient((N+1)*nx+ N*nu + (N+1) + i) = terminal_CSS[i].cost-lowest_cost2;
}
//
for (int i=0; i<nx; i++){
HessianMatrix.insert((N+1)*nx+ N*nu + (N+1) + 2*K_NEAR + i, (N+1)*nx+ N*nu + (N+1) + 2*K_NEAR + i) = q_s_terminal;
}
// for (int i=0; i<2*K_NEAR; i++){
// gradient((N+1)*nx+ N*nu + (N+1) + i) = terminal_CSS[i].cost;
// }
//cout<<"gradient: "<<gradient<<endl;
//x0 constraint
lower.head(nx) = -x0;
upper.head(nx) = -x0;
SparseMatrix<double> H_t = HessianMatrix.transpose();
SparseMatrix<double> sparse_I((N+1)*nx+ N*nu + (N+1)+ (2*K_NEAR) +nx, (N+1)*nx+ N*nu + (N+1)+ (2*K_NEAR) +nx);
sparse_I.setIdentity();
HessianMatrix = 0.5*(HessianMatrix + H_t) + 0.0000001*sparse_I;
OsqpEigen::Solver solver;
solver.settings()->setWarmStart(true);
solver.data()->setNumberOfVariables((N+1)*nx+ N*nu + (N+1)+ 2*K_NEAR +nx);
solver.data()->setNumberOfConstraints((N+1)*nx+ 2*(N+1) + N*nu + (N+1) + (N+1) + 2*K_NEAR + 2*nx+1);
if (!solver.data()->setHessianMatrix(HessianMatrix)) throw "fail set Hessian";
if (!solver.data()->setGradient(gradient)){throw "fail to set gradient";}
if (!solver.data()->setLinearConstraintsMatrix(constraintMatrix)) throw"fail to set constraint matrix";
if (!solver.data()->setLowerBound(lower)){throw "fail to set lower bound";}
if (!solver.data()->setUpperBound(upper)){throw "fail to set upper bound";}
if(!solver.initSolver()){ cout<< "fail to initialize solver"<<endl;}
if(!solver.solve()) {
return;
}
QPSolution_ = solver.getSolution();
visualize_mpc_solution(terminal_CSS, terminal_candidate);
// cout<<"Solution: "<<endl;
// cout<<QPSolution_<<endl;
solver.clearSolver();
if (use_dyn_) ROS_INFO("using dynamics");
else ROS_INFO("using kinematics");
}
void LMPC::applyControl() {
float accel = QPSolution_((N+1)*nx);
float steer = QPSolution_((N+1)*nx+1);
cout<<"accel_cmd: "<<accel<<endl;
cout<<"steer_cmd: "<<steer<<endl;
cout << "slip_angle: "<<slip_angle_<<endl;
steer = min(steer, 0.41f);
steer = max(steer, -0.41f);
ackermann_msgs::AckermannDriveStamped ack_msg;
ack_msg.drive.acceleration = accel;
ack_msg.drive.steering_angle = steer;
ack_msg.drive.steering_angle_velocity = 1.0;
drive_pub_.publish(ack_msg);
}
void LMPC::visualize_mpc_solution(const vector<Sample>& convex_safe_set, const Matrix<double,nx,1>& terminal_candidate) {
visualization_msgs::MarkerArray markers;
visualization_msgs::Marker pred_dots;
pred_dots.header.stamp = ros::Time::now();
pred_dots.header.frame_id = "map";
pred_dots.id = rviz_id::PREDICTION;
pred_dots.ns = "predicted_positions";
pred_dots.type = visualization_msgs::Marker::POINTS;
pred_dots.scale.x = pred_dots.scale.y = pred_dots.scale.z = 0.05;
pred_dots.action = visualization_msgs::Marker::ADD;
pred_dots.pose.orientation.w = 1.0;
pred_dots.color.g = 1.0;
pred_dots.color.a = 1.0;
for (int i=0; i<N+1; i++){
geometry_msgs::Point p;
p.x = QPSolution_(i*nx);
p.y = QPSolution_(i*nx+1);
pred_dots.points.push_back(p);
}
markers.markers.push_back(pred_dots);
visualization_msgs::Marker borderlines;
borderlines.header.stamp = ros::Time::now();
borderlines.header.frame_id = "map";
borderlines.id = rviz_id::BORDERLINES;
borderlines.ns = "borderlines";
borderlines.type = visualization_msgs::Marker::LINE_LIST;
borderlines.scale.x = 0.03;
borderlines.action = visualization_msgs::Marker::ADD;
borderlines.pose.orientation.w = 1.0;
borderlines.color.r = 1.0;
borderlines.color.a = 1.0;
borderlines.points = border_lines_;
markers.markers.push_back(borderlines);
visualization_msgs::Marker css_dots;
css_dots.header.stamp = ros::Time::now();
css_dots.header.frame_id = "map";
css_dots.id = rviz_id::SAFE_SET;
css_dots.ns = "safe_set";
css_dots.type = visualization_msgs::Marker::POINTS;
css_dots.scale.x = css_dots.scale.y = css_dots.scale.z = 0.04;
css_dots.action = visualization_msgs::Marker::ADD;
css_dots.pose.orientation.w = 1.0;
css_dots.color.g = 1.0;
css_dots.color.b = 1.0;
css_dots.color.a = 1.0;
VectorXd costs = VectorXd(convex_safe_set.size());
for (int i=0; i<convex_safe_set.size(); i++){
geometry_msgs::Point p;
p.x = convex_safe_set[i].x(0);
p.y = convex_safe_set[i].x(1);
css_dots.points.push_back(p);
costs(i) = convex_safe_set[i].cost;
}
//cout<<"costs: "<<costs<< endl;
markers.markers.push_back(css_dots);
visualization_msgs::Marker terminal_dot;
terminal_dot.header.stamp = ros::Time::now();
terminal_dot.header.frame_id = "map";
terminal_dot.id = rviz_id::TERMINAL_CANDIDATE;
terminal_dot.ns = "terminal_candidate";
terminal_dot.type = visualization_msgs::Marker::SPHERE;
terminal_dot.scale.x = terminal_dot.scale.y = terminal_dot.scale.z = 0.1;
terminal_dot.action = visualization_msgs::Marker::ADD;
terminal_dot.pose.orientation.w = 1.0;
terminal_dot.pose.position.x = terminal_candidate(0);
terminal_dot.pose.position.y = terminal_candidate(1);
terminal_dot.color.r = 0.5;
terminal_dot.color.b = 0.8;
terminal_dot.color.a = 1.0;
markers.markers.push_back(terminal_dot);
LMPC_viz_pub_.publish(markers);
}
int main(int argc, char **argv){
ros::init(argc, argv, "LMPC");
ros::NodeHandle nh;
LMPC lmpc(nh);
ros::Rate rate(20);
while(ros::ok()){
ros::spinOnce();
lmpc.run();
rate.sleep();
}
return 0;
}
================================================
FILE: src/record_initial_safe_set.cpp
================================================
//
// Created by yuwei on 4/2/20.
//
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <LearningMPC/track.h>
#include <iostream>
#include <fstream>
#include <string>
#include <std_msgs/Float32.h>
using namespace std;
class Record_SS{
public:
Record_SS(ros::NodeHandle& nh);
private:
ros::NodeHandle nh_;
ros::Subscriber cmd_sub_;
ros::Subscriber odom_sub_;
double s_prev_;
int lap_;
int time_;
ofstream data_file_;
double ros_time_prev_;
nav_msgs::Odometry odom_;
double acc_cmd_;
Track* track_;
void cmd_callback(const ackermann_msgs::AckermannDriveStampedConstPtr &cmd_msg);
void odom_callback(const nav_msgs::Odometry::ConstPtr &odom_msg);
void accel_cmd_callback(const std_msgs::Float32ConstPtr & accel_cmd);
};
Record_SS::Record_SS(ros::NodeHandle &nh) : nh_(nh) {
cmd_sub_ = nh_.subscribe("nav", 1, &Record_SS::cmd_callback, this);
odom_sub_ = nh_.subscribe("odom", 1, &Record_SS::odom_callback, this);
string wp_file;
double space;
nh.getParam("waypoint_file", wp_file);
nh.getParam("WAYPOINT_SPACE", space);
boost::shared_ptr<nav_msgs::OccupancyGrid const> map_ptr;
map_ptr = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("map", ros::Duration(5.0));
if (map_ptr == nullptr){ROS_INFO("No map received");}
else{
ROS_INFO("Map received");
nav_msgs::OccupancyGrid map = *map_ptr;
track_ = new Track(wp_file, map, true);
}
nav_msgs::Odometry odom_msg;
boost::shared_ptr<nav_msgs::Odometry const> odom_ptr;
odom_ptr = ros::topic::waitForMessage<nav_msgs::Odometry>("odom", ros::Duration(5));
if (odom_ptr == nullptr){cout<< "fail to receive odom message!"<<endl;}
else{
odom_msg = *odom_ptr;
}
float x = odom_msg.pose.pose.position.x;
float y = odom_msg.pose.pose.position.y;
s_prev_ = track_->findTheta(x,y,0,true);
ros_time_prev_ = ros::Time::now().toSec();
time_=0; lap_=0;
data_file_.open( "initial_safe_set.csv");
}
void Record_SS::odom_callback(const nav_msgs::Odometry::ConstPtr &odom_msg){
odom_ = *odom_msg;
}
void Record_SS::accel_cmd_callback(const std_msgs::Float32ConstPtr & accel_cmd){
acc_cmd_ = accel_cmd->data;
}
void Record_SS::cmd_callback(const ackermann_msgs::AckermannDriveStampedConstPtr &cmd_msg) {
if (cmd_msg != nullptr){
cout<<"recording: "<<time_<<endl;
double speed_cmd = cmd_msg->drive.speed;
double steer_cmd = cmd_msg->drive.steering_angle;
double yaw = tf::getYaw(odom_.pose.pose.orientation);
float x = odom_.pose.pose.position.x;
float y = odom_.pose.pose.position.y;
double s_curr = track_->findTheta(x,y,0,true);
double vel = odom_.twist.twist.linear.x;
// check if is a new lap;
if (s_curr - s_prev_ < -track_->length/2){
// cout<<"s_curr: "<<s_curr<<endl;
// cout<<"s_prev: "<<s_prev_<<endl;
time_ = 0;
lap_++;
if (lap_>1){ //initial two laps completed
data_file_.close();
delete(track_);
ros::shutdown();
}
}
data_file_ << time_ <<","<<x<< ","<<y<<","<<yaw<<","<<vel<<","<<acc_cmd_<<","<<steer_cmd<<","<<s_curr<<endl;
time_++;
s_prev_ = s_curr;
double ros_time_curr = ros::Time::now().toSec();
cout << "dt: "<< ros_time_curr - ros_time_prev_ <<endl;
ros_time_prev_ = ros_time_curr;
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "record_init_ss");
ros::NodeHandle nh;
Record_SS rec_ss(nh);
ros::spin();
return 0;
}
gitextract_5zinre62/
├── CMakeLists.txt
├── Lmpc_params.yaml
├── README.md
├── data/
│ ├── centerline_waypoints.csv
│ └── initial_safe_set.csv
├── include/
│ └── LearningMPC/
│ ├── CSVReader.h
│ ├── car_params.h
│ ├── occupancy_grid.h
│ ├── spline.h
│ └── track.h
├── launch/
│ └── lmpc.launch
├── package.xml
└── src/
├── LMPC.cpp
└── record_initial_safe_set.cpp
SYMBOL INDEX (40 symbols across 7 files)
FILE: include/LearningMPC/CSVReader.h
function class (line 15) | class CSVReader
FILE: include/LearningMPC/car_params.h
type CarParams (line 2) | struct CarParams {
FILE: include/LearningMPC/occupancy_grid.h
function namespace (line 13) | namespace occupancy_grid{
FILE: include/LearningMPC/spline.h
function namespace (line 49) | namespace tk
function resize (line 141) | void band_matrix::resize(int dim, int n_u, int n_l)
function const (line 176) | double band_matrix::operator () (int i, int j) const
function saved_diag (line 186) | double band_matrix::saved_diag(int i) const
function saved_diag (line 191) | double & band_matrix::saved_diag(int i)
function lu_decompose (line 198) | void band_matrix::lu_decompose()
function set_boundary (line 282) | void spline::set_boundary(spline::bd_type left, double left_value,
function set_points (line 295) | void spline::set_points(const std::vector<double>& x,
function const (line 386) | double spline::operator() (double x) const
function eval_d (line 409) | double spline::eval_d(double x){
function eval_dd (line 431) | double spline::eval_dd(double x){
FILE: include/LearningMPC/track.h
type Point_ref (line 36) | typedef struct Point_ref{
function class (line 44) | class Track{
function initialize_width (line 138) | void initialize_width(){
function wrapTheta (line 249) | void wrapTheta(double& theta){
function x_eval (line 254) | double x_eval(double theta){
function y_eval (line 259) | double y_eval(double theta){
function x_eval_d (line 264) | double x_eval_d(double theta){
function y_eval_d (line 269) | double y_eval_d(double theta){
function x_eval_dd (line 274) | double x_eval_dd(double theta){
function y_eval_dd (line 279) | double y_eval_dd(double theta){
function getPhi (line 284) | double getPhi(double theta){
function getLeftHalfWidth (line 292) | double getLeftHalfWidth(double theta){
function getRightHalfWidth (line 299) | double getRightHalfWidth(double theta){
function setHalfWidth (line 306) | void setHalfWidth(double theta, double left_val, double right_val){
function getcenterline_pointsCurvature (line 314) | double getcenterline_pointsCurvature(double theta){
function getcenterline_pointsRadius (line 319) | double getcenterline_pointsRadius(double theta){
FILE: src/LMPC.cpp
type Sample (line 37) | struct Sample{
type rviz_id (line 45) | enum rviz_id{
class LMPC (line 56) | class LMPC{
function compare_s (line 227) | int compare_s(Sample& s1, Sample& s2){
function Vector3d (line 543) | Vector3d LMPC::global_to_track(double x, double y, double yaw, double s){
function Vector3d (line 557) | Vector3d LMPC::track_to_global(double e_y, double e_yaw, double s){
function wrap_angle (line 688) | void wrap_angle(double& angle, const double angle_ref){
function main (line 1035) | int main(int argc, char **argv){
FILE: src/record_initial_safe_set.cpp
class Record_SS (line 15) | class Record_SS{
function main (line 116) | int main(int argc, char **argv){
Condensed preview — 14 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (187K chars).
[
{
"path": "CMakeLists.txt",
"chars": 7646,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(LearningMPC)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_"
},
{
"path": "Lmpc_params.yaml",
"chars": 913,
"preview": "pose_topic : \"odom\"\ndrive_topic: \"nav\"\nwp_file_name: \"/home/yuwei/yuwei_ws/src/LearningMPC/data/centerline_waypoints.csv"
},
{
"path": "README.md",
"chars": 3164,
"preview": "# LearningMPC\nLearning based Model Predictive Control for online iterative trajectory optimization for F1/10 autonomous "
},
{
"path": "data/centerline_waypoints.csv",
"chars": 701,
"preview": "0.688,-0.906\n1.049,-1.92\n1.95,-4.108\n2.453,-6.082\n2.522,-6.570\n2.444,-6.922\n2.181,-7.290\n1.277,-7.851\n-0.698,-8.757\n-1.9"
},
{
"path": "data/initial_safe_set.csv",
"chars": 89635,
"preview": "0,0.697918,-1.06102,-1.09096,0,0,-0.00363945,0.15\n1,0.698549,-1.06224,-1.09098,0.0539778,0,-0.00363945,0.15\n2,0.700624,-"
},
{
"path": "include/LearningMPC/CSVReader.h",
"chars": 860,
"preview": "//\n// Created by yuwei on 11/11/19.\n//\n\n\n#include <iostream>\n#include <fstream>\n#include <vector>\n#include <iterator>\n#i"
},
{
"path": "include/LearningMPC/car_params.h",
"chars": 405,
"preview": "\nstruct CarParams {\n double wheelbase;\n double friction_coeff;\n double h_cg; // height of car's CG\n double l"
},
{
"path": "include/LearningMPC/occupancy_grid.h",
"chars": 2572,
"preview": "//\n// Author: Yuwei Wang\n\n\n#include <nav_msgs/OccupancyGrid.h>\n#include <visualization_msgs/Marker.h>\n#include <math.h>\n"
},
{
"path": "include/LearningMPC/spline.h",
"chars": 16809,
"preview": "//\n// Created by yuwei on 11/19/19.\n//\n\n#ifndef SRC_SPLINE_H\n#define SRC_SPLINE_H\n\n#endif //SRC_SPLINE_H\n/*\n * spline.h\n"
},
{
"path": "include/LearningMPC/track.h",
"chars": 11783,
"preview": "//\n// Created by yuwei on 11/20/19.\n//\n\n#include <ros/ros.h>\n#include <ros/package.h>\n#include <sensor_msgs/LaserScan.h>"
},
{
"path": "launch/lmpc.launch",
"chars": 437,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <!-- Load all parameters -->\n <rosparam command=\"load\" file=\"$(find LearningMPC)/Lmpc_"
},
{
"path": "package.xml",
"chars": 2742,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>LearningMPC</name>\n <version>0.0.0</version>\n <description>The Lear"
},
{
"path": "src/LMPC.cpp",
"chars": 39912,
"preview": "\n#include <ros/ros.h>\n#include <ros/package.h>\n#include <sensor_msgs/LaserScan.h>\n#include <geometry_msgs/PoseStamped.h>"
},
{
"path": "src/record_initial_safe_set.cpp",
"chars": 3728,
"preview": "//\n// Created by yuwei on 4/2/20.\n//\n#include <ros/ros.h>\n#include <nav_msgs/Odometry.h>\n#include <ackermann_msgs/Ackerm"
}
]
About this extraction
This page contains the full source code of the mlab-upenn/LearningMPC GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 14 files (177.1 KB), approximately 80.9k tokens, and a symbol index with 40 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.